public void RotationMatrix_ToQuaternion_ToRotationMatrix()
{
RotationMatrix m1, m2;
Quaternion q;
Vector vecX, vecY;
int dir;
double[] r = new double[9];
for (var i = 0; i < 100; i++)
{
vecX = Vector.RandomFromDoubles(-100, 100);
vecY = Vector.RandomFromDoubles(-100, 100);
dir = Vector.CompareDirections(vecX, vecY);
m1 = new RotationMatrix(vecX, vecY);
q = m1.ToQuaternion();
m2 = q.ToRotationMatrix();
Trace.WriteLine("");
Trace.WriteLine(vecX + " " + vecY + " dir:" + dir);
Trace.WriteLine(m1);
Trace.WriteLine(q);
Trace.WriteLine(m2);
Assert.IsTrue(m1.IsSimilar(m2));
}
}
public void RotationMatrix_ToQuaternion_LowTrace()
{
RotationMatrix m = new RotationMatrix(new Vector(0, 1, 0), new Vector(-1, 0, 0));
Quaternion q = m.ToQuaternion();
RotationMatrix m1 = q.ToRotationMatrix();
Assert.IsTrue(m.IsSimilar(m1));
}
public void RotationMatrix_ToQuaternion_VsSystemNumerics()
{
RotationMatrix m;
Quaternion q;
SysMatrix44 m44, m44bis;
SysQuat sq;
Vector vecX, vecY;
int dir;
int runs = 500;
for (var i = 0; i < runs; i++)
{
if (i < 0.5 * runs)
{
vecX = Vector.RandomFromDoubles(-100, 100);
vecY = Vector.RandomFromDoubles(-100, 100);
}
else
{
vecX = Vector.RandomFromInts(-1, 1);
vecY = Vector.RandomFromInts(-1, 1);
}
dir = Vector.CompareDirections(vecX, vecY);
m = new RotationMatrix(vecX, vecY);
q = m.ToQuaternion();
Trace.WriteLine("");
Trace.WriteLine(vecX + " " + vecY + " dir:" + dir);
Trace.WriteLine(m);
Trace.WriteLine(q);
// Use the matrix's orthogonal values to create a M44 matrix with just rotation values:
m44 = new SysMatrix44((float)m.m00, (float)m.m01, (float)m.m02, 0,
(float)m.m10, (float)m.m11, (float)m.m12, 0,
(float)m.m20, (float)m.m21, (float)m.m22, 0,
0, 0, 0, 1);
m44 = SysMatrix44.Transpose(m44); // Numerics.Matrix4x4 uses a transposed convention, meaning the translation vector is horizontal in m41-42-43 instead of vertical in m14-24-34
sq = SysQuat.CreateFromRotationMatrix(m44);
m44bis = SysMatrix44.CreateFromQuaternion(sq);
Trace.WriteLine(m44);
Trace.WriteLine(sq);
Trace.WriteLine(m44bis);
Assert.IsTrue(q.IsEquivalent(new Quaternion(sq.W, sq.X, sq.Y, sq.Z)), "Quaternions are not equivalent!");
}
}