/// <summary>
/// Converts this Rotation to the axis angle representation.
/// </summary>
/// <param name="axis">Output of normalized axis of rotation.</param>
/// <param name="angleInRadians">Output of angle of rotation about axis (Right Hand Rule).</param>
public void ToAxisAngle(ref __v3t__ axis, ref __ft__ angleInRadians)
{
if (!Fun.ApproximateEquals(NormSquared, 1, 0.001))
{
throw new ArgumentException("Quaternion needs to be normalized to represent a rotation.");
}
angleInRadians = 2 * (__ft__)System.Math.Acos(W);
var s = (__ft__)System.Math.Sqrt(1 - W * W); // assuming quaternion normalised then w is less than 1, so term always positive.
if (s < 0.001)
{ // test to avoid divide by zero, s is always positive due to sqrt
// if s close to zero then direction of axis not important
axis.X = X; // if it is important that axis is normalised then replace with x=1; y=z=0;
axis.Y = Y;
axis.Z = Z;
}
else
{
axis.X = X / s; // normalise axis
axis.Y = Y / s;
axis.Z = Z / s;
}
}
