GoaLitiuM/FlaxEngine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'Muzz-IkFix'

Browse Source
This commit is contained in:
Wojtek Figat
2024-04-14 14:41:30 +02:00
parent 5f7293d0a1 af91ce7f3f
commit 80d7ac2581
2 changed files with 113 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Source/Engine/Animations/Graph/AnimGroup.Animation.cpp
View File

@@ -2119,6 +2119,7 @@ void AnimGraphExecutor::ProcessGroupAnimation(Box* boxBase, Node* nodeBase, Valu
value = Variant(Transform::Identity);
break;
}
// Two Bone IK
case 31:
{

158
Source/Engine/Animations/InverseKinematics.cpp
View File

@@ -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;
}