public void AxisAngle_ToQuaternion_ToAxisAngle_Negative()
{
AxisAngle aa1, aa2;
Quaternion q;
double x, y, z, angle;
// Test random quaternions
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(-360, 0); // choose only negative angles
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle + " length: " + Geometry.Length(x, y, z));
aa1 = new AxisAngle(x, y, z, angle);
q = aa1.ToQuaternion(); // this method will normalize the Quaternion, as neccesary for spatial rotation representation
aa2 = q.ToAxisAngle();
Trace.WriteLine(aa1);
Trace.WriteLine(q);
Trace.WriteLine(aa2);
aa2.Flip(); // from quaternion conversion, aa2 will be inverted. Flip for comparison
Trace.WriteLine(aa2 + " (flipped)");
Assert.IsTrue(aa1.IsSimilar(aa2), "Boooo! :(");
}
}
public void AxisAngle_ToQuaternion_ToAxisAngle_Modulation()
{
AxisAngle aa1, aa2;
Quaternion q;
double x, y, z, angle;
// Test random quaternions
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(-1000, 1000); // test any possible angle
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle + " length: " + Geometry.Length(x, y, z));
aa1 = new AxisAngle(x, y, z, angle);
q = aa1.ToQuaternion(); // this method will normalize the Quaternion, as necessary for spatial rotation representation
aa2 = q.ToAxisAngle();
Trace.WriteLine(aa1);
Trace.WriteLine(q);
Trace.WriteLine(aa2);
// If angle wasn't within 'module', modulate it
if (angle < 0 || angle > 360)
{
aa1.Modulate();
q = aa1.ToQuaternion();
aa2 = q.ToAxisAngle();
Trace.WriteLine("After modulation:");
Trace.WriteLine(aa1);
Trace.WriteLine(q);
Trace.WriteLine(aa2);
}
// Now this should work
Assert.IsTrue(aa1.IsSimilar(aa2));
}
}
public void AxisAngle_ToQuaternion_ToAxisAngle_Simple()
{
AxisAngle aa1, aa2;
Quaternion q;
double x, y, z, angle;
// Test random quaternions
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(360); // choose only positive angles, because quaternion coversion will always return a positive rotation in [0, 360]
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle + " length: " + Geometry.Length(x, y, z));
aa1 = new AxisAngle(x, y, z, angle);
q = aa1.ToQuaternion(); // this method will normalize the Quaternion, as neccesary for spatial rotation representation
aa2 = q.ToAxisAngle();
Trace.WriteLine(aa1);
Trace.WriteLine(q);
Trace.WriteLine(aa2);
Assert.IsTrue(aa1.IsSimilar(aa2), "Boooo! :(");
}
// Test all permutations of unitary components quaternions (including zero)
for (x = -1; x <= 1; x++)
{
for (y = -1; y <= 1; y++)
{
for (z = -1; z <= 1; z++)
{
for (angle = 0; angle <= 360; angle += 45)
{
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle + " length: " + Geometry.Length(x, y, z));
aa1 = new AxisAngle(x, y, z, angle);
q = aa1.ToQuaternion();
aa2 = q.ToAxisAngle();
Trace.WriteLine(aa1);
Trace.WriteLine(q);
Trace.WriteLine(aa2);
// Deal with zero angles
if (aa1.IsZero())
{
Assert.IsTrue(aa1.IsZero());
Assert.IsTrue(q.IsIdentity());
Assert.IsTrue(aa2.IsZero());
}
else
{
Assert.IsTrue(aa1.IsSimilar(aa2), "Booooo! :(");
}
}
}
}
}
}
public void AxisAngle_ToQuaternionConversion()
{
AxisAngle aa;
Quaternion q;
Vector v;
SysVec normV;
SysQuat sq;
double x, y, z, angle;
bool zero;
// Test random quaternions
for (var i = 0; i < 50; i++)
{
x = Random(-100, 100);
y = Random(-100, 100);
z = Random(-100, 100);
angle = Random(-720, 720);
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle + " length: " + Geometry.Length(x, y, z));
aa = new AxisAngle(x, y, z, angle);
q = aa.ToQuaternion(); // this method will normalize the Quaternion, as neccesary for spatial rotation representation
Trace.WriteLine(aa);
Trace.WriteLine(q);
// TEST 1: compare to System.Numeric.Quaternion
normV = new SysVec((float)x, (float)y, (float)z);
normV = SysVec.Normalize(normV);
sq = SysQuat.CreateFromAxisAngle(normV, (float)(angle * Math.PI / 180.0)); // now this Quaternion SHOULD be normalized...
Trace.WriteLine(sq + " length: " + sq.Length());
Assert.AreEqual(q.W, sq.W, 0.000001, "Failed W"); // can't go very precise due to float imprecision in sys quat
Assert.AreEqual(q.X, sq.X, 0.000001, "Failed X");
Assert.AreEqual(q.Y, sq.Y, 0.000001, "Failed Y");
Assert.AreEqual(q.Z, sq.Z, 0.000001, "Failed Z");
}
// Test all permutations of unitary components quaternions (including zero)
for (x = -1; x <= 1; x++)
{
for (y = -1; y <= 1; y++)
{
for (z = -1; z <= 1; z++)
{
for (angle = -720; angle <= 720; angle += 22.5)
{
Trace.WriteLine("");
Trace.WriteLine(x + " " + y + " " + z + " " + angle + " length: " + Geometry.Length(x, y, z));
// Normalize
v = new Vector(x, y, z);
v.Normalize();
aa = new AxisAngle(v, angle);
q = aa.ToQuaternion();
Trace.WriteLine(aa);
Trace.WriteLine(q);
zero = aa.IsZero();
if (zero)
{
Assert.IsTrue(new Quaternion(1, 0, 0, 0).IsSimilar(q), "Failed zero quaternion");
}
else
{
// TEST 1: compare to System.Numeric.Quaternion
//sq = SysQuat.CreateFromAxisAngle(new Vector3((float)x, (float)y, (float)z), (float)(angle * Math.PI / 180.0)); // this Quaternion is not a versor (not unit)
normV = new SysVec((float)x, (float)y, (float)z);
normV = SysVec.Normalize(normV);
sq = SysQuat.CreateFromAxisAngle(normV, (float)(angle * Math.PI / 180.0)); // now this Quaternion SHOULD be normalized...
Trace.WriteLine(sq + " length: " + sq.Length());
Assert.AreEqual(q.W, sq.W, 0.000001, "Failed W"); // can't go very precise due to float imprecision in sys quat
Assert.AreEqual(q.X, sq.X, 0.000001, "Failed X");
Assert.AreEqual(q.Y, sq.Y, 0.000001, "Failed Y");
Assert.AreEqual(q.Z, sq.Z, 0.000001, "Failed Z");
}
}
}
}
}
}
