Revert #2421 two bone ik to solve #2610 regression

Source/Engine/Animations/InverseKinematics.cpp

@@ -80,7 +80,6 @@ void InverseKinematics::SolveTwoBoneIK(Transform& rootTransform, Transform& midJ
     // Calculate new positions for joint and end effector
     Vector3 newEndEffectorPos = targetPosition;
     Vector3 newMidJointPos = midJointPos;
-
     if (toTargetLength >= totalLimbLength)
     {
         // Target is beyond the reach of the limb
@@ -90,13 +89,9 @@ void InverseKinematics::SolveTwoBoneIK(Transform& rootTransform, Transform& midJ
         Vector3 rootToPole = (poleVector - rootTransform.Translation).GetNormalized();
         Vector3 slightBendDirection = Vector3::Cross(rootToEnd, rootToPole);
         if (slightBendDirection.LengthSquared() < ZeroTolerance * ZeroTolerance)
-        {
             slightBendDirection = Vector3::Up;
-        }
         else
-        {
             slightBendDirection.Normalize();
-        }
 
         // Calculate the direction from root to mid joint with a slight offset towards the pole vector
         Vector3 midJointDirection = Vector3::Cross(slightBendDirection, rootToEnd).GetNormalized();
@@ -117,9 +112,16 @@ void InverseKinematics::SolveTwoBoneIK(Transform& rootTransform, Transform& midJ
             projJointDist *= -1.0f;
         newMidJointPos = rootTransform.Translation + projJointDist * toTargetDir + jointLineDist * bendDirection;
     }
-
+    // TODO: fix the new IK impl (https://github.com/FlaxEngine/FlaxEngine/pull/2421) to properly work for character from https://github.com/PrecisionRender/CharacterControllerPro
+#define OLD 1
     // Update root joint orientation
     {
+#if OLD
+        const Vector3 oldDir = (midJointPos - rootTransform.Translation).GetNormalized();
+        const Vector3 newDir = (newMidJointPos - rootTransform.Translation).GetNormalized();
+        const Quaternion deltaRotation = Quaternion::FindBetween(oldDir, newDir);
+        rootTransform.Orientation = deltaRotation * rootTransform.Orientation;
+#else
         // Vector from root joint to mid joint (local Y-axis direction)
         Vector3 localY = (newMidJointPos - rootTransform.Translation).GetNormalized();
 
@@ -136,20 +138,23 @@ void InverseKinematics::SolveTwoBoneIK(Transform& rootTransform, Transform& midJ
         localZ = Vector3::Cross(localX, localY).GetNormalized();
 
         // Construct a rotation from the orthogonal basis vectors
-        Quaternion newRootJointOrientation = Quaternion::LookRotation(localZ, localY);
+        rootTransform.Orientation = Quaternion::LookRotation(localZ, localY);
-
+#endif
-        // Apply the new rotation to the root joint
-        rootTransform.Orientation = newRootJointOrientation;
     }
 
     // Update mid joint orientation to point Y-axis towards the end effector and Z-axis perpendicular to the IK plane
     {
+#if OLD
+        const Vector3 oldDir = (endEffectorTransform.Translation - midJointPos).GetNormalized();
+        const Vector3 newDir = (newEndEffectorPos - newMidJointPos).GetNormalized();
+        const Quaternion deltaRotation = Quaternion::FindBetween(oldDir, newDir);
+        midJointTransform.Orientation = deltaRotation * midJointTransform.Orientation;
+#else
         // Vector from mid joint to end effector (local Y-axis direction after rotation)
         Vector3 midToEnd = (newEndEffectorPos - newMidJointPos).GetNormalized();
 
         // Calculate the plane normal using the root, mid joint, and end effector positions (will be the local Z-axis direction)
         Vector3 rootToMid = (newMidJointPos - rootTransform.Translation).GetNormalized();
-        Vector3 planeNormal = Vector3::Cross(rootToMid, midToEnd).GetNormalized();
 
         // Vector from mid joint to end effector (local Y-axis direction)
         Vector3 localY = (newEndEffectorPos - newMidJointPos).GetNormalized();
@@ -157,21 +162,18 @@ void InverseKinematics::SolveTwoBoneIK(Transform& rootTransform, Transform& midJ
         // Calculate the plane normal using the root, mid joint, and end effector positions (local Z-axis direction)
         Vector3 localZ = Vector3::Cross(rootToMid, localY).GetNormalized();
 
-        //// Calculate the local X-axis direction, should be perpendicular to the Y and Z axes
+        // Calculate the local X-axis direction, should be perpendicular to the Y and Z axes
         Vector3 localX = Vector3::Cross(localY, localZ).GetNormalized();
 
         // Correct the local Z-axis direction based on the cross product of X and Y to ensure orthogonality
         localZ = Vector3::Cross(localX, localY).GetNormalized();
 
         // Construct a rotation from the orthogonal basis vectors
-        // The axes are used differently here than a standard LookRotation to align Z towards the end and Y perpendicular
+        midJointTransform.Orientation = Quaternion::LookRotation(localZ, localY);
-        Quaternion newMidJointOrientation = Quaternion::LookRotation(localZ, localY); // Assuming FromLookRotation creates a rotation with the first vector as forward and the second as up
+#endif
-
-        // Apply the new rotation to the mid joint
-        midJointTransform.Orientation = newMidJointOrientation;
-        midJointTransform.Translation = newMidJointPos;
     }
 
-    // Update end effector transform
+    // Update mid and end locations
+    midJointTransform.Translation = newMidJointPos;
     endEffectorTransform.Translation = newEndEffectorPos;
 }