Skip to content

Instantly share code, notes, and snippets.

@shrinktofit
Created August 17, 2022 05:48
Show Gist options
  • Save shrinktofit/6a20779f9028c05359a94d17646f805a to your computer and use it in GitHub Desktop.
Save shrinktofit/6a20779f9028c05359a94d17646f805a to your computer and use it in GitHub Desktop.
Cocos Creator Two Bone IK
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;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment