-
Notifications
You must be signed in to change notification settings - Fork 9
Expand file tree
/
Copy pathgroundlock.ts
More file actions
257 lines (240 loc) · 9.96 KB
/
Copy pathgroundlock.ts
File metadata and controls
257 lines (240 loc) · 9.96 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
/**
* Floating-root ground-contact solving, independent of the viewer so both the
* render loop and the headless eval harness (posecode-eval) use the identical
* solver.
*
* "Ground-lock" = keep the declared grouped, per-side, or axial contacts
* planted while the body moves, tuned per support type:
*
* - **Hands + feet (push-up / plank):** pivot the whole rigid body about the
* foot line (the toes stay planted) until the hands reach the floor. As the
* elbows fold (FK), the hands rise toward the shoulders, so the body tips
* down around the toes: the torso lowers in one straight line, a real
* push-up. Rotating about an X-axis through the foot midpoint keeps both
* feet exactly planted (they differ from the pivot only along X).
* - **Feet only (squat / hinge / roll-down):** drop the body vertically so the
* feet stay planted while the legs keep their authored FK bend: the pelvis
* lowers. Legs are never CCD-solved (that would overwrite the pose).
* With `anchors`, grounded feet are also held HORIZONTALLY: FK leg motion
* (hip/knee) displaces the feet relative to the root, and without the
* correction the feet skate across the floor while the pelvis stays put —
* backwards from real movement, where planted feet stay fixed and the
* pelvis travels (a squat sits the hips back, a hinge shifts them behind
* the heels). Only feet near the floor anchor (a swing leg in a curl or
* march must stay free), and only the average delta is corrected so
* symmetric spreads (jumping jacks) don't fight the lock.
* - **Back (dead bug / supine floor work):** translate the body vertically so
* the pelvis-to-ribcage surface stays on the floor while the limbs move.
*
* Both paths ground the visible MESH (bounding boxes), not just bone origins:
* an ankle bone sits ~0.04m above the sole, so anchoring bones alone left the
* feet sunk into the floor.
*/
import * as THREE from "three";
import type { Mannequin } from "./mannequin.js";
import { floorTargetForEffector } from "./contacts.js";
const ROOT_X = new THREE.Vector3(1, 0, 0);
/**
* Drop the whole figure so its lowest point rests on the floor. Using the
* mesh bounding-box min (not just hand/foot joints) means ANY pose grounds
* correctly: standing/plank rest on feet/hands, while supine/prone/seated
* poses rest on the back, chest, or glutes.
*/
export function groundFigure(m: Mannequin): void {
m.root.updateMatrixWorld(true);
const box = new THREE.Box3().setFromObject(m.root);
if (Number.isFinite(box.min.y)) {
m.root.position.y -= box.min.y;
m.root.updateMatrixWorld(true);
}
}
/** Resolve active grouped/per-side effector names into bone ids. */
function activeEffectorIds(m: Mannequin, active: string[]): string[] {
const ids = new Set<string>();
for (const group of active) {
for (const id of m.effectors[group] ?? []) ids.add(id);
}
return [...ids];
}
/** Average world position of a set of effector bones. */
function avgWorld(m: Mannequin, ids: string[]): THREE.Vector3 {
const p = new THREE.Vector3();
let n = 0;
for (const id of ids) {
const node = m.bones.get(id);
if (!node) continue;
p.add(node.getWorldPosition(new THREE.Vector3()));
n++;
}
return n > 0 ? p.multiplyScalar(1 / n) : p;
}
/** Rotate the whole figure about a world-space pivot (X-axis through pivot). */
function rotateRootAboutPivot(m: Mannequin, pivot: THREE.Vector3, angle: number): void {
const q = new THREE.Quaternion().setFromAxisAngle(ROOT_X, angle);
m.root.position.sub(pivot).applyQuaternion(q).add(pivot);
m.root.quaternion.premultiply(q);
m.root.updateMatrixWorld(true);
}
/** A foot whose visible surface is within this height counts as planted. */
export const GROUND_LOCK_PLANTED_MAX_Y = 0.05;
/** Shared swing-foot predicate for ground-lock and its diagnostics. */
export function isGroundLockFootPlanted(surfaceMinY: number): boolean {
return Number.isFinite(surfaceMinY) && surfaceMinY <= GROUND_LOCK_PLANTED_MAX_Y;
}
/**
* Apply ground-lock for the phase's active effector groups (see module doc).
* `anchors` (optional) maps effector bone ids to the world position each
* planted foot should hold, already transformed by the phase's yaw/travel.
*/
export function applyGroundLock(
m: Mannequin,
active: string[],
anchors?: ReadonlyMap<string, THREE.Vector3>,
): void {
if (active.length === 0) return;
const ids = activeEffectorIds(m, active);
const hands = ids.filter((id) => id.startsWith("wrist"));
const forearms = ids.filter((id) => id.startsWith("elbow"));
const feet = ids.filter((id) => id.startsWith("ankle"));
const back = ids.filter((id) => id === "pelvis" || id === "spine" || id === "chest");
const upperSupports = forearms.length > 0 ? forearms : hands;
if (back.length > 0) {
dropOwnMeshesToFloor(m, back);
return;
}
if (upperSupports.length > 0 && feet.length > 0) {
// Plant the feet FIRST: drop the body so the foot mesh rests on the floor,
// so the pivot the body then rotates about is itself at floor level. The
// rotation keeps the pivot fixed, so grounding the feet up front is what
// lets BOTH ends land — without it the pivot sits at whatever height the
// toes happened to reach and only the forearms plant while the feet float
// (the "plank feet off the ground" bug). A straight, correctly-authored
// plank has its forearms/toes near-coplanar, so the follow-up rotation is
// small; a piked pose still ends with the toes planted.
dropFeetToFloor(m, feet);
const pivot = avgWorld(m, feet);
// Newton iterations: rotate about the toes until the authored upper-body
// support reaches the floor (palms for high plank, elbows for forearm
// plank). The final bbox correction accounts for the support mesh radius.
const upperSurfaceError = (): number => {
let sum = 0;
let count = 0;
for (const id of upperSupports) {
const node = m.bones.get(id);
if (!node) continue;
const semantic = id.replace("wrist_", "hand_");
const targetY = floorTargetForEffector(m, semantic)?.y ?? 0;
sum += node.getWorldPosition(new THREE.Vector3()).y - targetY;
count++;
}
return count > 0 ? sum / count : 0;
};
for (let i = 0; i < 8; i++) {
const y0 = upperSurfaceError();
if (Math.abs(y0) < 0.004) break;
rotateRootAboutPivot(m, pivot, 0.01);
const y1 = upperSurfaceError();
rotateRootAboutPivot(m, pivot, -0.01);
const deriv = (y1 - y0) / 0.01;
if (Math.abs(deriv) < 1e-4) break;
rotateRootAboutPivot(m, pivot, THREE.MathUtils.clamp(-y0 / deriv, -0.35, 0.35));
}
// The loop above zeroes the WRIST BONE's height, but the visible hand
// (wrist ball + forearm capsule) and foot (mesh box) extend a bit below
// their bones, leaving the mesh sunk into the floor by that offset. Catch
// it with one final rigid-body vertical nudge (rotation already set the
// correct tilt; this only corrects the residual bone-vs-mesh gap).
m.root.updateMatrixWorld(true);
const box = new THREE.Box3().setFromObject(m.root);
if (box.min.y < 0) {
m.root.position.y -= box.min.y;
m.root.updateMatrixWorld(true);
}
return;
}
if (feet.length > 0) {
// Ground the FOOT MESH's lowest point, not the ankle bone's origin: the
// bone sits ~0.04m above the sole (foot box + capsule radius), so
// anchoring the bone itself left the visible foot sunk into the floor.
dropFeetToFloor(m, feet);
if (anchors) plantFeetHorizontally(m, feet, anchors);
}
}
/**
* Drop only the meshes owned by the selected bones onto the floor. `Box3` on
* a torso bone would include its child limbs, so an overhead arm could
* otherwise lift a supine person's back. Bone child subtrees are deliberately
* excluded; non-bone groups (capsules/ellipsoids) remain part of the surface.
*/
function dropOwnMeshesToFloor(m: Mannequin, boneIds: string[]): void {
const boneNodes = new Set(m.bones.values());
const box = new THREE.Box3();
const childBox = new THREE.Box3();
let found = false;
for (const id of boneIds) {
const bone = m.bones.get(id);
if (!bone) continue;
for (const child of bone.children) {
if (boneNodes.has(child)) continue;
childBox.setFromObject(child);
if (!childBox.isEmpty()) {
box.union(childBox);
found = true;
}
}
}
if (found && Number.isFinite(box.min.y)) {
m.root.position.y -= box.min.y;
m.root.updateMatrixWorld(true);
}
}
/**
* Drop the whole body vertically so the lowest FOOT-mesh point rests on the
* floor. Grounds the visible sole (bounding box), not the ankle bone, which
* sits ~0.04m above it. Shared by the feet-only path and the plank/push-up path
* (which grounds the feet before pivoting the body onto its hands).
*/
function dropFeetToFloor(m: Mannequin, feet: string[]): void {
let minY = Infinity;
for (const id of feet) {
const node = m.bones.get(id);
if (!node) continue;
const box = new THREE.Box3().setFromObject(node);
if (Number.isFinite(box.min.y)) minY = Math.min(minY, box.min.y);
}
if (Number.isFinite(minY)) {
m.root.position.y -= minY;
m.root.updateMatrixWorld(true);
}
}
/**
* Translate the root in X/Z so grounded feet return to their anchors (see
* module doc). Runs after vertical grounding so "near the floor" is judged in
* the final vertical placement.
*/
function plantFeetHorizontally(
m: Mannequin,
feet: string[],
anchors: ReadonlyMap<string, THREE.Vector3>,
): void {
const p = new THREE.Vector3();
let dx = 0;
let dz = 0;
let n = 0;
for (const id of feet) {
const anchor = anchors.get(id);
const node = m.bones.get(id);
if (!anchor || !node) continue;
const box = new THREE.Box3().setFromObject(node);
if (!isGroundLockFootPlanted(box.min.y)) continue; // swing foot
node.getWorldPosition(p);
dx += anchor.x - p.x;
dz += anchor.z - p.z;
n++;
}
if (n > 0) {
m.root.position.x += dx / n;
m.root.position.z += dz / n;
m.root.updateMatrixWorld(true);
}
}