rzen/workouts
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

Fix figure IK snapping and gate the library on a fail-hard motion checker

Browse Source 
Three solver defects made limbs teleport, twist, or windmill: write-back
angles wrapped at ±180 and lerped the long way around; branch flips landed
on configurations the anatomical write-back cannot represent, silently
pulling pinned extremities off their pins; and the degenerate straight-limb
bend plane fell back to the camera axis instead of the anatomical anterior.
solve_limb now verifies each branch reproduces the solved end before
accepting it, resolve unwraps written-back angles toward the pose they
replace, and the degenerate plane comes from the parent's anterior axis.

render.py --check replays every exercise's full tween loop and fails hard
on six invariants (pin fidelity, continuity, wraps, authored-vs-resolved
drift, ground penetration, resolved ROM); --export refuses to ship a
failing exercise. All 66 motions re-authored or retouched to pass: honest
authored angles where pins used to override them silently, grounded feet
on the seated machines, a vertical bench-press bar path, straight-armed
child's pose, a butterfly stretch seated on the mat, and FK arms where
pins forced impossible reaches. MotionSolver.swift mirrors the solver
changes line for line, held by regenerated fixtures.

Claude-Session: https://claude.ai/code/session_01PKptrgbx74peTwHGRxBojv
This commit is contained in:
rzen
2026-07-12 00:37:23 -04:00
parent 400601283e
commit 79e75a9127
303 changed files with 3114 additions and 2272 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Workouts/ExerciseFigure/MotionSolver.swift
+89 -31
View File
@@ -352,45 +352,81 @@ enum MotionSolver {
/// Analytic two-bone IK in 3D: reach from `attach` toward `target` in the plane
/// picked by the authored (FK) mid joint, then convert back to anatomical angles.
///
/// The two bend solutions are tried in preference order (arm: nearest the FK guess;
/// leg: the authored/anterior side first). The write-back keeps only a branch whose
/// recovered angles forward-kinematic back to the solved end — a branch the angle
/// representation cannot express (acos loses the bend sign, a near-sagittal limb
/// loses its axial rotation) would silently move the pinned extremity, so it is
/// rejected in favor of the flip.
private static func solveLimb(_ limb: FigureLimb, attach: Vec3, target: Vec3, guessMid: Vec3, lengths: (Double, Double), parent: Mat3) -> (BallJoint, Hinge) {
let (a, b) = lengths
let toTarget = target - attach
let d = clampUnit(toTarget.length, abs(a - b) + 0.5, a + b - 0.01)
let dirTarget = toTarget.length > 1e-9 ? toTarget.normalized : Vec3(0, -1, 0)
var normal = dirTarget.cross(guessMid - attach)
if normal.length < 1e-6 {
normal = dirTarget.cross(Vec3(0, 0, 1))
if normal.length < 1e-6 { normal = dirTarget.cross(Vec3(0, 1, 0)) }
// Guess reliability: how far the authored mid sits off the attach→target line.
// Below kneeStraightFrac the two bend solutions straddle the line and the guess
// (near-parallel) can pick neither a plane nor a side.
let gm = guessMid - attach
let gmPerp = gm - dirTarget.scaled(gm.dot(dirTarget))
let reliable = gmPerp.length >= Self.kneeStraightFrac * a
var normal: Vec3
if reliable {
normal = dirTarget.cross(gm)
} else {
// Degenerate guess: bow the joint along the anatomical anterior axis.
let anterior = parent.apply(Vec3(1, 0, 0))
let ap = anterior - dirTarget.scaled(anterior.dot(dirTarget))
normal = dirTarget.cross(ap)
if normal.length < 1e-6 { // target along the anterior axis: any plane works
normal = dirTarget.cross(Vec3(0, 0, 1))
if normal.length < 1e-6 { normal = dirTarget.cross(Vec3(0, 1, 0)) }
}
}
normal = normal.normalized
let perp = normal.cross(dirTarget)
let along = (a * a + d * d - b * b) / (2 * d)
let h = max(a * a - along * along, 0).squareRoot()
let mid: Vec3
let base = attach + dirTarget.scaled(along)
let signs: (Double, Double)
if limb.isArm {
var best: (distance: Double, mid: Vec3)?
for sign in [1.0, -1.0] {
let m = attach + (dirTarget.scaled(along) + perp.scaled(sign * h))
let distance = (m - guessMid).length
if best == nil || distance < best!.distance { best = (distance, m) }
}
mid = best!.mid
// Prefer the mid nearest the FK guess; the flip is the fallback.
let d1 = (base + perp.scaled(h) - guessMid).length
let d2 = (base + perp.scaled(-h) - guessMid).length
signs = d1 <= d2 ? (1.0, -1.0) : (-1.0, 1.0)
} else {
// A knee bends one way only. Near full extension the two knee solutions
// straddle the hip→ankle line and the authored guess (also near it) can't
// reliably pick a side — the bend plane's normal is a cross of two
// near-parallel vectors, so its sign is noise and the knee can flip behind
// the leg, swinging the thigh backward. When the authored knee sits within
// `kneeStraightFrac` of the line, treat the leg as straight and bend the
// knee anatomically forward (anterior); otherwise honor the authored side.
let gm = guessMid - attach
let gmPerp = gm - dirTarget.scaled(gm.dot(dirTarget))
let ref = gmPerp.length < Self.kneeStraightFrac * a ? parent.apply(Vec3(1, 0, 0)) : gmPerp
let sign = perp.dot(ref) >= 0 ? 1.0 : -1.0
mid = attach + (dirTarget.scaled(along) + perp.scaled(sign * h))
// A knee bends one way only. Honor the authored side when it is reliable,
// else bend anatomically forward (anterior); the flip is the fallback.
let ref = reliable ? gmPerp : parent.apply(Vec3(1, 0, 0))
let first = perp.dot(ref) >= 0 ? 1.0 : -1.0
signs = (first, -first)
}
let end = mid + (target - mid).normalized.scaled(b)
return invertLimb(limb, attach: attach, mid: mid, end: end, parent: parent)
var fallback: (BallJoint, Hinge)?
for sign in [signs.0, signs.1] {
let mid = base + perp.scaled(sign * h)
let end = mid + (target - mid).normalized.scaled(b)
let (upper, lower) = invertLimb(limb, attach: attach, mid: mid, end: end, parent: parent)
// Re-pose the recovered angles (fkLimb's math, upper→lower) and accept this
// branch only if the pinned end lands back on the solved point in the screen
// plane — that is where the pin lives. A branch the write-back mirrors leaves
// the pin in the drawing and is rejected; one off only in depth (rotation
// gated at rotMinLateral) still holds its pin. Solving is unpitched, so
// view-space (x, y) is the canvas projection (the anchor offset cancels in
// the difference).
let fu = parent.times(ballMatrix(upper, limb.sigma))
let fl = limb.isArm ? fu.times(Mat3.rotZ(lower.flexion)) : fu.times(Mat3.rotZ(-lower.flexion))
let reMid = attach + fu.apply(Vec3(0, -a, 0))
let reEnd = reMid + fl.apply(Vec3(0, -b, 0))
if fallback == nil { fallback = (upper, lower) }
if hypot(reEnd.x - end.x, reEnd.y - end.y) <= Self.branchReproTol {
return (upper, lower)
}
}
return fallback!
}
/// Axial rotation of a two-bone limb is recoverable only from the lower bone's
@@ -398,12 +434,25 @@ enum MotionSolver {
/// rotation is left at 0 (see `invertLimb`).
private static let rotMinLateral = 0.08
/// When a leg's authored knee sits within this fraction of the thigh length off
/// the hip→ankle line (~sin 8.6°), the leg is treated as straight and its IK knee
/// is bent anatomically forward rather than trusting the (unreliable) authored
/// side (see `solveLimb`).
/// When an authored mid joint (knee or elbow) sits within this fraction of the
/// upper bone length off the attach→target line (~sin 8.6°), the FK guess is
/// unreliable: the two bend solutions straddle the line and the near-parallel guess
/// can pick neither a side nor a plane. The bend plane is then taken from the
/// anatomical anterior axis (knees forward, elbow flexion carries the forearm
/// anterior) instead of the guess, for both arms and legs (see `solveLimb`).
private static let kneeStraightFrac = 0.15
/// A recovered IK branch is kept only if its re-posed extremity lands within this
/// many canvas units of the solved point, measured in the screen plane where the
/// pin lives (see `solveLimb`). A branch the anatomical write-back cannot
/// represent — the acos bend-sign loss or a rotation gated at `rotMinLateral` —
/// mirrors the lower bone and misses by a wide margin; the small residual a
/// correctly-authored, on-pin branch leaves when its rotation grazes the gating
/// boundary is only a couple of units, so this margin flips genuine mirrors while
/// leaving well-authored branches on their FK-guess side (a tighter margin would
/// flip them too, shifting sound geometry).
private static let branchReproTol = 4.0
/// Recover anatomical angles from limb joint positions (the inverse of `fkLimb`,
/// ignoring the foot). Assumes |abduction| < 90; the leg's rotation sign flips
/// because knees hinge backward.
@@ -449,7 +498,16 @@ enum MotionSolver {
let target = viewFromCanvas(pin, anchor: anchor, depth: chainPts[2].z)
let lengths: (Double, Double) = limb.isArm ? (prof.upperArm, prof.foreArm) : (prof.thigh, prof.shin)
let parent = limb.isArm ? p.f2 : p.fRoot
let (upper, lower) = solveLimb(limb, attach: attach, target: target, guessMid: chainPts[1], lengths: lengths, parent: parent)
var upper: BallJoint
let lower: Hinge
(upper, lower) = solveLimb(limb, attach: attach, target: target, guessMid: chainPts[1], lengths: lengths, parent: parent)
// invertLimb returns principal-value angles (atan2/asin); unwrap the
// flexion/rotation toward the pose being replaced so key frames lerp the
// short way and tween ticks stay continuous. Abduction is asin-ranged and
// cannot wrap.
let prev = frame.ball(for: limb)
upper.flexion += 360 * ((prev.flexion - upper.flexion) / 360).rounded()
upper.rotation += 360 * ((prev.rotation - upper.rotation) / 360).rounded()
frame.setUpper(upper, for: limb)
frame.setLower(lower, for: limb)
solved = true