Cocos Creator Two Bone IK

TwoBoneIK.ts

import { clamp, Node, NodeSpace, Quat, Vec3 } from "cc";

/**
 * 解析双骨骼（三关节）的 IK 问题。
 * 三关节分别称为根关节、中间关节和末端关节。例如，分别对应于大腿、膝盖和脚关节。
 * @param root 根关节。
 * @param middle 中间关节。
 * @param end 末端关节。
 * @param target 末端关节要抵达的目标位置（世界空间）。
 * @param hint 中间关节的提示位置（世界空间），用于决定中间关节的朝向。
 */
export function solveTwoBoneIK(
    root: Node,
    middle: Node,
    end: Node,
    target: Vec3,
    hint?: Vec3,
) {
    hint ??= Vec3.clone(middle.worldPosition);

    const pA = Vec3.clone(root.worldPosition);
    const pB = Vec3.clone(middle.worldPosition);
    const pC = Vec3.clone(end.worldPosition);
    const qC = Quat.clone(end.worldRotation);

    const bSolved = new Vec3();
    const cSolved = new Vec3();
    solveTwoBoneIKPositions(
        pA,
        pB,
        pC,
        target,
        hint,
        bSolved,
        cSolved,
    );

    const qA = Quat.rotationTo(
        new Quat(),
        Vec3.subtract(new Vec3(), pB, pA).normalize(),
        Vec3.subtract(new Vec3(), bSolved, pA).normalize(),
    );
    root.rotate(
        qA,
        NodeSpace.WORLD,
    );
    root.worldPosition = pA;

    const qB = Quat.rotationTo(
        new Quat(),
        Vec3.subtract(new Vec3(), end.worldPosition, middle.worldPosition).normalize(),
        Vec3.subtract(new Vec3(), cSolved, middle.worldPosition).normalize(),
    );
    middle.rotate(
        qB,
        NodeSpace.WORLD,
    );
    middle.worldPosition = bSolved;

    end.worldPosition = cSolved;

    // End factor's rotation frame might be rotated in IK progress, revert it after all thing done.
    // The reverting does not affect the IK result indeed.
    end.worldRotation = qC;
}

function solveTwoBoneIKPositions(
    a: Readonly<Vec3>,
    b: Readonly<Vec3>,
    c: Readonly<Vec3>,
    target: Readonly<Vec3>,
    middleTarget: Readonly<Vec3>,
    bSolved: Vec3,
    cSolved: Vec3,
) {
    const dAB = Vec3.distance(a, b);
    const dBC = Vec3.distance(b, c);
    const dAT = Vec3.distance(a, target);

    const dirAT = Vec3.subtract(new Vec3(), target, a);
    dirAT.normalize();

    const chainLength = dAB + dBC;
    if (dAT >= chainLength) {
        // Target is too far
        Vec3.scaleAndAdd(bSolved, a, dirAT, dAB);
        Vec3.scaleAndAdd(cSolved, a, dirAT, chainLength);
        return;
    }

    // Now we should have a solution with target reached.
    // And then solve the middle joint B as Ḃ.
    Vec3.copy(cSolved, target);
    // Calculate ∠BAC's cosine.
    const cosḂAT = clamp(
        (dAB * dAB + dAT * dAT - dBC * dBC) / (2 * dAB * dAT),
        -1.0,
        1.0,
    );
    // Then use basic trigonometry(instead of rotation) to solve Ḃ.
    // Let D the intersect point of the height line passing Ḃ.
    const dirAB = Vec3.subtract(new Vec3(), middleTarget, a);
    const dirHeightLine = Vec3.projectOnPlane(new Vec3(), dirAB, dirAT);
    dirHeightLine.normalize();
    const dAD = dAB * cosḂAT;
    const hSqr = dAB * dAB - dAD * dAD;
    if (hSqr < 0) {
        'Shall handle this case';
        debugger;
    }
    const h = Math.sqrt(hSqr);
    Vec3.scaleAndAdd(
        bSolved,
        a,
        dirAT,
        dAD,
    );
    Vec3.scaleAndAdd(
        bSolved,
        bSolved,
        dirHeightLine,
        h,
    );
}