public void AxisAngle_ToYawPitchRoll_ToAxisAngle()
{
AxisAngle aa1, aa2, aa3;
YawPitchRoll eu1, eu2, eu3;
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);
eu1 = aa1.ToYawPitchRoll();
aa2 = eu1.ToAxisAngle();
eu2 = aa2.ToYawPitchRoll();
aa3 = eu2.ToAxisAngle();
eu3 = aa3.ToYawPitchRoll();
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle);
Trace.WriteLine(" " + aa1 + " --> " + eu1);
Trace.WriteLine("--> " + aa2 + " --> " + eu2);
Trace.WriteLine("--> " + aa3 + " --> " + eu3);
Assert.IsTrue(aa1.IsEquivalent(aa2));
Assert.IsTrue(aa2.IsSimilar(aa3));
Assert.IsTrue(eu1.IsSimilar(eu2));
Assert.IsTrue(eu2.IsSimilar(eu3));
}
// Test singularities
for (var i = 0; i < 200; i++)
{
axis = Vector.RandomFromInts(-1, 1);
angle = 90 * RandomInt(-8, 8);
aa1 = new AxisAngle(axis, angle);
eu1 = aa1.ToYawPitchRoll();
aa2 = eu1.ToAxisAngle();
eu2 = aa2.ToYawPitchRoll();
aa3 = eu2.ToAxisAngle();
eu3 = aa3.ToYawPitchRoll();
Trace.WriteLine("");
Trace.WriteLine(axis + " " + angle);
Trace.WriteLine(" " + aa1 + " --> " + eu1);
Trace.WriteLine("--> " + aa2 + " --> " + eu2);
Trace.WriteLine("--> " + aa3 + " --> " + eu3);
Assert.IsTrue(aa1.IsEquivalent(aa2));
Assert.IsTrue(aa2.IsSimilar(aa3));
Assert.IsTrue(eu1.IsSimilar(eu2));
Assert.IsTrue(eu2.IsSimilar(eu3));
}
}
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_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 AxisAngle_Equivalence()
{
AxisAngle a = new AxisAngle(0, 0, 1, 45);
AxisAngle b = new AxisAngle(0, 0, 1, 45 + 360);
Trace.WriteLine(a);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
Assert.IsTrue(b.IsEquivalent(a));
b = new AxisAngle(0, 0, 1, 45 - 360);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
b = new AxisAngle(0, 0, -1, -45);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
b = new AxisAngle(0, 0, -1, -45 - 360);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
b = new AxisAngle(0, 0, -1, -45 + 360);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
b = new AxisAngle(0, 0, 10, 45);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
// Zero vectors
a = new AxisAngle(0, 0, 0, 0);
Trace.WriteLine(a);
for (var i = 0; i < 50; i++)
{
b = new AxisAngle(Random(-10, 10), Random(-10, 10), Random(-10, 10), RandomInt(-3, 3) * 360);
Trace.WriteLine(b);
Assert.IsTrue(a.IsEquivalent(b));
Assert.IsTrue(b.IsEquivalent(a));
}
}
