c# - CCD で関節の回転を制限する方法

Question

Unity で IK ソリューションを作成しようとしています。

意図したとおりに動作する単純な CCD アルゴリズムを作成しました: https://www.youtube.com/watch?v=tZvdZGUxbbc

しかし、関節角度に制約を追加しようとすると、動作が不正確で歪んでいます: https://www.youtube.com/watch?v=k1YY9KqqaYY

現在、ターゲットに到達せず、現在の試行が試行しきい値未満である間、チェーン内のすべてのジョイントを反復処理する for ループがあります。

各関節の補正回転が計算されたら、それを関節 [i] に適用します。次に、各ジョイントの回転をチェックし、最小から最大の範囲内に固定します。

回転を適用し、回転制限をチェックするコードは次のとおりです。

Vector3 vectorCurrJointToEndEffector = (joints[joints.Length - 1].transform.position - joints[i].transform.position).normalized; //normalised vector from current joint to end effector position
Vector3 vectorCurrJointToTarget = (target.transform.position - joints[i].transform.position).normalized; //normalised vector from current joint to targetposition

float cosineAngle = Vector3.Dot(vectorCurrJointToEndEffector, vectorCurrJointToTarget); //dot product gives the cosine of the angle for the corrective rotation

//limit possibilities
if (cosineAngle < 1)
{
    Vector3 crossProduct = Vector3.Cross(vectorCurrJointToEndEffector, vectorCurrJointToTarget).normalized; //normalised cross product gives the axis on which to rotate the joint

    float toRotateDegrees = Mathf.Acos(cosineAngle) * Mathf.Rad2Deg; //calculate joint rotation in degrees

    joints[i].transform.rotation = Quaternion.AngleAxis(toRotateDegrees, crossProduct) * joints[i].transform.rotation; //apply joint rotation

    joints[i].transform.rotation = Quaternion.AngleAxis(toRotateDegrees, crossProduct) * joints[i].transform.rotation; //apply joint rotation


    //clamp current joint rotation within set limits
    Vector3 currentJointEulerAngles = joints[i].joint.transform.localRotation.eulerAngles;

    if (currentJointEulerAngles.x > 180f)
        currentJointEulerAngles.x -= 360f;
    currentJointEulerAngles.x = Mathf.Clamp(currentJointEulerAngles.x, joints[i].XaxisMin, joints[i].XaxisMax);

    if (currentJointEulerAngles.y > 180f)
        currentJointEulerAngles.y -= 360f;
    currentJointEulerAngles.y = Mathf.Clamp(currentJointEulerAngles.y, joints[i].YaxisMin, joints[i].YaxisMax);

    if (currentJointEulerAngles.z > 180f)
        currentJointEulerAngles.z -= 360f;
currentJointEulerAngles.z = Mathf.Clamp(currentJointEulerAngles.z, joints[i].ZaxisMin, joints[i].ZaxisMax);

    joints[i].joint.transform.localEulerAngles = currentJointEulerAngles;

2 番目のビデオに示されているように、結果は非常に貧弱です。

誰かが解決策と、おそらくこれが機能しない理由の説明を提案できますか?

貼り付けたコードの更新、修正されたタイプミス。