From 35f33b67461f00bb53e7bf7ef92f224b79ebc972 Mon Sep 17 00:00:00 2001 From: Muzz Date: Fri, 12 Apr 2024 12:07:26 +0800 Subject: [PATCH] Fixed IK to have correct bone roll --- .../Animations/Graph/AnimGroup.Animation.cpp | 1 + .../Engine/Animations/InverseKinematics.cpp | 158 +++++++++++++----- 2 files changed, 113 insertions(+), 46 deletions(-) diff --git a/Source/Engine/Animations/Graph/AnimGroup.Animation.cpp b/Source/Engine/Animations/Graph/AnimGroup.Animation.cpp index b09624d65..c2c7127a5 100644 --- a/Source/Engine/Animations/Graph/AnimGroup.Animation.cpp +++ b/Source/Engine/Animations/Graph/AnimGroup.Animation.cpp @@ -2119,6 +2119,7 @@ void AnimGraphExecutor::ProcessGroupAnimation(Box* boxBase, Node* nodeBase, Valu value = Variant(Transform::Identity); break; } + // Two Bone IK case 31: { diff --git a/Source/Engine/Animations/InverseKinematics.cpp b/Source/Engine/Animations/InverseKinematics.cpp index 098e612c1..06523be81 100644 --- a/Source/Engine/Animations/InverseKinematics.cpp +++ b/Source/Engine/Animations/InverseKinematics.cpp @@ -10,79 +10,102 @@ void InverseKinematics::SolveAimIK(const Transform& node, const Vector3& target, Quaternion::FindBetween(fromNode, toTarget, outNodeCorrection); } -void InverseKinematics::SolveTwoBoneIK(Transform& rootNode, Transform& jointNode, Transform& targetNode, const Vector3& target, const Vector3& jointTarget, bool allowStretching, float maxStretchScale) -{ - Real lowerLimbLength = (targetNode.Translation - jointNode.Translation).Length(); - Real upperLimbLength = (jointNode.Translation - rootNode.Translation).Length(); - Vector3 jointPos = jointNode.Translation; - Vector3 desiredDelta = target - rootNode.Translation; - Real desiredLength = desiredDelta.Length(); - Real limbLengthLimit = lowerLimbLength + upperLimbLength; - Vector3 desiredDir; - if (desiredLength < ZeroTolerance) +void InverseKinematics::SolveTwoBoneIK(Transform& rootTransform, Transform& midJointTransform, Transform& endEffectorTransform, const Vector3& targetPosition, const Vector3& poleVector, bool allowStretching, float maxStretchScale) +{ + // Calculate limb segment lengths + Real lowerLimbLength = (endEffectorTransform.Translation - midJointTransform.Translation).Length(); + Real upperLimbLength = (midJointTransform.Translation - rootTransform.Translation).Length(); + Vector3 midJointPos = midJointTransform.Translation; + + // Calculate the direction and length towards the target + Vector3 toTargetVector = targetPosition - rootTransform.Translation; + Real toTargetLength = toTargetVector.Length(); + Real totalLimbLength = lowerLimbLength + upperLimbLength; + + // Normalize the direction vector or set a default direction if too small + Vector3 toTargetDir; + if (toTargetLength < ZeroTolerance) { - desiredLength = ZeroTolerance; - desiredDir = Vector3(1, 0, 0); + toTargetLength = ZeroTolerance; + toTargetDir = Vector3(1, 0, 0); } else { - desiredDir = desiredDelta.GetNormalized(); + toTargetDir = toTargetVector.GetNormalized(); } - Vector3 jointTargetDelta = jointTarget - rootNode.Translation; - const Real jointTargetLengthSqr = jointTargetDelta.LengthSquared(); + // Calculate the pole vector direction + Vector3 poleVectorDelta = poleVector - rootTransform.Translation; + const Real poleVectorLengthSqr = poleVectorDelta.LengthSquared(); - Vector3 jointPlaneNormal, jointBendDir; - if (jointTargetLengthSqr < ZeroTolerance * ZeroTolerance) + Vector3 jointPlaneNormal, bendDirection; + if (poleVectorLengthSqr < ZeroTolerance * ZeroTolerance) { - jointBendDir = Vector3::Forward; + bendDirection = Vector3::Forward; jointPlaneNormal = Vector3::Up; } else { - jointPlaneNormal = desiredDir ^ jointTargetDelta; + jointPlaneNormal = toTargetDir ^ poleVectorDelta; if (jointPlaneNormal.LengthSquared() < ZeroTolerance * ZeroTolerance) { - desiredDir.FindBestAxisVectors(jointPlaneNormal, jointBendDir); + toTargetDir.FindBestAxisVectors(jointPlaneNormal, bendDirection); } else { jointPlaneNormal.Normalize(); - jointBendDir = jointTargetDelta - (jointTargetDelta | desiredDir) * desiredDir; - jointBendDir.Normalize(); + bendDirection = poleVectorDelta - (poleVectorDelta | toTargetDir) * toTargetDir; + bendDirection.Normalize(); } } + // Handle limb stretching if allowed if (allowStretching) { const Real initialStretchRatio = 1.0f; - const Real range = maxStretchScale - initialStretchRatio; - if (range > ZeroTolerance && limbLengthLimit > ZeroTolerance) + const Real stretchRange = maxStretchScale - initialStretchRatio; + if (stretchRange > ZeroTolerance && totalLimbLength > ZeroTolerance) { - const Real reachRatio = desiredLength / limbLengthLimit; - const Real scalingFactor = (maxStretchScale - 1.0f) * Math::Saturate((reachRatio - initialStretchRatio) / range); + const Real reachRatio = toTargetLength / totalLimbLength; + const Real scalingFactor = (maxStretchScale - 1.0f) * Math::Saturate((reachRatio - initialStretchRatio) / stretchRange); if (scalingFactor > ZeroTolerance) { lowerLimbLength *= 1.0f + scalingFactor; upperLimbLength *= 1.0f + scalingFactor; - limbLengthLimit *= 1.0f + scalingFactor; + totalLimbLength *= 1.0f + scalingFactor; } } } - Vector3 resultEndPos = target; - Vector3 resultJointPos = jointPos; + // Calculate new positions for joint and end effector + Vector3 newEndEffectorPos = targetPosition; + Vector3 newMidJointPos = midJointPos; - if (desiredLength >= limbLengthLimit) - { - resultEndPos = rootNode.Translation + limbLengthLimit * desiredDir; - resultJointPos = rootNode.Translation + upperLimbLength * desiredDir; + if (toTargetLength >= totalLimbLength) { + // Target is beyond the reach of the limb + Vector3 rootToEnd = (targetPosition - rootTransform.Translation).GetNormalized(); + + // Calculate the slight offset towards the pole vector + 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(); + Real slightOffset = upperLimbLength * 0.01f; // Small percentage of the limb length for slight offset + newMidJointPos = rootTransform.Translation + rootToEnd * (upperLimbLength - slightOffset) + midJointDirection * slightOffset; } else { - const Real twoAb = 2.0f * upperLimbLength * desiredLength; - const Real cosAngle = twoAb > ZeroTolerance ? (upperLimbLength * upperLimbLength + desiredLength * desiredLength - lowerLimbLength * lowerLimbLength) / twoAb : 0.0f; + // Target is within reach, calculate joint position + const Real twoAb = 2.0f * upperLimbLength * toTargetLength; + const Real cosAngle = twoAb > ZeroTolerance ? (upperLimbLength * upperLimbLength + toTargetLength * toTargetLength - lowerLimbLength * lowerLimbLength) / twoAb : 0.0f; const bool reverseUpperBone = cosAngle < 0.0f; const Real angle = Math::Acos(cosAngle); const Real jointLineDist = upperLimbLength * Math::Sin(angle); @@ -90,23 +113,66 @@ void InverseKinematics::SolveTwoBoneIK(Transform& rootNode, Transform& jointNode Real projJointDist = projJointDistSqr > 0.0f ? Math::Sqrt(projJointDistSqr) : 0.0f; if (reverseUpperBone) projJointDist *= -1.0f; - resultJointPos = rootNode.Translation + projJointDist * desiredDir + jointLineDist * jointBendDir; + newMidJointPos = rootTransform.Translation + projJointDist * toTargetDir + jointLineDist * bendDirection; } + // Update root joint orientation { - const Vector3 oldDir = (jointPos - rootNode.Translation).GetNormalized(); - const Vector3 newDir = (resultJointPos - rootNode.Translation).GetNormalized(); - const Quaternion deltaRotation = Quaternion::FindBetween(oldDir, newDir); - rootNode.Orientation = deltaRotation * rootNode.Orientation; + // Vector from root joint to mid joint (local Y-axis direction) + Vector3 localY = (newMidJointPos - rootTransform.Translation).GetNormalized(); + + // Vector from mid joint to end effector (used to calculate plane normal) + Vector3 midToEnd = (newEndEffectorPos - newMidJointPos).GetNormalized(); + + // Calculate the plane normal (local Z-axis direction) + Vector3 localZ = Vector3::Cross(localY, midToEnd).GetNormalized(); + + // 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 + Quaternion newRootJointOrientation = Quaternion::LookRotation(localZ, localY); + + // 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 { - const Vector3 oldDir = (targetNode.Translation - jointPos).GetNormalized(); - const Vector3 newDir = (resultEndPos - resultJointPos).GetNormalized(); - const Quaternion deltaRotation = Quaternion::FindBetween(oldDir, newDir); - jointNode.Orientation = deltaRotation * jointNode.Orientation; - jointNode.Translation = resultJointPos; + // 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(); + + // 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 + 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 + Quaternion newMidJointOrientation = Quaternion::LookRotation(localZ, localY); // Assuming FromLookRotation creates a rotation with the first vector as forward and the second as up + + // Apply the new rotation to the mid joint + midJointTransform.Orientation = newMidJointOrientation; + midJointTransform.Translation = newMidJointPos; } - targetNode.Translation = resultEndPos; + // Update end effector transform + endEffectorTransform.Translation = newEndEffectorPos; }