/// <summary>
/// Extracts out the rotational axis and angles for a swing and twist. Note that this function expects the
/// bone to be oriented along the 'Y' axis.
/// </summary>
/// <param name="rThis"></param>
/// <param name="rTwistAxis"></param>
/// <param name="rTwistAngle"></param>
/// <param name="rSwingAxis"></param>
/// <param name="rSwingAngle"></param>
public static void DecomposeTwistSwingAxisAngles(this Quaternion rThis, Vector3 rTwistAxis, ref float rTwistAngle, ref Vector3 rSwingAxis, ref float rSwingAngle)
{
rTwistAngle = 2.0f * Mathf.Atan2(rThis.y, rThis.w) * Mathf.Rad2Deg;
Vector4 lComponents = new Vector4(0, rThis.y, 0, rThis.w) / Mathf.Sqrt(rThis.y * rThis.y + rThis.w * rThis.w);
Quaternion lTwist = new Quaternion(lComponents.x, lComponents.y, lComponents.z, lComponents.w);
Quaternion lSwing = rThis * lTwist.Conjugate();
float lLength = Mathf.Sqrt(lSwing.x * lSwing.x + lSwing.y * lSwing.y + lSwing.z * lSwing.z);
if (lLength > 0.000001f)
{
float lInvLength = 1.0f / lLength;
rSwingAxis.x = lSwing.x * lInvLength;
rSwingAxis.y = lSwing.y * lInvLength;
rSwingAxis.z = lSwing.z * lInvLength;
if (lSwing.w < 0.0f)
rSwingAngle = 2.0f * Mathf.Atan2(-lLength, -lSwing.w) * Mathf.Rad2Deg; //-PI,0
else
rSwingAngle = 2.0f * Mathf.Atan2(lLength, lSwing.w) * Mathf.Rad2Deg; //0,PI
}
else
{
rSwingAxis = Vector3.right;
rSwingAngle = 0.0f;
}
}
