-
Notifications
You must be signed in to change notification settings - Fork 9
Expand file tree
/
Copy pathdiagnostics.ts
More file actions
123 lines (116 loc) · 4.43 KB
/
Copy pathdiagnostics.ts
File metadata and controls
123 lines (116 loc) · 4.43 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
/** Non-mutating renderer constraint diagnostics shared by live UI and eval. */
import type { Mannequin } from "./mannequin.js";
import { floorContactHeight, measureFootContact, type BodySide } from "./contacts.js";
import { measureSelfCollisions } from "./depenetrate.js";
import { isGroundLockFootPlanted } from "./groundlock.js";
/** Maximum absolute heel/toe surface offset accepted as floor contact. */
export const FOOT_CONTACT_HEIGHT_MAX = 0.02;
/** Maximum sole tilt accepted for an explicitly plantigrade ground-lock. */
export const PLANTIGRADE_SOLE_ANGLE_MAX = 12;
/** Sampled capsule overlap tolerated as solver/numeric margin. */
export const SELF_COLLISION_DEPTH_MAX = 0.01;
export type ConstraintDiagnosticKind =
| "heel-height"
| "toe-height"
| "sole-angle"
| "grounding-rom-conflict"
| "self-collision";
/** One named outcome measured on the post-solver procedural-driver frame. */
export interface ConstraintDiagnostic {
id: string;
kind: ConstraintDiagnosticKind;
pass: boolean;
value: number;
limit: number;
unit: "m" | "deg";
detail: string;
}
export interface DiagnosticPin {
effector: string;
anchor: string;
}
function groundedFootSides(
activeGroundLock: readonly string[],
activePins: readonly DiagnosticPin[],
): BodySide[] {
return (["left", "right"] as const).filter((side) =>
activeGroundLock.includes("feet")
|| activeGroundLock.includes(`foot_${side}`)
|| activePins.some((pin) =>
pin.anchor === "floor"
&& (pin.effector === "feet" || pin.effector === `foot_${side}`)),
);
}
/**
* Measure constraints after the procedural driver's frame solvers have run.
* This precedes optional skinned-character and mocap surface reconciliation.
* Intentional tiptoe poses require toe contact without demanding a flat sole.
*/
export function measureConstraintDiagnostics(
m: Mannequin,
activeGroundLock: readonly string[],
activePins: readonly DiagnosticPin[] = [],
): ConstraintDiagnostic[] {
const out: ConstraintDiagnostic[] = [];
for (const side of groundedFootSides(activeGroundLock, activePins)) {
const foot = measureFootContact(m, side);
if (!foot) continue;
const explicitlyPinned = activePins.some((pin) =>
pin.anchor === "floor"
&& (pin.effector === "feet" || pin.effector === `foot_${side}`));
if (
!explicitlyPinned
&& activeGroundLock.includes("feet")
&& !activeGroundLock.includes(`foot_${side}`)
&& !isGroundLockFootPlanted(floorContactHeight(m, `foot_${side}`) ?? NaN)
) continue;
const height = (edge: "heel" | "toe", value: number): ConstraintDiagnostic => ({
id: `grounding:foot_${side}:${edge}-height`,
kind: `${edge}-height`,
pass: Math.abs(value) <= FOOT_CONTACT_HEIGHT_MAX,
value,
limit: FOOT_CONTACT_HEIGHT_MAX,
unit: "m",
detail: `foot_${side} ${edge} ${value.toFixed(3)}m from floor (want ±${FOOT_CONTACT_HEIGHT_MAX.toFixed(3)}m)`,
});
// Every grounded foot needs a toe/ball contact. A plantigrade declaration
// additionally promises that the heel and full sole remain down.
out.push(height("toe", foot.toeHeight));
if (foot.plantigrade) {
const heel = height("heel", foot.heelHeight);
out.push(heel);
if (!heel.pass && foot.atDorsiflexionLimit) {
out.push({
id: `grounding-rom-conflict:foot_${side}`,
kind: "grounding-rom-conflict",
pass: false,
value: Math.abs(foot.heelHeight),
limit: FOOT_CONTACT_HEIGHT_MAX,
unit: "m",
detail: `foot_${side} heel is ${Math.abs(foot.heelHeight).toFixed(3)}m off floor while ankle is at its dorsiflexion ROM limit`,
});
}
out.push({
id: `grounding:foot_${side}:sole-angle`,
kind: "sole-angle",
pass: foot.soleAngleDeg <= PLANTIGRADE_SOLE_ANGLE_MAX,
value: foot.soleAngleDeg,
limit: PLANTIGRADE_SOLE_ANGLE_MAX,
unit: "deg",
detail: `foot_${side} sole ${foot.soleAngleDeg.toFixed(1)}° from flat (want ≤ ${PLANTIGRADE_SOLE_ANGLE_MAX}°)`,
});
}
}
for (const collision of measureSelfCollisions(m)) {
out.push({
id: collision.id,
kind: "self-collision",
pass: collision.depth <= SELF_COLLISION_DEPTH_MAX,
value: collision.depth,
limit: SELF_COLLISION_DEPTH_MAX,
unit: "m",
detail: `${collision.id} residual ${collision.depth.toFixed(3)}m (want ≤ ${SELF_COLLISION_DEPTH_MAX.toFixed(3)}m)`,
});
}
return out;
}