|
3 | 3 | * invariant checks are written in. All angles in degrees, distances in metres. |
4 | 4 | */ |
5 | 5 |
|
6 | | -import type { PhasePose, Vec3 } from "./probe.js"; |
| 6 | +import type { PhasePose, ProbeResult, Quat, Vec3 } from "./probe.js"; |
7 | 7 |
|
8 | 8 | const RAD2DEG = 180 / Math.PI; |
9 | 9 |
|
@@ -67,6 +67,157 @@ export function heightOf(pose: PhasePose, id: string): number { |
67 | 67 | return bone(pose, id)[1]; |
68 | 68 | } |
69 | 69 |
|
| 70 | +/** World-space distance between two body landmarks. */ |
| 71 | +export function distanceBetween(pose: PhasePose, a: string, b: string): number { |
| 72 | + return norm(sub(bone(pose, a), bone(pose, b))); |
| 73 | +} |
| 74 | + |
| 75 | +function rotateByQuat(v: Vec3, q: Quat): Vec3 { |
| 76 | + const [x, y, z, w] = q; |
| 77 | + const tx = 2 * (y * v[2] - z * v[1]); |
| 78 | + const ty = 2 * (z * v[0] - x * v[2]); |
| 79 | + const tz = 2 * (x * v[1] - y * v[0]); |
| 80 | + return [ |
| 81 | + v[0] + w * tx + (y * tz - z * ty), |
| 82 | + v[1] + w * ty + (z * tx - x * tz), |
| 83 | + v[2] + w * tz + (x * ty - y * tx), |
| 84 | + ]; |
| 85 | +} |
| 86 | + |
| 87 | +/** Angle between the palm face normal and the downward floor normal. */ |
| 88 | +export function palmFloorAngleDeg(pose: PhasePose, side: "left" | "right"): number { |
| 89 | + const q = pose.boneQuaternions.get(`wrist_${side}`); |
| 90 | + if (!q) return 180; |
| 91 | + // The flattened palm's face normal is mirrored local X on the two wrists. |
| 92 | + return angleBetweenDeg(rotateByQuat(side === "left" ? [1, 0, 0] : [-1, 0, 0], q), [0, -1, 0]); |
| 93 | +} |
| 94 | + |
| 95 | +const MASS_WEIGHTS: ReadonlyArray<readonly [string, number]> = [ |
| 96 | + ["pelvis", 0.22], ["spine", 0.13], ["chest", 0.2], ["head", 0.08], |
| 97 | + ["hip_left", 0.07], ["hip_right", 0.07], ["knee_left", 0.05], ["knee_right", 0.05], |
| 98 | + ["shoulder_left", 0.025], ["shoulder_right", 0.025], |
| 99 | + ["elbow_left", 0.025], ["elbow_right", 0.025], |
| 100 | + ["ankle_left", 0.015], ["ankle_right", 0.015], |
| 101 | +]; |
| 102 | + |
| 103 | +/** Approximate whole-body COM from anthropometrically weighted landmarks. */ |
| 104 | +export function centerOfMass(pose: PhasePose): Vec3 { |
| 105 | + let x = 0, y = 0, z = 0, total = 0; |
| 106 | + for (const [id, weight] of MASS_WEIGHTS) { |
| 107 | + const p = pose.bones.get(id); |
| 108 | + if (!p) continue; |
| 109 | + x += p[0] * weight; y += p[1] * weight; z += p[2] * weight; total += weight; |
| 110 | + } |
| 111 | + return total > 0 ? [x / total, y / total, z / total] : [0, 0, 0]; |
| 112 | +} |
| 113 | + |
| 114 | +function supportBoneIds(pose: PhasePose): string[] { |
| 115 | + const ids = new Set<string>(); |
| 116 | + const addGroup = (name: string) => { |
| 117 | + if (name === "feet") { ids.add("ankle_left"); ids.add("ankle_right"); } |
| 118 | + if (name === "hands") { ids.add("wrist_left"); ids.add("wrist_right"); } |
| 119 | + if (name === "forearms") { ids.add("elbow_left"); ids.add("elbow_right"); } |
| 120 | + }; |
| 121 | + pose.groundLock.forEach(addGroup); |
| 122 | + for (const reach of pose.reaches) { |
| 123 | + if (reach.target !== "floor") continue; |
| 124 | + addGroup(reach.effector); |
| 125 | + const mapped = reach.effector.replace("hand_", "wrist_").replace("foot_", "ankle_"); |
| 126 | + if (pose.bones.has(mapped)) ids.add(mapped); |
| 127 | + } |
| 128 | + for (const pin of pose.pins) { |
| 129 | + addGroup(pin.effector); |
| 130 | + const mapped = pin.effector.replace("hand_", "wrist_").replace("foot_", "ankle_"); |
| 131 | + if (pose.bones.has(mapped)) ids.add(mapped); |
| 132 | + } |
| 133 | + // Floor poses also distribute load through the torso/pelvis even when the |
| 134 | + // authored contact declaration only mentions hands or feet. |
| 135 | + for (const id of ["pelvis", "chest", "head"]) { |
| 136 | + const p = pose.bones.get(id); |
| 137 | + if (p && p[1] < 0.5) ids.add(id); |
| 138 | + } |
| 139 | + return [...ids]; |
| 140 | +} |
| 141 | + |
| 142 | +/** Horizontal COM distance outside the active support bounding box (0 = inside). */ |
| 143 | +export function balanceOverflow(pose: PhasePose): number { |
| 144 | + const supports = supportBoneIds(pose).map((id) => bone(pose, id)); |
| 145 | + if (supports.length < 2) return 0; |
| 146 | + const com = centerOfMass(pose); |
| 147 | + const margin = 0.14; |
| 148 | + const minX = Math.min(...supports.map((p) => p[0])) - margin; |
| 149 | + const maxX = Math.max(...supports.map((p) => p[0])) + margin; |
| 150 | + const minZ = Math.min(...supports.map((p) => p[2])) - margin; |
| 151 | + const maxZ = Math.max(...supports.map((p) => p[2])) + margin; |
| 152 | + const dx = Math.max(minX - com[0], 0, com[0] - maxX); |
| 153 | + const dz = Math.max(minZ - com[2], 0, com[2] - maxZ); |
| 154 | + return Math.hypot(dx, dz); |
| 155 | +} |
| 156 | + |
| 157 | +/** Clearance between the head sphere and known prop geometry; Infinity if none. */ |
| 158 | +export function headPropClearance(result: ProbeResult, pose: PhasePose): number { |
| 159 | + const h = bone(pose, "head"); |
| 160 | + let clearance = Infinity; |
| 161 | + if (result.propTypes.includes("bar")) { |
| 162 | + const closestX = Math.max(-0.6, Math.min(0.6, h[0])); |
| 163 | + clearance = Math.min(clearance, Math.hypot(h[0] - closestX, h[1] - 2.3, h[2]) - 0.13); |
| 164 | + } |
| 165 | + if (result.propTypes.includes("wall")) { |
| 166 | + clearance = Math.min(clearance, Math.abs(h[2] - (-0.29)) - 0.105); |
| 167 | + } |
| 168 | + if (result.propTypes.includes("chair")) { |
| 169 | + const dx = Math.max(Math.abs(h[0]) - 0.21, 0); |
| 170 | + const dy = Math.max(Math.abs(h[1] - 0.78) - 0.25, 0); |
| 171 | + const dz = Math.max(Math.abs(h[2] - (-0.34)) - 0.03, 0); |
| 172 | + clearance = Math.min(clearance, Math.hypot(dx, dy, dz) - 0.105); |
| 173 | + } |
| 174 | + return clearance; |
| 175 | +} |
| 176 | + |
| 177 | +/** Fastest landmark's average speed from the previous endpoint into this phase. */ |
| 178 | +export function phaseMaxLandmarkSpeed(previous: PhasePose | null, pose: PhasePose): number { |
| 179 | + if (!previous || pose.durationSec <= 0) return 0; |
| 180 | + let max = 0; |
| 181 | + for (const [id, p] of pose.bones) { |
| 182 | + const before = previous.bones.get(id); |
| 183 | + if (before) max = Math.max(max, norm(sub(p, before)) / pose.durationSec); |
| 184 | + } |
| 185 | + return max; |
| 186 | +} |
| 187 | + |
| 188 | +export function footSkateDistance(previous: PhasePose, pose: PhasePose, side: "left" | "right"): number { |
| 189 | + const id = `ankle_${side}`; |
| 190 | + const local = (p: Vec3, phase: PhasePose): readonly [number, number] => { |
| 191 | + const x = p[0] - phase.rootOffset[0], z = p[2] - phase.rootOffset[2]; |
| 192 | + const c = Math.cos(-phase.rootYaw), s = Math.sin(-phase.rootYaw); |
| 193 | + return [x * c - z * s, x * s + z * c]; |
| 194 | + }; |
| 195 | + const a = local(bone(previous, id), previous), b = local(bone(pose, id), pose); |
| 196 | + return Math.hypot(b[0] - a[0], b[1] - a[1]); |
| 197 | +} |
| 198 | + |
| 199 | +/** Drift of the planted foot-pair center, ignoring intentional stance-width changes. */ |
| 200 | +export function feetCenterSkateDistance(previous: PhasePose, pose: PhasePose): number { |
| 201 | + const delta = (side: "left" | "right") => { |
| 202 | + const id = `ankle_${side}`; |
| 203 | + const a = bone(previous, id), b = bone(pose, id); |
| 204 | + const unyaw = (p: Vec3, phase: PhasePose) => { |
| 205 | + const x = p[0] - phase.rootOffset[0], z = p[2] - phase.rootOffset[2]; |
| 206 | + const c = Math.cos(-phase.rootYaw), s = Math.sin(-phase.rootYaw); |
| 207 | + return [x * c - z * s, x * s + z * c] as const; |
| 208 | + }; |
| 209 | + const aa = unyaw(a, previous), bb = unyaw(b, pose); |
| 210 | + return [bb[0] - aa[0], bb[1] - aa[1]] as const; |
| 211 | + }; |
| 212 | + const l = delta("left"), r = delta("right"); |
| 213 | + return Math.hypot((l[0] + r[0]) / 2, (l[1] + r[1]) / 2); |
| 214 | +} |
| 215 | + |
| 216 | +export function footIsSupported(pose: PhasePose, side: "left" | "right"): boolean { |
| 217 | + return pose.groundLock.includes("feet") || pose.pins.some((p) => |
| 218 | + (p.effector === "feet" || p.effector === `foot_${side}`) && p.anchor === "floor"); |
| 219 | +} |
| 220 | + |
70 | 221 | /** Lowest bone height in the pose (should never be much below 0). */ |
71 | 222 | export function lowestPoint(pose: PhasePose): number { |
72 | 223 | let min = Infinity; |
|
0 commit comments