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));
}
