/// <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; } }