Cocos Creator Two Bone IK

TwoBoneIK.ts

export function solveTwoBoneIK(
    a: Node,
    b: Node,
    c: Node,
    target: Vec3,
) {
    const sanityChecker = new TwoBoneIKNodeSanityChecker(a, b, c);

    const pA = Vec3.clone(a.worldPosition);
    const pB = Vec3.clone(b.worldPosition);
    const pC = Vec3.clone(c.worldPosition);
    const qC = Quat.clone(c.worldRotation);

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

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

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

    c.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.
    c.worldRotation = qC;

    sanityChecker.check();
}

function solveTwoBoneIKPositions(
    a: Readonly<Vec3>,
    b: Readonly<Vec3>,
    c: Readonly<Vec3>,
    target: Readonly<Vec3>,
    bSolved: Vec3,
    cSolved: Vec3,
) {
    const sanityChecker = new TwoBoneIKPositionSanityChecker(a, b, c);
    const sanityCheck = () => sanityChecker.check(a, bSolved, cSolved);

    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);
        sanityCheck();
        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 dirHeightLine = Vec3.multiplyScalar(
        new Vec3(),
        dirAT,
        Vec3.dot(dirAT, Vec3.subtract(new Vec3(), b, a)),
    );
    Vec3.subtract(
        dirHeightLine,
        Vec3.subtract(new Vec3(), b, a),
        dirHeightLine,
    );
    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,
    );
    if (DEBUG) {
        sanityCheck();
    }
}

class TwoBoneIKNodeSanityChecker {
    constructor(private _a: Node, private _b: Node, private _c: Node) {
        const pA = _a.worldPosition;
        const pB = _b.worldPosition;
        const pC = _c.worldPosition;
        this._pA = Vec3.clone(pA);
        this._dAB = Vec3.distance(pA, pB);
        this._dBC = Vec3.distance(pB, pC);
        this._rC = Quat.clone(_c.worldRotation);
    }

    public check() {
        const { _a, _b, _c } = this;
        const pA = _a.worldPosition;
        const pB = _b.worldPosition;
        const pC = _c.worldPosition;
        const CHECK_EPSILON = 1e-3;
        const dAB = Vec3.distance(pA, pB);
        const dBC = Vec3.distance(pB, pC);
        // Root's world position shall not change
        if (!Vec3.equals(pA, this._pA, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        // Joint length shall not change
        if (!approx(dAB, this._dAB, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        if (!approx(dBC, this._dBC, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        // End factor's world rotation shall not change
        if (!Quat.equals(_c.worldRotation, this._rC, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        return true;
    }

    private _pA: Vec3;
    private _dAB: number;
    private _dBC: number;
    private _rC: Quat;
}

class TwoBoneIKPositionSanityChecker {
    constructor(private _a: Readonly<Vec3>, _b: Readonly<Vec3>, _c: Readonly<Vec3>) {
        this._dAB = Vec3.distance(_a, _b);
        this._dBC = Vec3.distance(_b, _c);
    }

    public check(_a: Readonly<Vec3>, _b: Readonly<Vec3>, _c: Readonly<Vec3>) {
        const CHECK_EPSILON = 1e-3;
        const dAB = Vec3.distance(_a, _b);
        const dBC = Vec3.distance(_b, _c);
        if (!approx(Vec3.distance(_a, this._a), 0.0, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        if (!approx(dAB, this._dAB, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        if (!approx(dBC, this._dBC, CHECK_EPSILON)) {
            debugger;
            return false;
        }
        return true;
    }

    private declare _dAB: number;
    private declare _dBC: number;
}