public void RotationMatrix_ToAxisAngle_ToRotationMatrix() { RotationMatrix m1, m2; AxisAngle aa1, aa2; double x, y, z, angle; Vector axis; // Test random permutations for (var i = 0; i < 200; i++) { x = Random(-100, 100); y = Random(-100, 100); z = Random(-100, 100); angle = Random(-1440, 1440); // test any possible angle aa1 = new AxisAngle(x, y, z, angle); // a random AA is easier to create than a random RM m1 = aa1.ToRotationMatrix(); aa2 = m1.ToAxisAngle(); m2 = aa2.ToRotationMatrix(); Trace.WriteLine(""); Trace.WriteLine(x + " " + y + " " + z + " " + angle); Trace.WriteLine(aa1); Trace.WriteLine(m1); Trace.WriteLine(aa2); Trace.WriteLine(m2); Assert.IsTrue(m1.IsSimilar(m2)); Assert.IsTrue(aa1.IsEquivalent(aa2)); // just for the sake of it, not the point of this test ;) } // Test singularities for (var i = 0; i < 1000; i++) { axis = Vector.RandomFromInts(-1, 1); angle = 90 * RandomInt(-8, 8); aa1 = new AxisAngle(axis, angle); // a random AA is easier to create than a random RM m1 = aa1.ToRotationMatrix(); aa2 = m1.ToAxisAngle(); m2 = aa2.ToRotationMatrix(); Trace.WriteLine(""); Trace.WriteLine(axis + " " + angle); Trace.WriteLine(aa1); Trace.WriteLine(m1); Trace.WriteLine(aa2); Trace.WriteLine(m2); Assert.IsTrue(m1.IsSimilar(m2)); Assert.IsTrue(aa1.IsEquivalent(aa2)); // just for the sake of it, not the point of this test ;) } }
public void AxisAngle_ToYawPitchRoll_ComparisonThroughRotationMatrix() { AxisAngle aa; RotationMatrix m; YawPitchRoll eu1, eu2; double x, y, z, angle; Vector axis; // Test random permutations for (var i = 0; i < 200; i++) { x = Random(-100, 100); y = Random(-100, 100); z = Random(-100, 100); angle = Random(-1440, 1440); // test any possible angle aa = new AxisAngle(x, y, z, angle); eu1 = aa.ToYawPitchRoll(); m = aa.ToRotationMatrix(); eu2 = m.ToYawPitchRoll(); Trace.WriteLine(""); Trace.WriteLine(x + " " + y + " " + z + " " + angle); Trace.WriteLine(aa + " --> " + eu1); Trace.WriteLine(aa + " --> " + m + " --> " + eu2); Assert.IsTrue(eu1.IsSimilar(eu2)); } // Test singularities for (var i = 0; i < 200; i++) { axis = Vector.RandomFromInts(-1, 1); angle = 90 * RandomInt(-8, 8); aa = new AxisAngle(axis, angle); eu1 = aa.ToYawPitchRoll(); m = aa.ToRotationMatrix(); eu2 = m.ToYawPitchRoll(); Trace.WriteLine(""); Trace.WriteLine(axis + " " + angle); Trace.WriteLine(aa + " --> " + eu1); Trace.WriteLine(aa + " --> " + m + " --> " + eu2); Assert.IsTrue(eu1.IsSimilar(eu2)); } }
public void AxisAngle_ToRotationMatrix_ToAxisAngle() { AxisAngle aa, aabis; RotationMatrix m; double x, y, z, angle; Vector axis; // Test random permutations for (var i = 0; i < 50; i++) { x = Random(-100, 100); y = Random(-100, 100); z = Random(-100, 100); angle = Random(-1440, 1440); // test any possible angle aa = new AxisAngle(x, y, z, angle); m = aa.ToRotationMatrix(); aabis = m.ToAxisAngle(); Trace.WriteLine(""); Trace.WriteLine(x + " " + y + " " + z + " " + angle); Trace.WriteLine(aa); Trace.WriteLine(m); Trace.WriteLine(aabis); Assert.IsTrue(aa.IsEquivalent(aabis)); } // Test singularities for (var i = 0; i < 1000; i++) { axis = Vector.RandomFromInts(-1, 1); angle = 90 * RandomInt(-8, 8); aa = new AxisAngle(axis, angle); m = aa.ToRotationMatrix(); aabis = m.ToAxisAngle(); Trace.WriteLine(""); Trace.WriteLine(axis + " " + angle); Trace.WriteLine(aa); Trace.WriteLine(m); Trace.WriteLine(aabis); Assert.IsTrue(aa.IsEquivalent(aabis)); } }
public void RotationMatrix_ToYawPitchRoll_ToRotationMatrix() { RotationMatrix m1, m2, m3; YawPitchRoll eu1, eu2, eu3; AxisAngle aa; double x, y, z, angle; Vector axis; // Test random permutations for (var i = 0; i < 200; i++) { x = Random(-100, 100); y = Random(-100, 100); z = Random(-100, 100); angle = Random(-1440, 1440); // test any possible angle aa = new AxisAngle(x, y, z, angle); // a random AA is easier to create than a random RM m1 = aa.ToRotationMatrix(); eu1 = m1.ToYawPitchRoll(); m2 = eu1.ToRotationMatrix(); eu2 = m2.ToYawPitchRoll(); m3 = eu2.ToRotationMatrix(); eu3 = m3.ToYawPitchRoll(); Trace.WriteLine(""); Trace.WriteLine(x + " " + y + " " + z + " " + angle); Trace.WriteLine(aa); Trace.WriteLine(m1); Trace.WriteLine(eu1); Trace.WriteLine(m2); Trace.WriteLine(eu2); Trace.WriteLine(m3); Trace.WriteLine(eu3); Assert.IsTrue(m1.IsSimilar(m2)); Assert.IsTrue(m2.IsSimilar(m3)); Assert.IsTrue(eu1.IsEquivalent(eu2)); Assert.IsTrue(eu2.IsSimilar(eu3)); } // Test singularities for (var i = 0; i < 500; i++) { axis = Vector.RandomFromInts(-1, 1); angle = 90 * RandomInt(-16, 16); aa = new AxisAngle(axis, angle); // a random AA is easier to create than a random RM m1 = aa.ToRotationMatrix(); eu1 = m1.ToYawPitchRoll(); m2 = eu1.ToRotationMatrix(); eu2 = m2.ToYawPitchRoll(); m3 = eu2.ToRotationMatrix(); eu3 = m3.ToYawPitchRoll(); Trace.WriteLine(""); Trace.WriteLine(axis + " " + angle); Trace.WriteLine(aa); Trace.WriteLine(m1); Trace.WriteLine(eu1); Trace.WriteLine(m2); Trace.WriteLine(eu2); Trace.WriteLine(m3); Trace.WriteLine(eu3); Assert.IsTrue(m1.IsSimilar(m2)); Assert.IsTrue(m2.IsSimilar(m3)); Assert.IsTrue(eu1.IsEquivalent(eu2)); Assert.IsTrue(eu2.IsSimilar(eu3)); } }