Skip to content

Instantly share code, notes, and snippets.

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