任意の回転から振り(Swing)と捻り(Twist)を取り出す

元々は任意の回転から特定の軸に沿った捻りだけを取り出したかったのですがコレといった文献が見つからず、色々寄せ集めて作ったコードです。

DecomposeSwingTwist.cs

//------------------------------------------------------------
/// <summary>
/// QuaternionからTwistとSwingを取り出す
/// </summary>
/// <param name="q">与える回転(Local)</param>
/// <param name="twistAxis">捻りを取り出すための基準軸(Local)</param>
/// <param name="swing">分解したSwing回転(Local)</param>
/// <param name="twist">分解したTwist回転(Local)</param>
/// <seealso cref="http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/" />
/// <seealso cref="http://allenchou.net/2018/05/game-math-swing-twist-interpolation-sterp/" />
//------------------------------------------------------------
public static void DecomposeSwingTwist(Quaternion q, Vector3 
twistAxis, out Quaternion swing, out Quaternion twist) {
    // (rotation axis) * cos(angle / 2)
    Vector3 r = new Vector3(q.x, q.y, q.z);

    // singularity: rotation by 180 degree
    if (r.sqrMagnitude < Mathf.Epsilon) {
        Vector3 rotatedTwistAxis = q * twistAxis;
        Vector3 swingAxis = Vector3.Cross(twistAxis, rotatedTwistAxis);

        if (swingAxis.sqrMagnitude > Mathf.Epsilon) {
            float swingAngle = Vector3.Angle(twistAxis, 
rotatedTwistAxis);
            swing = Quaternion.AngleAxis(swingAngle, swingAxis);
        } else {
            // more singularity: rotation axis parallel to twist axis
            swing = Quaternion.identity;    // no swing
        }

        // always twist 180 degree on singularity
        twist = Quaternion.AngleAxis(180.0f, twistAxis);
    } else {
        // formula & proof:
        // http://www.euclideanspace.com/maths/geometry/rotations/for/decomposition/
        Vector3 p = Vector3.Project(r, twistAxis);
        twist = new Quaternion(p.x, p.y, p.z, q.w);
        twist = Quaternion.Normalize(twist);
        swing = q * Quaternion.Inverse(twist);
    }
}