feat(render): levelPlantedFeet — plantigrade foot-flat correction

Author: a-baran-orhan

packages/posecode-render/src/contacts.ts

@@ -1,9 +1,10 @@
 /** Semantic contact-orientation helpers shared by viewer and eval. */
 import * as THREE from "three";
-import type { PinTarget, ReachTarget } from "posecode-parser";
+import { eulerRomFor, type PinTarget, type ReachTarget } from "posecode-parser";
 import type { Mannequin } from "./mannequin.js";
 
 const DOWN = new THREE.Vector3(0, -1, 0);
+const DEG = Math.PI / 180;
 
 /** Rotate contacting wrists so the palm face normal points into the floor. */
 export function alignFloorPalms(
@@ -35,3 +36,70 @@ export function alignFloorPalms(
   }
   if (sides.size > 0) m.root.updateMatrixWorld(true);
 }
+
+const SOLE_LOCAL = new THREE.Vector3(0, -1, 0);
+/** Foot mesh-bottom height at/below which the sole is fully leveled (m). */
+export const PLANT_FADE = 0.06;
+/** Authored plantarflex (ankle local +X) beyond this opts out of leveling (rad). */
+export const PLANTARFLEX_SKIP = 15 * DEG;
+
+const FOOT_SIDES: Array<"left" | "right"> = ["left", "right"];
+const TMP_EULER = new THREE.Euler();
+
+/**
+ * Level each ground-locked foot: rotate the ankle so the sole normal points
+ * world-down (the whole sole rests flat), weighted by how planted the foot is
+ * and skipped when the ankle is authored into plantarflexion (tiptoe intent).
+ * The plantigrade analogue of `alignFloorPalms` for feet: it fixes the
+ * squat/lunge "balancing on the toes" artifact that ground-lock alone leaves,
+ * where a leg-induced foot tilt makes the ball the lowest mesh point.
+ */
+export function levelPlantedFeet(m: Mannequin, activeGroundLock: readonly string[]): void {
+  if (!activeGroundLock.includes("feet")) return;
+  let changed = false;
+  for (const side of FOOT_SIDES) {
+    const ankle = m.bones.get(`ankle_${side}`);
+    if (!ankle?.parent) continue;
+    // Tiptoe opt-out: an ankle authored into plantarflexion (local +X) is a
+    // deliberate relevé / calf-raise / demi-plié — leave it on its toes.
+    TMP_EULER.setFromQuaternion(ankle.quaternion, "XYZ");
+    const authoredX = TMP_EULER.x;
+    const authoredZ = TMP_EULER.z;
+    if (authoredX > PLANTARFLEX_SKIP) continue;
+    // Planted-ness weight from the foot mesh bottom height: fully level when the
+    // sole is on the floor, fading out as a swing foot lifts past PLANT_FADE.
+    const box = new THREE.Box3().setFromObject(ankle);
+    const y = Number.isFinite(box.min.y) ? box.min.y : 0;
+    const weight = THREE.MathUtils.clamp((PLANT_FADE - y) / PLANT_FADE, 0, 1);
+    if (weight <= 1e-3) continue;
+    // Minimal rotation aligning the sole normal to world-down (preserves yaw).
+    const world = ankle.getWorldQuaternion(new THREE.Quaternion());
+    const current = SOLE_LOCAL.clone().applyQuaternion(world).normalize();
+    const correction = new THREE.Quaternion().setFromUnitVectors(current, DOWN);
+    if (weight < 1) correction.slerp(new THREE.Quaternion(), 1 - weight);
+    const desiredWorld = correction.multiply(world);
+    const parentWorld = ankle.parent.getWorldQuaternion(new THREE.Quaternion());
+    const local = parentWorld.invert().multiply(desiredWorld);
+    // Clamp the corrected ankle to its ROM, widened to admit the authored angle
+    // so leveling can never push the joint past a healthy range.
+    const rom = eulerRomFor(`ankle_${side}`);
+    if (rom) {
+      TMP_EULER.setFromQuaternion(local, "XYZ");
+      const cx = THREE.MathUtils.clamp(
+        TMP_EULER.x,
+        Math.min(rom.x.min * DEG, authoredX),
+        Math.max(rom.x.max * DEG, authoredX),
+      );
+      const cz = THREE.MathUtils.clamp(
+        TMP_EULER.z,
+        Math.min(rom.z.min * DEG, authoredZ),
+        Math.max(rom.z.max * DEG, authoredZ),
+      );
+      TMP_EULER.set(cx, TMP_EULER.y, cz, "XYZ");
+      local.setFromEuler(TMP_EULER);
+    }
+    ankle.quaternion.copy(local);
+    changed = true;
+  }
+  if (changed) m.root.updateMatrixWorld(true);
+}

packages/posecode-render/test/contacts.test.ts

@@ -0,0 +1,53 @@
+import { describe, it, expect } from "vitest";
+import * as THREE from "three";
+import { buildMannequin } from "../src/mannequin.js";
+import { levelPlantedFeet } 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<typeof buildMannequin>, 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);
+  });
+});