/// <summary>
/// Converts the quaternion rotation into axis/angle rotation.
/// </summary>
/// <param name="axis">Axis around which the rotation is performed.</param>
/// <param name="angle">Amount of rotation.</param>
public void ToAxisAngle(out Vector3 axis, out Degree angle)
{
float fSqrLength = x * x + y * y + z * z;
if (fSqrLength > 0.0f)
{
angle = 2.0f * MathEx.Acos(w);
float fInvLength = MathEx.InvSqrt(fSqrLength);
axis.x = x * fInvLength;
axis.y = y * fInvLength;
axis.z = z * fInvLength;
}
else
{
// Angle is 0, so any axis will do
angle = (Degree)0.0f;
axis.x = 1.0f;
axis.y = 0.0f;
axis.z = 0.0f;
}
}
/// <summary>
/// Decomposes the matrix into a set of values.
/// </summary>
/// <param name="matQ">Columns form orthonormal bases. If your matrix is affine and doesn't use non-uniform scaling
/// this matrix will be the rotation part of the matrix.
/// </param>
/// <param name="vecD">If the matrix is affine these will be scaling factors of the matrix.</param>
/// <param name="vecU">If the matrix is affine these will be shear factors of the matrix.</param>
public void QDUDecomposition(out Matrix3 matQ, out Vector3 vecD, out Vector3 vecU)
{
matQ = new Matrix3();
vecD = new Vector3();
vecU = new Vector3();
// Build orthogonal matrix Q
float invLength = MathEx.InvSqrt(m00 * m00 + m10 * m10 + m20 * m20);
matQ.m00 = m00 * invLength;
matQ.m10 = m10 * invLength;
matQ.m20 = m20 * invLength;
float dot = matQ.m00 * m01 + matQ.m10 * m11 + matQ.m20 * m21;
matQ.m01 = m01 - dot * matQ.m00;
matQ.m11 = m11 - dot * matQ.m10;
matQ.m21 = m21 - dot * matQ.m20;
invLength = MathEx.InvSqrt(matQ.m01 * matQ.m01 + matQ.m11 * matQ.m11 + matQ.m21 * matQ.m21);
matQ.m01 *= invLength;
matQ.m11 *= invLength;
matQ.m21 *= invLength;
dot = matQ.m00 * m02 + matQ.m10 * m12 + matQ.m20 * m22;
matQ.m02 = m02 - dot * matQ.m00;
matQ.m12 = m12 - dot * matQ.m10;
matQ.m22 = m22 - dot * matQ.m20;
dot = matQ.m01 * m02 + matQ.m11 * m12 + matQ.m21 * m22;
matQ.m02 -= dot * matQ.m01;
matQ.m12 -= dot * matQ.m11;
matQ.m22 -= dot * matQ.m21;
invLength = MathEx.InvSqrt(matQ.m02 * matQ.m02 + matQ.m12 * matQ.m12 + matQ.m22 * matQ.m22);
matQ.m02 *= invLength;
matQ.m12 *= invLength;
matQ.m22 *= invLength;
// Guarantee that orthogonal matrix has determinant 1 (no reflections)
float fDet = matQ.m00 * matQ.m11 * matQ.m22 + matQ.m01 * matQ.m12 * matQ.m20 +
matQ.m02 * matQ.m10 * matQ.m21 - matQ.m02 * matQ.m11 * matQ.m20 -
matQ.m01 * matQ.m10 * matQ.m22 - matQ.m00 * matQ.m12 * matQ.m21;
if (fDet < 0.0f)
{
matQ.m00 = -matQ.m00;
matQ.m01 = -matQ.m01;
matQ.m02 = -matQ.m02;
matQ.m10 = -matQ.m10;
matQ.m11 = -matQ.m11;
matQ.m12 = -matQ.m12;
matQ.m20 = -matQ.m20;
matQ.m21 = -matQ.m21;
matQ.m21 = -matQ.m22;
}
// Build "right" matrix R
Matrix3 matRight = new Matrix3();
matRight.m00 = matQ.m00 * m00 + matQ.m10 * m10 + matQ.m20 * m20;
matRight.m01 = matQ.m00 * m01 + matQ.m10 * m11 + matQ.m20 * m21;
matRight.m11 = matQ.m01 * m01 + matQ.m11 * m11 + matQ.m21 * m21;
matRight.m02 = matQ.m00 * m02 + matQ.m10 * m12 + matQ.m20 * m22;
matRight.m12 = matQ.m01 * m02 + matQ.m11 * m12 + matQ.m21 * m22;
matRight.m22 = matQ.m02 * m02 + matQ.m12 * m12 + matQ.m22 * m22;
// The scaling component
vecD[0] = matRight.m00;
vecD[1] = matRight.m11;
vecD[2] = matRight.m22;
// The shear component
float invD0 = 1.0f / vecD[0];
vecU[0] = matRight.m01 * invD0;
vecU[1] = matRight.m02 * invD0;
vecU[2] = matRight.m12 / vecD[1];
}