/// <summary>
/// Converts a rotation matrix to a quaternion.
/// <see href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/"/>
/// </summary>
/// <param name="rotationMatrix">Rotation matrix to be converted. </param>
/// <returns>Quaternion representation of the rotation matrix.</returns>
public static Quaternion FromRotationMatrix(RotationMatrix rotationMatrix)
{
float w, x, y, z;
float tr = rotationMatrix.XX + rotationMatrix.YY + rotationMatrix.ZZ;
if (tr > 0)
{
float S = (float)Math.Sqrt(tr + 1.0) * 2; // S=4*w
w = 0.25f * S;
x = (rotationMatrix.ZY - rotationMatrix.YZ) / S;
y = (rotationMatrix.XZ - rotationMatrix.ZX) / S;
z = (rotationMatrix.YX - rotationMatrix.XY) / S;
}
else if ((rotationMatrix.XX > rotationMatrix.YY) & (rotationMatrix.XX > rotationMatrix.ZZ))
{
float S = (float)Math.Sqrt(1.0 + rotationMatrix.XX - rotationMatrix.YY - rotationMatrix.ZZ) * 2; // S=4*x
w = (rotationMatrix.ZY - rotationMatrix.YZ) / S;
x = 0.25f * S;
y = (rotationMatrix.XY + rotationMatrix.YX) / S;
z = (rotationMatrix.XZ + rotationMatrix.ZX) / S;
}
else if (rotationMatrix.YY > rotationMatrix.ZZ)
{
float S = (float)Math.Sqrt(1.0 + rotationMatrix.YY - rotationMatrix.XX - rotationMatrix.ZZ) * 2; // S=4*y
w = (rotationMatrix.XZ - rotationMatrix.ZX) / S;
x = (rotationMatrix.XY + rotationMatrix.YX) / S;
y = 0.25f * S;
z = (rotationMatrix.YZ + rotationMatrix.ZY) / S;
}
else
{
float S = (float)Math.Sqrt(1.0 + rotationMatrix.ZZ - rotationMatrix.XX - rotationMatrix.YY) * 2; // S=4*z
w = (rotationMatrix.YX - rotationMatrix.XY) / S;
x = (rotationMatrix.XZ + rotationMatrix.ZX) / S;
y = (rotationMatrix.YZ + rotationMatrix.ZY) / S;
z = 0.25f * S;
}
return(new Quaternion(w, x, y, z));
}
/// <summary>
/// Converts a quaternion to a rotation matrix.
/// </summary>
/// <param name="quaternion">Quaternion to be converted.</param>
/// <returns>Rotation matrix representation of the quaternion. </returns>
public static RotationMatrix ToRotationMatrix(Quaternion quaternion)
{
float qwqw = quaternion.W * quaternion.W; // calculate common terms to avoid repetition
float qwqx = quaternion.W * quaternion.X;
float qwqy = quaternion.W * quaternion.Y;
float qwqz = quaternion.W * quaternion.Z;
float qxqy = quaternion.X * quaternion.Y;
float qxqz = quaternion.X * quaternion.Z;
float qyqz = quaternion.Y * quaternion.Z;
RotationMatrix matrix = new RotationMatrix();
matrix.XX = 2.0f * (qwqw - 0.5f + quaternion.X * quaternion.X);
matrix.XY = 2.0f * (qxqy + qwqz);
matrix.XZ = 2.0f * (qxqz - qwqy);
matrix.YX = 2.0f * (qxqy - qwqz);
matrix.YY = 2.0f * (qwqw - 0.5f + quaternion.Y * quaternion.Y);
matrix.YZ = 2.0f * (qyqz + qwqx);
matrix.ZX = 2.0f * (qxqz + qwqy);
matrix.ZY = 2.0f * (qyqz - qwqx);
matrix.ZZ = 2.0f * (qwqw - 0.5f + quaternion.Z * quaternion.Z);
return(matrix);
}