Method SolveTwoBoneIK

Source/Engine/Animations/InverseKinematics.cpp:13–179

}

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)
    {
        toTargetLength = ZeroTolerance;
        toTargetDir = Vector3(1, 0, 0);
    }
    else
    {
        toTargetDir = toTargetVector.GetNormalized();
    }

    // Calculate the pole vector direction
    Vector3 poleVectorDelta = poleVector - rootTransform.Translation;
    const Real poleVectorLengthSqr = poleVectorDelta.LengthSquared();

    Vector3 jointPlaneNormal, bendDirection;
    if (poleVectorLengthSqr < ZeroTolerance * ZeroTolerance)
    {
        bendDirection = Vector3::Forward;
        jointPlaneNormal = Vector3::Up;
    }
    else
    {
        jointPlaneNormal = toTargetDir ^ poleVectorDelta;
        if (jointPlaneNormal.LengthSquared() < ZeroTolerance * ZeroTolerance)
        {
            toTargetDir.FindBestAxisVectors(jointPlaneNormal, bendDirection);
        }
        else
        {
            jointPlaneNormal.Normalize();
            bendDirection = poleVectorDelta - (poleVectorDelta | toTargetDir) * toTargetDir;
            bendDirection.Normalize();
        }
    }

    // Handle limb stretching if allowed
    if (allowStretching)
    {
        const Real initialStretchRatio = 1.0f;
        const Real stretchRange = maxStretchScale - initialStretchRatio;
        if (stretchRange > ZeroTolerance && totalLimbLength > ZeroTolerance)
        {
            const Real reachRatio = toTargetLength / totalLimbLength;
            const Real scalingFactor = (maxStretchScale - 1.0f) * Math::Saturate((reachRatio - initialStretchRatio) / stretchRange);