/// <summary>
/// Converts an orthonormal matrix to axis angle representation.
/// </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 trace = m00 + m11 + m22;
float cos = 0.5f * (trace - 1.0f);
Radian radians = MathEx.Acos(cos); // In [0, PI]
angle = radians.Degrees;
if (radians > 0.0f)
{
if (radians < MathEx.Pi)
{
axis.x = m21 - m12;
axis.y = m02 - m20;
axis.z = m10 - m01;
axis.Normalize();
}
else
{
// Angle is PI
float halfInverse;
if (m00 >= m11)
{
// r00 >= r11
if (m00 >= m22)
{
// r00 is maximum diagonal term
axis.x = 0.5f * MathEx.Sqrt(m00 - m11 - m22 + 1.0f);
halfInverse = 0.5f / axis.x;
axis.y = halfInverse * m01;
axis.z = halfInverse * m02;
}
else
{
// r22 is maximum diagonal term
axis.z = 0.5f * MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
halfInverse = 0.5f / axis.z;
axis.x = halfInverse * m02;
axis.y = halfInverse * m12;
}
}
else
{
// r11 > r00
if (m11 >= m22)
{
// r11 is maximum diagonal term
axis.y = 0.5f * MathEx.Sqrt(m11 - m00 - m22 + 1.0f);
halfInverse = 0.5f / axis.y;
axis.x = halfInverse * m01;
axis.z = halfInverse * m12;
}
else
{
// r22 is maximum diagonal term
axis.z = 0.5f * MathEx.Sqrt(m22 - m00 - m11 + 1.0f);
halfInverse = 0.5f / axis.z;
axis.x = halfInverse * m02;
axis.y = halfInverse * m12;
}
}
}
}
else
{
// The angle is 0 and the matrix is the identity. Any axis will
// work, so just use the x-axis.
axis.x = 1.0f;
axis.y = 0.0f;
axis.z = 0.0f;
}
}
/// <summary>
/// Calculates the magnitude of the provided vector.
/// </summary>
/// <param name="v">Vector to calculate the magnitude for.</param>
/// <returns>Magnitude of the vector.</returns>
public static float Magnitude(Vector2 v)
{
return(MathEx.Sqrt(v.x * v.x + v.y * v.y));
}
/// <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];
}
/// <summary>
/// Calculates the distance between two points.
/// </summary>
/// <param name="a">First two dimensional point.</param>
/// <param name="b">Second two dimensional point.</param>
/// <returns>Distance between the two points.</returns>
public static float Distance(Vector2 a, Vector2 b)
{
Vector2 vector2 = new Vector2(a.x - b.x, a.y - b.y);
return(MathEx.Sqrt(vector2.x * vector2.x + vector2.y * vector2.y));
}
