public void LeftIntersection_WhenSweeplinePassesThroughLowerOfTheTwoFocii_ShouldReturnDirectionOfThatFocus (Arc arc, Sweepline sweepline) { arc.LeftNeighbour.Position = Utilities.VectorAt(114, 94); arc.Site.Position = Utilities.VectorAt(96, 77); sweepline = Utilities.SweeplineAt(121); // Fixture setup var focus = arc.Site.Position; var leftFocus = arc.LeftNeighbour.Position; var lowerFocus = focus.Z < leftFocus.Z ? focus : leftFocus; var directionOfLowerFocus = AngleUtilities.EquatorialDirection(lowerFocus); sweepline.Z = lowerFocus.Z; // Exercise system var directionOfLeftIntersection = arc.LeftIntersection(sweepline); // Verify outcome var failureString = String.Format("Direction of left intersection: {0},\n", directionOfLeftIntersection); Assert.True(Vector.AlmostEqual(directionOfLowerFocus, directionOfLeftIntersection, Tolerance), failureString); // Teardown }
public static Vector3 LeftIntersection(this IArc arc, Sweepline sweepline) { var p = arc.LeftNeighbour.Position; var q = arc.Site.Position; var Z = sweepline.Z; if (Vector.AlmostEqual(p, q)) { return(AngleUtilities.EquatorialDirection(p)); } if (Number.AlmostEqual(Z, p.Z) && Number.AlmostEqual(Z, q.Z)) { return(AngleUtilities.EquatorialMidpoint(p, q)); } var A = p.X * (Z - q.Z) - q.X * (Z - p.Z); var B = p.Y * (Z - q.Z) - q.Y * (Z - p.Z); var C = (p.Z - q.Z) * Math.Sign(sweepline.Priority) * Math.Sqrt(1 - Z * Z); var A2PlusB2MinusC2 = Math.Max(A * A + B * B - C * C, 0); var x = (A * C + B * Math.Sqrt(A2PlusB2MinusC2)) / (A * A + B * B); var y = (B * C - A * Math.Sqrt(A2PlusB2MinusC2)) / (A * A + B * B); return(new Vector3(x, y, 0)); }
public static bool UnorderedEquals(IEnumerable <Vector> expected, IEnumerable <Vector> actual, double relativeAccuracy) { var expectedList = expected.ToList(); var actualList = actual.ToList(); var missingFromActual = expectedList.Where(x => expectedList.Count(y => Vector.AlmostEqual(x, y, relativeAccuracy)) != actualList.Count(y => Vector.AlmostEqual(x, y, relativeAccuracy))); var missingFromExpected = actualList.Where(x => expectedList.Count(y => Vector.AlmostEqual(x, y, relativeAccuracy)) != actualList.Count(y => Vector.AlmostEqual(x, y, relativeAccuracy))); return(!(missingFromActual.Any() || missingFromExpected.Any())); }
public static bool AreInOrder(Vector3 a, Vector3 b, Vector3 c) { if (Vector.AlmostEqual(b, c) || Vector.AlmostEqual(a, c)) { return(false); } else { return((c - b).CrossMultiply(a - b)[2] >= 0); } }
public void TestQuatproduct() { Vector <double> q_1 = Vector <double> .Build.DenseOfArray(new[] { 0.63867877, 0.52251797, 0.56156573, 0.06089615 }); Vector <double> q_2 = Vector <double> .Build.DenseOfArray(new[] { 0.35764716, 0.61051424, 0.11540801, 0.69716703 }); Matrix <double> R_t = Q2R(q_1).Multiply(Q2R(q_2)); Vector <double> q_t = R2Q(R_t); Vector <double> q = Quatproduct(q_1).Multiply(q_2); Assert.IsTrue(q.AlmostEqual(q_t, 1e-6)); }
public void TestR2Q() { Matrix <double> rot = Matrix <double> .Build.DenseOfRowArrays( new[] { -0.5057639, -0.1340537, 0.8521928 }, new[] { 0.6456962, -0.7139224, 0.2709081 }, new[] { 0.5720833, 0.6872731, 0.4476342 }); Vector <double> q_t = Vector <double> .Build.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 }); Vector <double> q = R2Q(rot); Assert.IsTrue(q.AlmostEqual(q_t, 1e-6)); }
public void LeftIntersection_WhenLeftNeigbourIsSameAsArcSite_ShouldReturnDirectionOfFocus (Arc arc, Sweepline sweepline) { // Fixture setup arc.LeftNeighbour = arc.Site; var directionOfFocus = AngleUtilities.EquatorialDirection(arc.Site.Position); // Exercise system var directionOfLeftIntersection = arc.LeftIntersection(sweepline); // Verify outcome var failureString = String.Format("Direction of left intersection: {0},\n", directionOfLeftIntersection); Assert.True(Vector.AlmostEqual(directionOfFocus, directionOfLeftIntersection, Tolerance), failureString); // Teardown }
public void TestAlmostEquals_Vector() { Vector a1 = Vector.Random(3, new ContinuousUniformDistribution()); Vector a2 = a1.Clone(); Vector b = -a1; Vector c = a1 * (1.0 + (1e+10 * Number.PositiveEpsilonOf(1.0))); Vector d = a1 * (1.0 + (2 * Number.PositiveEpsilonOf(1.0))); Helper_TestAlmostEqualityForGenericType(a1, a2, b, c, d); // Wrapper Assert.That(Vector.AlmostEqual(a1, c), Is.False); Assert.That(Vector.AlmostEqual(a1, c, 1e-10), Is.False); Assert.That(Vector.AlmostEqual(a1, c, 1e-2), Is.True); // reference type -> no boxing Assert.That(a1, Is.SameAs(a1)); }
public void TestRot2Q() { Tuple <Vector <double>, double> rot = R2rot(Matrix <double> .Build.DenseOfRowArrays( new[] { -0.5057639, -0.1340537, 0.8521928 }, new[] { 0.6456962, -0.7139224, 0.2709081 }, new[] { 0.5720833, 0.6872731, 0.4476342 })); Vector <double> k = Vector <double> .Build.Dense(3); for (int i = 0; i < rot.Item1.Count; i++) { k[i] = (double)rot.Item1[i]; } double theta = (double)rot.Item2; Vector <double> q_t = Vector <double> .Build.DenseOfArray(new[] { 0.2387194, 0.4360402, 0.2933459, 0.8165967 }); Vector <double> q = Rot2Q(k, theta); Assert.IsTrue(q.AlmostEqual(q_t, 1e-6)); }
public void PointAt_IfSweeplinePassesThroughFocusAndVectorIsNotAtSameAzimuthAsFocus_ShouldReturnNorthPole (Arc arc, Vector3 vector) { // Fixture setup var sweepline = new Sweepline { Z = arc.Site.Position.Z }; var northPole = new Vector3(0, 0, 1); // Exercise system var point = arc.PointAt(vector, sweepline); // Verify outcome var failureString = String.Format("Point was {0}", point); Assert.True(Vector.AlmostEqual(northPole, point, Tolerance), failureString); // Teardown }
public void Curl_OverACube_ShouldSumToZero (IPolyhedron polyhedron, VectorField <Vertex> V) { // Fixture setup var operators = new VectorFieldOperators(polyhedron); // Exercise system var divergence = operators.Curl(V); // Verify outcome var expected = Vector.Zeros(3); var actual = divergence.Values.Aggregate(Vector.Zeros(3), (c, v) => c + v); TestUtilities.WriteExpectedAndActual(expected, actual); Assert.True(Vector.AlmostEqual(expected, actual, TestUtilities.RelativeAccuracy)); // Teardown }
// singularRPI requested like this: //[ExpectedException(typeof(ArgumentException), //"A userId of null was inappropriately allowed.")] public void Testrpy2R() { Vector <double> rpy1 = Vector <double> .Build.DenseOfArray(new[] { 10 * Math.PI / 180, -30 * Math.PI / 180, 90 * Math.PI / 180 }); Matrix <double> R1 = rpy2R(rpy1); Matrix <double> R1_t = Matrix <double> .Build.DenseOfRowArrays( new[] { -0.0000000, -0.9848077, 0.1736482 }, new[] { 0.8660254, -0.0868241, -0.4924039 }, new[] { 0.5000000, 0.1503837, 0.8528686 }); Assert.IsTrue(R1.AlmostEqual(R1_t, 1e-6)); Vector <double> rpy2 = R2rpy(R1); Assert.IsTrue(rpy1.AlmostEqual(rpy2, 1e-6)); // Check singularity Vector <double> rpy3 = Vector <double> .Build.DenseOfArray(new[] { 10 * Math.PI / 180, 90 * Math.PI / 180, -30 * Math.PI / 180 }); Matrix <double> R3 = rpy2R(rpy3); }
public void LocalDirection_AnyVectorFromTheNorthPole_ShouldBeTheSameAsThatVectorProjectedOntoEquator (double colatitude, double azimuth) { // Fixture setup var vector = VectorUtilities.NewVector(colatitude, azimuth); var northPole = VectorUtilities.NewVector(0, 0); var expected = vector; expected[2] = 0; expected = expected.Normalize(); // Exercise system var actual = VectorUtilities.LocalDirection(northPole, vector); // Verify outcome TestUtilities.WriteExpectedAndActual(expected, actual); Assert.True(Vector.AlmostEqual(expected, actual, TestUtilities.RelativeAccuracy)); // Teardown }
public void EdgeNormals_OnTheAllPositiveVertexOfACube_ShouldCalculateTheCorrectNormals (IPolyhedron polyhedron) { // Fixture setup var expected = new List <Vector> { VectorUtilities.NewVector(0, -1, 1).Normalize(), VectorUtilities.NewVector(1, 0, -1).Normalize(), VectorUtilities.NewVector(-1, 1, 0).Normalize(), }; // Exercise system var normals = VertexIndexedTableFactory.EdgeNormals(polyhedron); // Verify outcome var vertex = polyhedron.Vertices.First(v => Vector.AlmostEqual(v.Position, VectorUtilities.NewVector(1, 1, 1))); var actual = normals[polyhedron.IndexOf(vertex)]; TestUtilities.WriteExpectedAndActual(expected, actual); Assert.True(TestUtilities.UnorderedEquals(expected, actual, TestUtilities.RelativeAccuracy)); // Teardown }
public static Vector3 PointAt(Vector3 focus, Vector3 vector, Sweepline sweepline) { var p = focus; var n = AngleUtilities.EquatorialDirection(vector); var Z = sweepline.Z; if (Number.AlmostEqual(p.Z, Z) && Vector.AlmostEqual(AngleUtilities.EquatorialDirection(p), n)) { return(p); } if (Number.AlmostEqual(p.Z, Z)) { return(new Vector3(0, 0, 1)); } var tanOfColatitude = (Z - p.Z) / (p.ScalarMultiply(n) - Math.Sign(sweepline.Priority) * Math.Sqrt(1 - Z * Z)); var x = n.X * Math.Abs(tanOfColatitude / Math.Sqrt(1 + tanOfColatitude * tanOfColatitude)); var y = n.Y * Math.Abs(tanOfColatitude / Math.Sqrt(1 + tanOfColatitude * tanOfColatitude)); var z = Math.Sign(tanOfColatitude) / Math.Sqrt(1 + tanOfColatitude * tanOfColatitude); return(new Vector3(x, y, z)); }
public bool _test_last_configuration(Robot r, double[] theta, double[] last_theta) { Transform pose1 = Fwdkin(r, theta); double[][] theta2 = InverseKin.robot6_sphericalwrist_invkin(r, pose1); Transform pose2 = Fwdkin(r, theta2[0]); if (!(pose1 == pose2)) { return(false); } Vector <double> theta2_vec = Vector <double> .Build.DenseOfArray(theta2[0]); Vector <double> last_vec = Vector <double> .Build.DenseOfArray(last_theta); Console.WriteLine("last config: {0}", theta2_vec); Console.WriteLine("last config: {0}", last_theta); if (theta2_vec.AlmostEqual(last_vec, 1e-8)) { return(true); } return(false); }
public void LeftIntersection_WhenSweeplinePassesThroughBothFocuses_ShouldReturnEquatorialMidpointOfFocii (Arc arc, Sweepline sweepline) { // Fixture setup var colatitudeOfFocus = arc.Site.Position.SphericalCoordinates().Colatitude; var azimuthOfFocus = arc.Site.Position.SphericalCoordinates().Azimuth; var azimuthOfLeftFocus = arc.LeftNeighbour.Position.SphericalCoordinates().Azimuth; arc.Site.Position = new SphericalCoords(colatitudeOfFocus, azimuthOfFocus).CartesianCoordinates(); arc.LeftNeighbour.Position = new SphericalCoords(colatitudeOfFocus, azimuthOfLeftFocus).CartesianCoordinates(); // Exercise system var directionOfLeftIntersection = arc.LeftIntersection(sweepline); // Verify outcome var equatorialMidpoint = AngleUtilities.EquatorialMidpoint(arc.LeftNeighbour.Position, arc.Site.Position); var failureString = String.Format("Direction of left intersection: {0},\n", directionOfLeftIntersection); Assert.True(Vector.AlmostEqual(equatorialMidpoint, directionOfLeftIntersection, Tolerance), failureString); // Teardown }
public void Test_Subproblems() { Vector <double> x = Vector <double> .Build.DenseOfArray(new[] { 1.0, 0, 0 }); Vector <double> y = Vector <double> .Build.DenseOfArray(new[] { 0, 1.0, 0 }); Vector <double> z = Vector <double> .Build.DenseOfArray(new[] { 0, 0, 1.0 }); // Subproblem0 Assert.IsTrue(Subproblem0(x, y, z) == Math.PI / 2); // Subproblem1 Vector <double> k1 = (x + z) / (x + z).L2Norm(); Vector <double> k2 = (y + z) / (y + z).L2Norm(); Assert.IsTrue(Subproblem1(k1, k2, z) == Math.PI / 2); // Subproblem2 Vector <double> p2 = x; Vector <double> q2 = x.Add(y).Add(z); q2 = q2 / q2.L2Norm(); double[] a2 = Subproblem2(p2, q2, z, y); Assert.IsTrue(a2.Length == 4); //NOTE: DIFFERENT THAN PYTHON VERSION Matrix <double> r1_0 = Rot(z, a2[0]); Matrix <double> r1_1 = Rot(y, a2[1]); Vector <double> r1 = (r1_0 * r1_1).Column(0); Matrix <double> r2_0 = Rot(z, a2[2]); Matrix <double> r2_1 = Rot(y, a2[3]); Vector <double> r2 = (r2_0 * r2_1).Column(0); Assert.IsTrue(r1.AlmostEqual(q2, 1e-4)); Assert.IsTrue(r2.AlmostEqual(q2, 1e-4)); double[] a3 = Subproblem2(x, z, z, y); //Console.WriteLine(a3); Assert.IsTrue(a3.Length == 2); //NOTE: DIFFERENT THAN PYTHON VERSION Matrix <double> r3_0 = Rot(z, a3[0]); Matrix <double> r3_1 = Rot(y, a3[1]); Vector <double> r3 = (r3_0 * r3_1).Column(0); Assert.IsTrue(r3.AlmostEqual(z, 1e-4)); // Subproblem3 Vector <double> p4 = Vector <double> .Build.DenseOfArray(new[] { .5, 0, 0 }); Vector <double> q4 = Vector <double> .Build.DenseOfArray(new[] { 0, .75, 0 }); double[] a4 = Subproblem3(p4, q4, z, .5); double[] a5 = Subproblem3(p4, q4, z, 1.25); Assert.IsTrue(a4.Length == 2); Assert.IsTrue((q4 + Rot(z, a4[0]) * p4).L2Norm().AlmostEqual(0.5, 1e-8)); Assert.IsTrue((q4 + Rot(z, a4[1]) * p4).L2Norm().AlmostEqual(0.5, 1e-8)); Assert.IsTrue(a5.Length == 1); Assert.IsTrue((q4 + Rot(z, a5[0]) * p4).L2Norm().AlmostEqual(1.25, 1e-8)); // Subproblem4 Vector <double> p6 = y; Vector <double> q6 = Vector <double> .Build.DenseOfArray(new[] { .8, .2, .5 }); double d6 = .3; double[] a6 = Subproblem4(p6, q6, z, d6); Assert.IsTrue((p6 * Rot(z, a6[0]) * q6).AlmostEqual(d6, 1e-4)); Assert.IsTrue((p6 * Rot(z, a6[1]) * q6).AlmostEqual(d6, 1e-4)); }