/// <summary>
/// Calculates an angle between two rotations.
/// </summary>
/// <param name="a">First rotation.</param>
/// <param name="b">Second rotation.</param>
/// <returns>Angle between the rotations, in degrees.</returns>
public static Degree Angle(Quaternion a, Quaternion b)
{
return(MathEx.Acos(MathEx.Min(MathEx.Abs(Dot(a, b)), 1.0f)) * 2.0f);
}
/// <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 = (Radian)MathEx.Acos(cos); // In [0, PI]
angle = radians;
if (radians > (Radian)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;
}
}
