import { describe, it, expect } from "vitest"; import * as THREE from "three"; import { buildMannequin } from "../src/mannequin.js"; import { FIST_LOCAL_NORMAL, PALM_LOCAL_NORMAL, alignFloorContacts, alignGripFrames, enforceContactRom, floorTargetForEffector, formFists, levelPlantedFeet, measureFootContact, prepareGripFrames, relaxHands, swingArms, aimHead, } from "../src/contacts.js"; import { groundFigure } from "../src/groundlock.js"; const DEG = Math.PI / 180; /** World-space sole normal (ankle local -Y) for a foot. */ function soleNormal(m: ReturnType, side: "left" | "right") { const ankle = m.bones.get(`ankle_${side}`)!; const q = ankle.getWorldQuaternion(new THREE.Quaternion()); return new THREE.Vector3(0, -1, 0).applyQuaternion(q).normalize(); } describe("levelPlantedFeet", () => { it("levels a tilted planted foot so the sole faces down", () => { const m = buildMannequin(); // Tilt the leg so the foot pitches within the ankle's dorsiflexion range // (deeper tilts legitimately lift the heel — capped by ROM), then plant it // on the floor as the frame loop's ground-lock does before leveling. m.bones.get("knee_left")!.rotation.x = 12 * DEG; m.root.updateMatrixWorld(true); groundFigure(m); const tiltedDot = soleNormal(m, "left").dot(new THREE.Vector3(0, -1, 0)); levelPlantedFeet(m, ["feet"]); m.root.updateMatrixWorld(true); const n = soleNormal(m, "left"); // Leveling makes the sole face world-down (dot ~ 1), flatter than the tilt. expect(n.dot(new THREE.Vector3(0, -1, 0))).toBeGreaterThan(0.98); expect(n.dot(new THREE.Vector3(0, -1, 0))).toBeGreaterThan(tiltedDot); }); it("leaves an authored-plantarflex foot on its toes", () => { const m = buildMannequin(); m.bones.get("ankle_left")!.rotation.x = 30 * DEG; // plantarflex (toe-down) m.root.updateMatrixWorld(true); const before = m.bones.get("ankle_left")!.quaternion.clone(); levelPlantedFeet(m, ["feet"]); expect(m.bones.get("ankle_left")!.quaternion.angleTo(before)).toBeLessThan(1e-6); }); it("does not touch a swing foot lifted off the floor", () => { const m = buildMannequin(); // Lift the foot well above the floor by bending the knee back and raising hip. m.bones.get("hip_left")!.rotation.x = -60 * DEG; m.root.position.y = 0.5; m.root.updateMatrixWorld(true); const before = m.bones.get("ankle_left")!.quaternion.clone(); levelPlantedFeet(m, ["feet"]); expect(m.bones.get("ankle_left")!.quaternion.angleTo(before)).toBeLessThan(1e-3); }); it("levels only the selected per-side foot", () => { const m = buildMannequin(); m.bones.get("knee_left")!.rotation.x = 12 * DEG; m.bones.get("knee_right")!.rotation.x = 12 * DEG; m.root.updateMatrixWorld(true); groundFigure(m); const leftBefore = m.bones.get("ankle_left")!.quaternion.clone(); const rightBefore = m.bones.get("ankle_right")!.quaternion.clone(); levelPlantedFeet(m, ["foot_left"]); expect(m.bones.get("ankle_left")!.quaternion.angleTo(leftBefore)).toBeGreaterThan(1e-3); expect(m.bones.get("ankle_right")!.quaternion.angleTo(rightBefore)).toBeLessThan(1e-6); }); it("reports visible heel/toe height and sole angle, not ankle-bone height", () => { const m = buildMannequin(); groundFigure(m); const flat = measureFootContact(m, "left")!; expect(flat.heelHeight).toBeCloseTo(0, 5); expect(flat.toeHeight).toBeCloseTo(0, 5); expect(flat.soleAngleDeg).toBeCloseTo(0, 5); m.bones.get("knee_left")!.rotation.x = 25 * DEG; m.root.updateMatrixWorld(true); groundFigure(m); levelPlantedFeet(m, ["foot_left"]); groundFigure(m); const limited = measureFootContact(m, "left")!; expect(Math.min(limited.heelHeight, limited.toeHeight)).toBeLessThan(0.02); expect(limited.heelHeight).toBeGreaterThan(0.02); expect(limited.soleAngleDeg).toBeGreaterThan(5); }); }); describe("contact geometry", () => { const down = new THREE.Vector3(0, -1, 0); it("uses the flattened palm mesh's +Z face on both sides and stays in wrist ROM", () => { const m = buildMannequin(); for (const side of ["left", "right"] as const) { const palm = m.bones.get(`wrist_${side}`)!.getObjectByName(`palm_${side}`) as THREE.Mesh; palm.geometry.computeBoundingBox(); const size = palm.geometry.boundingBox!.getSize(new THREE.Vector3()).multiply(palm.scale); // Ground the axis convention in the actual mesh: Z is its shallow face. expect(size.z).toBeLessThan(size.x); expect(size.z).toBeLessThan(size.y); // Twenty degrees of safe shoulder extension lets the wrist's 70deg // extension ceiling reach a fully palm-down 90deg world frame. m.bones.get(`shoulder_${side}`)!.rotation.x = 20 * DEG; } m.root.updateMatrixWorld(true); alignFloorContacts(m, [], [{ effector: "hands", anchor: "floor" }], []); for (const side of ["left", "right"] as const) { const wrist = m.bones.get(`wrist_${side}`)!; const normal = new THREE.Vector3(...PALM_LOCAL_NORMAL) .applyQuaternion(wrist.getWorldQuaternion(new THREE.Quaternion())) .normalize(); expect(normal.dot(down)).toBeGreaterThan(0.995); const local = new THREE.Euler().setFromQuaternion(wrist.quaternion, "XYZ"); expect(local.x).toBeLessThanOrEqual(70 * DEG + 1e-6); expect(Math.abs(local.y)).toBeLessThan(1e-6); expect(Math.abs(local.z)).toBeLessThan(1e-6); } }); it("plants authored-pronated palms by twisting forearms without moving wrist targets", () => { const m = buildMannequin(); const wristPositions = new Map(); for (const side of ["left", "right"] as const) { // Cobra-like arms: flexed/externally-rotated shoulders, bent elbows, and // full semantic pronation (mirrored in the two local rig frames). m.bones.get(`shoulder_${side}`)!.rotation.set( -100 * DEG, (side === "left" ? 60 : -60) * DEG, 0, ); m.bones.get(`elbow_${side}`)!.rotation.set( -40 * DEG, (side === "left" ? -80 : 80) * DEG, 0, ); } m.root.updateMatrixWorld(true); for (const side of ["left", "right"] as const) { wristPositions.set( side, m.bones.get(`wrist_${side}`)!.getWorldPosition(new THREE.Vector3()), ); } alignFloorContacts(m, [], [{ effector: "hands", anchor: "floor" }], []); for (const side of ["left", "right"] as const) { const elbow = m.bones.get(`elbow_${side}`)!; const wrist = m.bones.get(`wrist_${side}`)!; const normal = new THREE.Vector3(...PALM_LOCAL_NORMAL) .applyQuaternion(wrist.getWorldQuaternion(new THREE.Quaternion())) .normalize(); const local = new THREE.Euler().setFromQuaternion(elbow.quaternion, "XYZ"); // The solver uses the matching anatomical supination frame to enter a // strong palm-down cone while preserving the solved wrist endpoint. expect(normal.dot(down)).toBeGreaterThan(Math.cos(45.5 * DEG)); expect(wrist.getWorldPosition(new THREE.Vector3()).distanceTo(wristPositions.get(side)!)) .toBeLessThan(1e-7); expect(local.x).toBeGreaterThanOrEqual(-154 * DEG - 1e-6); expect(local.x).toBeLessThanOrEqual(10 * DEG + 1e-6); expect(local.y).toBeGreaterThanOrEqual((side === "left" ? -84 : -92) * DEG - 1e-6); expect(local.y).toBeLessThanOrEqual((side === "left" ? 92 : 84) * DEG + 1e-6); expect(Math.abs(local.z)).toBeLessThan(1e-6); } }); it("plants a fist on its knuckles, distinctly from a palm, without losing curl", () => { const m = buildMannequin(); m.bones.get("wrist_left")!.rotation.x = -30 * DEG; formFists(m, new Set(["left"])); const curlBefore = m.bones.get("index_left")!.quaternion.clone(); const elbowBefore = m.bones.get("elbow_left")!.quaternion.clone(); alignFloorContacts(m, [{ effector: "fist_left", target: "floor", weight: 1 }], [], []); const wristWorld = m.bones.get("wrist_left")!.getWorldQuaternion(new THREE.Quaternion()); const knuckles = new THREE.Vector3(...FIST_LOCAL_NORMAL).applyQuaternion(wristWorld).normalize(); const palm = new THREE.Vector3(...PALM_LOCAL_NORMAL).applyQuaternion(wristWorld).normalize(); expect(knuckles.dot(down)).toBeGreaterThan(0.995); expect(Math.abs(palm.dot(down))).toBeLessThan(0.1); expect(m.bones.get("index_left")!.quaternion.angleTo(curlBefore)).toBeLessThan(1e-8); expect(m.bones.get("elbow_left")!.quaternion.angleTo(elbowBefore)).toBeLessThan(1e-8); expect(m.bones.get("index_left")!.rotation.x).toBeLessThan(-1); }); it("targets the knee surface, not the bottom of its shin/foot subtree", () => { const m = buildMannequin(); m.root.updateMatrixWorld(true); const wristBefore = m.bones.get("wrist_left")!.quaternion.clone(); const target = floorTargetForEffector(m, "knee_left"); expect(target).not.toBeNull(); expect(m.contactSurfaces.knee_left).toHaveLength(2); expect(target!.y).toBeGreaterThan(0.05); expect(target!.y).toBeLessThan(0.07); alignFloorContacts(m, [{ effector: "knee_left", target: "floor", weight: 1 }], [], []); expect(m.bones.get("wrist_left")!.quaternion.angleTo(wristBefore)).toBeLessThan(1e-8); }); it("targets the local pelvis surface instead of its whole body subtree", () => { const m = buildMannequin(); m.root.updateMatrixWorld(true); const target = floorTargetForEffector(m, "pelvis"); expect(target).not.toBeNull(); expect(m.contactSurfaces.pelvis.length).toBeGreaterThan(0); expect(target!.y).toBeGreaterThan(0.08); expect(target!.y).toBeLessThan(0.16); }); it("strictly clears forbidden ankle Y/Z and clamps terminal joints", () => { const m = buildMannequin(); m.bones.get("ankle_left")!.rotation.set(0.2, 0.5, -0.4); m.bones.get("wrist_right")!.rotation.set(0.2, 0.3, -0.8); enforceContactRom(m); const ankle = new THREE.Euler().setFromQuaternion(m.bones.get("ankle_left")!.quaternion, "XYZ"); const wrist = new THREE.Euler().setFromQuaternion(m.bones.get("wrist_right")!.quaternion, "XYZ"); expect(Math.abs(ankle.y)).toBeLessThan(1e-7); expect(Math.abs(ankle.z)).toBeLessThan(1e-7); expect(ankle.x).toBeLessThanOrEqual(50 * DEG + 1e-6); expect(wrist.x).toBeLessThanOrEqual(70 * DEG + 1e-6); expect(Math.abs(wrist.y)).toBeLessThan(1e-7); expect(wrist.z).toBeGreaterThanOrEqual(-20 * DEG - 1e-6); expect(wrist.z).toBeLessThanOrEqual(30 * DEG + 1e-6); }); it("builds a stable palm-down dip-bar frame", () => { const m = buildMannequin(); m.bones.get("shoulder_left")!.rotation.x = 20 * DEG; m.bones.get("elbow_left")!.rotation.y = 80 * DEG; const grips = [{ effector: "hand_left", anchor: "bars_left" }]; prepareGripFrames(m, grips); expect(Math.abs(m.bones.get("elbow_left")!.rotation.y)).toBeLessThan(1e-7); alignGripFrames(m, grips); const normal = new THREE.Vector3(...PALM_LOCAL_NORMAL).applyQuaternion( m.bones.get("wrist_left")!.getWorldQuaternion(new THREE.Quaternion()), ); expect(normal.dot(down)).toBeGreaterThan(0.995); }); }); describe("relaxHands (L4.1)", () => { it("curls the fingers of an idle, un-authored hand into a natural rest", () => { const m = buildMannequin(); expect(m.bones.get("index_left")!.rotation.x).toBeCloseTo(0, 5); // flat at rest relaxHands(m, new Set(), new Set()); expect(m.bones.get("index_left")!.rotation.x).toBeLessThan(-0.1); expect(m.bones.get("middle_right")!.rotation.x).toBeLessThan(-0.1); }); it("leaves a gripping hand for wrapGrip (skips grip sides)", () => { const m = buildMannequin(); relaxHands(m, new Set(["left"]), new Set()); expect(m.bones.get("index_left")!.rotation.x).toBeCloseTo(0, 5); // untouched expect(m.bones.get("index_right")!.rotation.x).toBeLessThan(-0.1); // right relaxed }); it("does not override an explicitly authored finger", () => { const m = buildMannequin(); m.bones.get("index_left")!.rotation.x = 1.4; // authored fist relaxHands(m, new Set(), new Set(["index_left"])); expect(m.bones.get("index_left")!.rotation.x).toBeCloseTo(1.4, 5); }); it("keeps a floor-planted hand's fingers flat instead of clawing", () => { const m = buildMannequin(); // A free hand takes the soft inward hook... relaxHands(m, new Set(), new Set(), new Set()); const freeCurl = m.bones.get("index_left")!.rotation.x; expect(freeCurl).toBeLessThan(-0.3); expect(freeCurl).toBeGreaterThan(-0.4); // relaxed, not a near-fist // ...but a hand pressed to the floor (plank/push-up) lies extended. relaxHands(m, new Set(), new Set(), new Set(["left"])); expect(m.bones.get("index_left")!.rotation.x).toBeLessThan(0.1); // flat expect(m.bones.get("index_right")!.rotation.x).toBeLessThan(-0.3); // right still hooked }); }); describe("swingArms (L4.2)", () => { it("adds contralateral arm swing when a hip is flexed and the arm is free", () => { const m = buildMannequin(); // Flex the right hip forward (walking: right leg forward → left arm forward). m.bones.get("hip_right")!.rotation.x = -0.6; const before = m.bones.get("shoulder_left")!.rotation.x; swingArms(m, new Set(), new Set()); const after = m.bones.get("shoulder_left")!.rotation.x; expect(Math.abs(after - before)).toBeGreaterThan(0.05); // swung // swings the same sagittal direction as the contralateral hip (forward) expect(Math.sign(after - before)).toBe(Math.sign(-0.6)); }); it("respects an authored shoulder (no swing)", () => { const m = buildMannequin(); m.bones.get("hip_right")!.rotation.x = -0.6; m.bones.get("shoulder_left")!.rotation.x = 0.9; // authored swingArms(m, new Set(["shoulder_left"]), new Set()); expect(m.bones.get("shoulder_left")!.rotation.x).toBeCloseTo(0.9, 5); }); it("skips a gripping side", () => { const m = buildMannequin(); m.bones.get("hip_right")!.rotation.x = -0.6; const before = m.bones.get("shoulder_left")!.rotation.x; swingArms(m, new Set(), new Set(["left"])); expect(m.bones.get("shoulder_left")!.rotation.x).toBeCloseTo(before, 5); }); it("is idempotent across render frames instead of accumulating a spin", () => { const m = buildMannequin(); m.bones.get("hip_right")!.rotation.x = -0.6; swingArms(m, new Set(), new Set()); const once = m.bones.get("shoulder_left")!.quaternion.clone(); for (let frame = 0; frame < 120; frame++) swingArms(m, new Set(), new Set()); expect(m.bones.get("shoulder_left")!.quaternion.angleTo(once)).toBeLessThan(1e-6); }); }); describe("aimHead (L4.3 look-at)", () => { it("turns the head toward a focus point (face +Z tracks the target)", () => { const m = buildMannequin(); m.root.updateMatrixWorld(true); const head = m.bones.get("head")!; const headPos = head.getWorldPosition(new THREE.Vector3()); const focus = headPos.clone().add(new THREE.Vector3(0, 1.2, 0.6)); const faceDir = () => new THREE.Vector3(0, 0, 1) .applyQuaternion(head.getWorldQuaternion(new THREE.Quaternion())) .normalize(); const want = focus.clone().sub(headPos).normalize(); const before = faceDir().dot(want); aimHead(m, focus); m.root.updateMatrixWorld(true); expect(faceDir().dot(want)).toBeGreaterThan(before); }); it("clamps the look so the head never spins past its range", () => { const m = buildMannequin(); m.root.updateMatrixWorld(true); const head = m.bones.get("head")!; const headPos = head.getWorldPosition(new THREE.Vector3()); const behind = headPos.clone().add(new THREE.Vector3(0, 0, -2)); aimHead(m, behind); const e = new THREE.Euler().setFromQuaternion(m.bones.get("head")!.quaternion, "XYZ"); expect(Math.hypot(e.x, e.y, e.z)).toBeLessThan(1.2); }); });