Merge branch 'IkFix' of https://github.com/Muzz/FlaxEngine into Muzz-IkFix
This commit is contained in:
Wojtek Figat
@@ -2119,6 +2119,7 @@ void AnimGraphExecutor::ProcessGroupAnimation(Box* boxBase, Node* nodeBase, Valu
|
||||
value = Variant(Transform::Identity);
|
||||
break;
|
||||
}
|
||||
|
||||
// Two Bone IK
|
||||
case 31:
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user