rzen
79e75a9127
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
907 lines
47 KiB
Swift
907 lines
47 KiB
Swift
//
|
||
// MotionSolver.swift
|
||
// Workouts
|
||
//
|
||
// Copyright 2026 Rouslan Zenetl. All Rights Reserved.
|
||
//
|
||
|
||
import CoreGraphics
|
||
import Foundation
|
||
|
||
/// Swift port of the Exercise Library's anatomical 3D solver (`Exercise Library/
|
||
/// kinematics.py` + `render.py`'s `frame_geometry`). It poses one shared skeleton by
|
||
/// anatomical joint angles, runs forward kinematics in ISB model space (X anterior,
|
||
/// Y up, Z anatomical right), projects orthographically through a per-exercise camera
|
||
/// yaw, and resolves each frame into drawable canvas geometry — near/far shading, the
|
||
/// readability offset, draw order, spine curve, and the gaze nose tick. The math is
|
||
/// kept 1:1 with the Python so both renderers produce the same figure; it is held to
|
||
/// the reference by `WorkoutsTests/Fixtures/figure-fixtures.json`. Change them in
|
||
/// lockstep.
|
||
///
|
||
/// Rotation conventions (all degrees): root = Ry(yaw)·Rz(−pitch)·Rx(roll) after the
|
||
/// camera Ry(−camYaw); ball joints (shoulder/hip) = Rz(flexion)·Rx(−σ·abduction)·
|
||
/// Ry(−σ·rotation) with σ = +1 right / −1 left; the knee hinges backward via Rz(−flexion);
|
||
/// spine segments = Rz(−flexion)·Rx(lateral)·Ry(−rotation); the neck = Rz(−flexion)·
|
||
/// Ry(−rotation). Canvas maps view x → x, view y → −y (origin at the root anchor).
|
||
|
||
// MARK: - Linear algebra
|
||
|
||
/// A 3-vector in model / view space.
|
||
struct Vec3 {
|
||
var x, y, z: Double
|
||
init(_ x: Double, _ y: Double, _ z: Double) { self.x = x; self.y = y; self.z = z }
|
||
static func + (a: Vec3, b: Vec3) -> Vec3 { Vec3(a.x + b.x, a.y + b.y, a.z + b.z) }
|
||
static func - (a: Vec3, b: Vec3) -> Vec3 { Vec3(a.x - b.x, a.y - b.y, a.z - b.z) }
|
||
func scaled(_ s: Double) -> Vec3 { Vec3(x * s, y * s, z * s) }
|
||
func dot(_ b: Vec3) -> Double { x * b.x + y * b.y + z * b.z }
|
||
func cross(_ b: Vec3) -> Vec3 { Vec3(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x) }
|
||
var length: Double { dot(self).squareRoot() }
|
||
var normalized: Vec3 { let d = length; return d > 1e-9 ? scaled(1 / d) : Vec3(0, 0, 0) }
|
||
}
|
||
|
||
/// A row-major 3×3 rotation matrix (rows r0/r1/r2), matching the reference's
|
||
/// tuple-of-rows so multiplication and transpose stay identical.
|
||
struct Mat3 {
|
||
var r0, r1, r2: Vec3
|
||
|
||
static func rotX(_ deg: Double) -> Mat3 {
|
||
let r = deg * .pi / 180, c = cos(r), s = sin(r)
|
||
return Mat3(r0: Vec3(1, 0, 0), r1: Vec3(0, c, -s), r2: Vec3(0, s, c))
|
||
}
|
||
static func rotY(_ deg: Double) -> Mat3 {
|
||
let r = deg * .pi / 180, c = cos(r), s = sin(r)
|
||
return Mat3(r0: Vec3(c, 0, s), r1: Vec3(0, 1, 0), r2: Vec3(-s, 0, c))
|
||
}
|
||
static func rotZ(_ deg: Double) -> Mat3 {
|
||
let r = deg * .pi / 180, c = cos(r), s = sin(r)
|
||
return Mat3(r0: Vec3(c, -s, 0), r1: Vec3(s, c, 0), r2: Vec3(0, 0, 1))
|
||
}
|
||
|
||
/// `self · v`.
|
||
func apply(_ v: Vec3) -> Vec3 { Vec3(r0.dot(v), r1.dot(v), r2.dot(v)) }
|
||
|
||
/// `self · b`.
|
||
func times(_ b: Mat3) -> Mat3 {
|
||
let c0 = Vec3(b.r0.x, b.r1.x, b.r2.x)
|
||
let c1 = Vec3(b.r0.y, b.r1.y, b.r2.y)
|
||
let c2 = Vec3(b.r0.z, b.r1.z, b.r2.z)
|
||
return Mat3(r0: Vec3(r0.dot(c0), r0.dot(c1), r0.dot(c2)),
|
||
r1: Vec3(r1.dot(c0), r1.dot(c1), r1.dot(c2)),
|
||
r2: Vec3(r2.dot(c0), r2.dot(c1), r2.dot(c2)))
|
||
}
|
||
|
||
var transposed: Mat3 {
|
||
Mat3(r0: Vec3(r0.x, r1.x, r2.x), r1: Vec3(r0.y, r1.y, r2.y), r2: Vec3(r0.z, r1.z, r2.z))
|
||
}
|
||
}
|
||
|
||
/// Left-to-right matrix product (`chain(a, b, c) == a·b·c`).
|
||
private func chain(_ mats: Mat3...) -> Mat3 {
|
||
var m = mats[0]
|
||
for n in mats.dropFirst() { m = m.times(n) }
|
||
return m
|
||
}
|
||
|
||
private func clampUnit(_ x: Double, _ lo: Double = -1, _ hi: Double = 1) -> Double { max(lo, min(hi, x)) }
|
||
|
||
// MARK: - Frame model
|
||
|
||
/// The four two-bone limbs, keyed by their motion-script names.
|
||
enum FigureLimb: String, CaseIterable {
|
||
case armR = "arm_r"
|
||
case armL = "arm_l"
|
||
case legR = "leg_r"
|
||
case legL = "leg_l"
|
||
|
||
var isArm: Bool { self == .armR || self == .armL }
|
||
/// Side sign for a ball joint's abduction/rotation (+1 right, −1 left).
|
||
var sigma: Double { (self == .armR || self == .legR) ? 1 : -1 }
|
||
/// The key a planted extremity uses in a key frame's `pins`.
|
||
var pinKey: String {
|
||
switch self {
|
||
case .armR: "hand_r"
|
||
case .armL: "hand_l"
|
||
case .legR: "foot_r"
|
||
case .legL: "foot_l"
|
||
}
|
||
}
|
||
}
|
||
|
||
/// Shoulder / hip: forward flexion, abduction away from the midline, external rotation.
|
||
struct BallJoint { var flexion, abduction, rotation: Double }
|
||
/// One spine segment: forward curl, right side-bend, turn-right rotation.
|
||
struct SpineSeg { var flexion, lateral, rotation: Double }
|
||
/// Neck: forward flexion plus turn-right rotation.
|
||
struct NeckJoint { var flexion, rotation: Double }
|
||
/// Elbow / knee / ankle: a single flexion angle (knees hinge backward automatically).
|
||
struct Hinge { var flexion: Double }
|
||
|
||
/// A key frame expanded to full anatomical dicts (defaults filled in), the shape FK
|
||
/// and tweening operate on. Pinned limbs' `Ball`/`Hinge` values are overwritten by the
|
||
/// IK solution during `resolve`, so poses always interpolate in anatomical space.
|
||
struct NormalizedFrame {
|
||
var rootPos: CGPoint
|
||
var yaw, pitch, roll: Double
|
||
var spine: [SpineSeg] // two chained segments
|
||
var neck: NeckJoint
|
||
var head: Double // extra gaze pitch (flexion)
|
||
var shoulderR, shoulderL, hipR, hipL: BallJoint
|
||
var elbowR, elbowL, kneeR, kneeL, ankleR, ankleL: Hinge
|
||
var pins: [String: CGPoint]
|
||
var hold, tween: Double
|
||
|
||
func ball(for limb: FigureLimb) -> BallJoint {
|
||
switch limb { case .armR: shoulderR; case .armL: shoulderL; case .legR: hipR; case .legL: hipL }
|
||
}
|
||
func lower(for limb: FigureLimb) -> Hinge {
|
||
switch limb { case .armR: elbowR; case .armL: elbowL; case .legR: kneeR; case .legL: kneeL }
|
||
}
|
||
func ankle(for limb: FigureLimb) -> Hinge { limb == .legR ? ankleR : ankleL }
|
||
|
||
mutating func setUpper(_ b: BallJoint, for limb: FigureLimb) {
|
||
switch limb { case .armR: shoulderR = b; case .armL: shoulderL = b; case .legR: hipR = b; case .legL: hipL = b }
|
||
}
|
||
mutating func setLower(_ h: Hinge, for limb: FigureLimb) {
|
||
switch limb { case .armR: elbowR = h; case .armL: elbowL = h; case .legR: kneeR = h; case .legL: kneeL = h }
|
||
}
|
||
}
|
||
|
||
/// Within a limb pair, the member nearer the camera (`near`) draws dark and in front;
|
||
/// the far member draws light, behind, and nudged by the readability offset.
|
||
enum Shade { case near, far }
|
||
|
||
/// The two equipment inks: recessive `equipment` gray for scene shapes and cables
|
||
/// behind the figure, darker `prop` gray for joint-attached items over the limbs.
|
||
enum PropInk {
|
||
case equipment, prop
|
||
|
||
init(_ name: String?) { self = name == "prop" ? .prop : .equipment }
|
||
}
|
||
|
||
/// One resolved equipment drawing primitive, in canvas coordinates — the shape the
|
||
/// prop layer reduces to for a single frame (mirroring the reference renderer's
|
||
/// `resolve_props` output; kept 1:1, change them in lockstep).
|
||
enum PropPrimitive {
|
||
case line(points: [CGPoint], width: Double, ink: PropInk)
|
||
/// A closed, filled, outlined polygon — an extruded scene slab. Degenerates to
|
||
/// the plain line whenever the sweep collapses edge-on (the authored view).
|
||
case poly(points: [CGPoint], width: Double, ink: PropInk)
|
||
case circle(center: CGPoint, radius: Double, width: Double, fill: Bool, ink: PropInk)
|
||
}
|
||
|
||
/// A pose resolved to drawable canvas points (plus the depth-sorted draw order and
|
||
/// per-limb shading the view needs).
|
||
struct FigureGeometry {
|
||
var headCenter: CGPoint
|
||
var headRadius: Double
|
||
/// Nil when the face points at (or away from) the camera — no nose tick.
|
||
var noseStart, noseEnd: CGPoint?
|
||
/// Quadratic Bézier through pelvis → mid → neck (control = 2·mid − (pelvis+neck)/2).
|
||
var spineStart, spineControl, spineEnd: CGPoint
|
||
/// Shoulder girdle and pelvis bars, drawn with the spine: left attach → center →
|
||
/// right attach, far offsets included so the bars meet the drawn limbs exactly.
|
||
var girdle: [CGPoint]
|
||
var pelvisBar: [CGPoint]
|
||
/// The exercise mat: a world-space quad on the ground plane, sized to the
|
||
/// motion's footprint and rotating with the camera. Nil when no mat was given.
|
||
var floor: [CGPoint]?
|
||
/// Attach → elbow/knee → hand (arms: 3 points); hip → knee → ankle → toe (legs: 4).
|
||
var limbs: [FigureLimb: [CGPoint]]
|
||
/// Parts far-to-near, `"head"` always last (`"spine"`, `"arm_r"`, … then `"head"`).
|
||
var order: [String]
|
||
var shade: [FigureLimb: Shade]
|
||
/// Per-vertex nearness in [0, 1] (1 = near/dark side), one value per joint of each
|
||
/// limb chain, driving the shading gradient: each joint is toned by its own camera
|
||
/// depth so the ink flows along a limb that reaches in depth. Unlike `shade` (which
|
||
/// stays binary for geometry and draw order) this fades to a shared mid-tone as a
|
||
/// pair crosses, so the ink never snaps. See `frameGeometry`.
|
||
var nearness: [FigureLimb: [Double]]
|
||
/// The equipment layer resolved for this frame: scene shapes and cables drawn
|
||
/// behind the figure, joint-attached items drawn over the limbs (before the head).
|
||
var propsBackground: [PropPrimitive] = []
|
||
var propsForeground: [PropPrimitive] = []
|
||
}
|
||
|
||
/// FK output in view space (x right, y up, z toward the camera; origin at the root).
|
||
struct FigurePose {
|
||
var pelvis, mid, neckB, head: Vec3
|
||
var shoulderR, shoulderL, hipR, hipL: Vec3
|
||
var limbs: [FigureLimb: [Vec3]]
|
||
var f2, fRoot: Mat3 // spine-top and root frames, for IK inversion
|
||
var noseDir: Vec3
|
||
/// How side-on the view is: 1 in profile (lateral axis is pure depth), 0 face-on.
|
||
var k: Double
|
||
|
||
func attach(for limb: FigureLimb) -> Vec3 {
|
||
switch limb { case .armR: shoulderR; case .armL: shoulderL; case .legR: hipR; case .legL: hipL }
|
||
}
|
||
}
|
||
|
||
// MARK: - Solver
|
||
|
||
enum MotionSolver {
|
||
private static let depthBucket = 3.0
|
||
private static let pairs: [(FigureLimb, FigureLimb)] = [(.armR, .armL), (.legR, .legL)]
|
||
/// Depth shading is a smooth per-vertex gradient: a joint reaches full near/far
|
||
/// contrast once it sits this fraction of the shoulder/pelvis half-width in front of
|
||
/// (or behind) its pair's central depth plane, so a straight limb in profile is still
|
||
/// fully dark/light. See `frameGeometry` and `render.py`'s `SHADE_SPAN_FRAC` — the
|
||
/// two must stay in sync.
|
||
private static let shadeSpanFrac = 0.6
|
||
/// Fixed draw rank breaking depth-bucket ties (far parts first, `spine` mid-stack).
|
||
private static let fixedRank: [String: Int] = ["arm_l": 0, "leg_l": 1, "spine": 2, "arm_r": 3, "leg_r": 4]
|
||
|
||
/// Unit vector for a y-up angle in y-down canvas coordinates (props' fixed angles).
|
||
static func direction(_ degrees: Double) -> CGVector {
|
||
let r = degrees * .pi / 180
|
||
return CGVector(dx: cos(r), dy: -sin(r))
|
||
}
|
||
|
||
// MARK: Normalize / tween
|
||
|
||
/// Expand a key frame to a `NormalizedFrame` with defaults filled in.
|
||
static func normalize(_ kf: MotionKeyFrame) -> NormalizedFrame {
|
||
let spineValues = kf.spine ?? [.scalar(0), .scalar(0)]
|
||
let spine = spineValues.map { SpineSeg(flexion: $0.flexion, lateral: $0.lateral, rotation: $0.rotation) }
|
||
func ball(_ v: JointValue?) -> BallJoint { BallJoint(flexion: v?.flexion ?? 0, abduction: v?.abduction ?? 0, rotation: v?.rotation ?? 0) }
|
||
func hinge(_ v: JointValue?) -> Hinge { Hinge(flexion: v?.flexion ?? 0) }
|
||
var pins: [String: CGPoint] = [:]
|
||
for (key, xy) in kf.pins ?? [:] where xy.count == 2 { pins[key] = CGPoint(x: xy[0], y: xy[1]) }
|
||
return NormalizedFrame(
|
||
rootPos: CGPoint(x: kf.root.pos[0], y: kf.root.pos[1]),
|
||
yaw: kf.root.yaw ?? 0, pitch: kf.root.pitch ?? 0, roll: kf.root.roll ?? 0,
|
||
spine: spine,
|
||
neck: NeckJoint(flexion: kf.neck?.flexion ?? 0, rotation: kf.neck?.rotation ?? 0),
|
||
head: kf.head?.flexion ?? 0,
|
||
shoulderR: ball(kf.shoulderR), shoulderL: ball(kf.shoulderL),
|
||
hipR: ball(kf.hipR), hipL: ball(kf.hipL),
|
||
elbowR: hinge(kf.elbowR), elbowL: hinge(kf.elbowL),
|
||
kneeR: hinge(kf.kneeR), kneeL: hinge(kf.kneeL),
|
||
ankleR: hinge(kf.ankleR), ankleL: hinge(kf.ankleL),
|
||
pins: pins,
|
||
hold: kf.hold ?? 0.5, tween: kf.tween ?? 0.6)
|
||
}
|
||
|
||
/// Ease-in-out: 3t² − 2t³.
|
||
static func ease(_ t: Double) -> Double { 3 * t * t - 2 * t * t * t }
|
||
|
||
/// Interpolate two resolved frames — plain linear per degree of freedom, so limbs
|
||
/// swing in natural anatomical arcs. A pin survives the tween only when planted in
|
||
/// BOTH neighboring key frames (a one-sided pin releases naturally).
|
||
static func lerpFrames(_ a: NormalizedFrame, _ b: NormalizedFrame, _ t: Double) -> NormalizedFrame {
|
||
func n(_ x: Double, _ y: Double) -> Double { x + (y - x) * t }
|
||
func lerpBall(_ p: BallJoint, _ q: BallJoint) -> BallJoint {
|
||
BallJoint(flexion: n(p.flexion, q.flexion), abduction: n(p.abduction, q.abduction), rotation: n(p.rotation, q.rotation))
|
||
}
|
||
func lerpHinge(_ p: Hinge, _ q: Hinge) -> Hinge { Hinge(flexion: n(p.flexion, q.flexion)) }
|
||
var pins: [String: CGPoint] = [:]
|
||
for (key, pa) in a.pins {
|
||
guard let pb = b.pins[key] else { continue }
|
||
pins[key] = CGPoint(x: n(pa.x, pb.x), y: n(pa.y, pb.y))
|
||
}
|
||
return NormalizedFrame(
|
||
rootPos: CGPoint(x: n(a.rootPos.x, b.rootPos.x), y: n(a.rootPos.y, b.rootPos.y)),
|
||
yaw: n(a.yaw, b.yaw), pitch: n(a.pitch, b.pitch), roll: n(a.roll, b.roll),
|
||
spine: zip(a.spine, b.spine).map { SpineSeg(flexion: n($0.flexion, $1.flexion), lateral: n($0.lateral, $1.lateral), rotation: n($0.rotation, $1.rotation)) },
|
||
neck: NeckJoint(flexion: n(a.neck.flexion, b.neck.flexion), rotation: n(a.neck.rotation, b.neck.rotation)),
|
||
head: n(a.head, b.head),
|
||
shoulderR: lerpBall(a.shoulderR, b.shoulderR), shoulderL: lerpBall(a.shoulderL, b.shoulderL),
|
||
hipR: lerpBall(a.hipR, b.hipR), hipL: lerpBall(a.hipL, b.hipL),
|
||
elbowR: lerpHinge(a.elbowR, b.elbowR), elbowL: lerpHinge(a.elbowL, b.elbowL),
|
||
kneeR: lerpHinge(a.kneeR, b.kneeR), kneeL: lerpHinge(a.kneeL, b.kneeL),
|
||
ankleR: lerpHinge(a.ankleR, b.ankleR), ankleL: lerpHinge(a.ankleL, b.ankleL),
|
||
pins: pins, hold: a.hold, tween: a.tween)
|
||
}
|
||
|
||
// MARK: Forward kinematics
|
||
|
||
/// Local rotation of a ball joint (shoulder/hip) for side sign `sigma`.
|
||
private static func ballMatrix(_ j: BallJoint, _ sigma: Double) -> Mat3 {
|
||
chain(Mat3.rotZ(j.flexion), Mat3.rotX(-sigma * j.abduction), Mat3.rotY(-sigma * j.rotation))
|
||
}
|
||
|
||
/// FK one limb from its resolved attach point (arm: [shoulder, elbow, hand];
|
||
/// leg: [hip, knee, ankle, toe]).
|
||
private static func fkLimb(_ limb: FigureLimb, attach: Vec3, upper: BallJoint, lower: Hinge, ankle: Hinge, prof: SkeletonProfile, parent: Mat3) -> [Vec3] {
|
||
let fu = parent.times(ballMatrix(upper, limb.sigma))
|
||
if limb.isArm {
|
||
let elbow = attach + fu.apply(Vec3(0, -prof.upperArm, 0))
|
||
let fl = fu.times(Mat3.rotZ(lower.flexion))
|
||
let hand = elbow + fl.apply(Vec3(0, -prof.foreArm, 0))
|
||
return [attach, elbow, hand]
|
||
}
|
||
let knee = attach + fu.apply(Vec3(0, -prof.thigh, 0))
|
||
let fl = fu.times(Mat3.rotZ(-lower.flexion))
|
||
let ankleJoint = knee + fl.apply(Vec3(0, -prof.shin, 0))
|
||
let toe = ankleJoint + fl.times(Mat3.rotZ(ankle.flexion)).apply(Vec3(prof.foot, 0, 0))
|
||
return [attach, knee, ankleJoint, toe]
|
||
}
|
||
|
||
/// FK a normalized frame into view space through the camera yaw; `camPitch`
|
||
/// tilts the viewpoint down from slightly above (the scene rotates about the root).
|
||
static func pose(_ nf: NormalizedFrame, prof: SkeletonProfile, cam: Double,
|
||
camPitch: Double = 0) -> FigurePose {
|
||
let fRoot = chain(Mat3.rotX(camPitch), Mat3.rotY(-cam),
|
||
Mat3.rotY(nf.yaw), Mat3.rotZ(-nf.pitch), Mat3.rotX(nf.roll))
|
||
let origin = Vec3(0, 0, 0)
|
||
let s1 = nf.spine[0], s2 = nf.spine[1]
|
||
let f1 = chain(fRoot, Mat3.rotZ(-s1.flexion), Mat3.rotX(s1.lateral), Mat3.rotY(-s1.rotation))
|
||
let mid = origin + f1.apply(Vec3(0, prof.spine1, 0))
|
||
let f2 = chain(f1, Mat3.rotZ(-s2.flexion), Mat3.rotX(s2.lateral), Mat3.rotY(-s2.rotation))
|
||
let neckB = mid + f2.apply(Vec3(0, prof.spine2, 0))
|
||
let fn = chain(f2, Mat3.rotZ(-nf.neck.flexion), Mat3.rotY(-nf.neck.rotation))
|
||
let head = neckB + fn.apply(Vec3(0, prof.neck, 0))
|
||
let noseDir = fn.times(Mat3.rotZ(-nf.head)).apply(Vec3(1, 0, 0))
|
||
var p = FigurePose(
|
||
pelvis: origin, mid: mid, neckB: neckB, head: head,
|
||
shoulderR: neckB + f2.apply(Vec3(0, 0, prof.shoulderHalf)),
|
||
shoulderL: neckB + f2.apply(Vec3(0, 0, -prof.shoulderHalf)),
|
||
hipR: origin + fRoot.apply(Vec3(0, 0, prof.hipHalf)),
|
||
hipL: origin + fRoot.apply(Vec3(0, 0, -prof.hipHalf)),
|
||
limbs: [:], f2: f2, fRoot: fRoot, noseDir: noseDir,
|
||
k: abs(fRoot.apply(Vec3(0, 0, 1)).z))
|
||
for limb in FigureLimb.allCases {
|
||
let parent = limb.isArm ? f2 : fRoot
|
||
p.limbs[limb] = fkLimb(limb, attach: p.attach(for: limb), upper: nf.ball(for: limb),
|
||
lower: nf.lower(for: limb), ankle: nf.ankle(for: limb), prof: prof, parent: parent)
|
||
}
|
||
return p
|
||
}
|
||
|
||
// MARK: Inverse kinematics
|
||
|
||
/// 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)
|
||
|
||
// 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 base = attach + dirTarget.scaled(along)
|
||
|
||
let signs: (Double, Double)
|
||
if limb.isArm {
|
||
// 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. 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)
|
||
}
|
||
|
||
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
|
||
/// lateral tip; below this magnitude the limb is effectively in-plane and its
|
||
/// rotation is left at 0 (see `invertLimb`).
|
||
private static let rotMinLateral = 0.08
|
||
|
||
/// 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.
|
||
private static func invertLimb(_ limb: FigureLimb, attach: Vec3, mid: Vec3, end: Vec3, parent: Mat3) -> (BallJoint, Hinge) {
|
||
let sigma = limb.sigma
|
||
let parentT = parent.transposed
|
||
let u = parentT.apply(mid - attach).normalized
|
||
let abduction = asin(clampUnit(sigma * u.z)) * 180 / .pi
|
||
let flexion = atan2(u.x, -u.y) * 180 / .pi
|
||
let peel = chain(Mat3.rotZ(flexion), Mat3.rotX(-sigma * abduction)).transposed
|
||
let w = peel.apply(parentT.apply(end - mid)).normalized
|
||
let bend = acos(clampUnit(-w.y)) * 180 / .pi
|
||
// Axial rotation is observable only through the lower bone's lateral tip
|
||
// (w.z); a near-sagittal limb carries no recoverable rotation. `bend`
|
||
// alone is too weak a guard: a limb can straighten through this degeneracy
|
||
// while still visibly bent, and atan2 then snaps to ±180 on the sign of a
|
||
// near-zero anterior component — twisting the limb a half-turn and
|
||
// flipping a pinned hand/foot backward.
|
||
let twist = bend > 0.5 && abs(w.z) > Self.rotMinLateral
|
||
let rotation: Double
|
||
if limb.isArm {
|
||
rotation = twist ? sigma * atan2(w.z, w.x) * 180 / .pi : 0
|
||
} else {
|
||
rotation = twist ? sigma * atan2(-w.z, -w.x) * 180 / .pi : 0
|
||
}
|
||
return (BallJoint(flexion: flexion, abduction: abduction, rotation: rotation), Hinge(flexion: bend))
|
||
}
|
||
|
||
private static func viewFromCanvas(_ pt: CGPoint, anchor: CGPoint, depth: Double) -> Vec3 {
|
||
Vec3(pt.x - anchor.x, anchor.y - pt.y, depth)
|
||
}
|
||
|
||
/// Pose a frame and apply pins: for each pinned limb, solve IK against the canvas
|
||
/// target (at the limb's FK depth), write the solved angles back, and re-pose.
|
||
private static func resolve(_ nf: NormalizedFrame, prof: SkeletonProfile, cam: Double) -> (NormalizedFrame, FigurePose) {
|
||
var frame = nf
|
||
var p = pose(frame, prof: prof, cam: cam)
|
||
let anchor = frame.rootPos
|
||
var solved = false
|
||
for limb in FigureLimb.allCases {
|
||
guard let pin = frame.pins[limb.pinKey], let chainPts = p.limbs[limb] else { continue }
|
||
let attach = chainPts[0]
|
||
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
|
||
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
|
||
}
|
||
if solved { p = pose(frame, prof: prof, cam: cam) }
|
||
return (frame, p)
|
||
}
|
||
|
||
// MARK: Frame geometry
|
||
|
||
/// Python's `round(x)` uses banker's rounding; match it so depth buckets agree.
|
||
private static func bucket(_ depth: Double) -> Int { Int((depth / depthBucket).rounded(.toNearestOrEven)) }
|
||
private static func chainDepth(_ pts: [Vec3]) -> Double { pts.reduce(0) { $0 + $1.z } / Double(pts.count) }
|
||
|
||
/// Resolve a normalized frame into drawable geometry. Returns the frame with
|
||
/// IK-resolved angles (for tweening) plus the canvas geometry: near/far shading
|
||
/// (the near pair member draws dark and in front; canvas-right wins depth-bucket
|
||
/// ties in face-on views), the readability offset applied to the far member of
|
||
/// each pair and subtracted from a far pin's target before IK (authored pins
|
||
/// restored afterward), the depth-sorted draw order (head last), the spine curve,
|
||
/// and the foreshortened gaze nose tick (hidden when the face points at/away from
|
||
/// the camera).
|
||
/// The standard slightly-elevated viewpoint: the camera pitches down a touch so
|
||
/// the floor reads as a plane. Kept 1:1 with the reference renderer's CAMERA_PITCH.
|
||
static let defaultPitch: Double = 10
|
||
/// Exercise-mat half-depth (into the screen) and the ground line's canvas y,
|
||
/// shared with the reference renderer.
|
||
static let floorHalfDepth: Double = 30
|
||
static let groundY: Double = 152
|
||
|
||
static func frameGeometry(_ nf: NormalizedFrame, prof: SkeletonProfile, cam: Double,
|
||
pitch: Double = MotionSolver.defaultPitch,
|
||
mat: (lo: Double, hi: Double)? = nil) -> (NormalizedFrame, FigureGeometry) {
|
||
let p0 = pose(nf, prof: prof, cam: cam, camPitch: pitch)
|
||
var shade: [FigureLimb: Shade] = [:]
|
||
for (right, left) in pairs {
|
||
guard let rp = p0.limbs[right], let lp = p0.limbs[left] else { continue }
|
||
let dr = chainDepth(rp), dl = chainDepth(lp)
|
||
let near: FigureLimb
|
||
if bucket(dr) == bucket(dl) {
|
||
near = rp[0].x >= lp[0].x ? right : left // view x == canvas offset from anchor
|
||
} else {
|
||
near = dr > dl ? right : left
|
||
}
|
||
shade[right] = near == right ? .near : .far
|
||
shade[left] = near == left ? .near : .far
|
||
}
|
||
|
||
let fo = prof.farOffset
|
||
let off = CGPoint(x: (fo.first ?? 6) * p0.k, y: (fo.count > 1 ? fo[1] : 2) * p0.k)
|
||
var work = nf
|
||
for limb in FigureLimb.allCases where shade[limb] == .far {
|
||
if let pin = work.pins[limb.pinKey] {
|
||
work.pins[limb.pinKey] = CGPoint(x: pin.x - off.x, y: pin.y - off.y)
|
||
}
|
||
}
|
||
// Pins are canvas targets in the authored, unpitched view: solve IK flat,
|
||
// then tilt the *posed* body - the camera elevation is pure presentation,
|
||
// so contacts straddle the floor plane instead of pins going out of reach.
|
||
var (resolved, p) = resolve(work, prof: prof, cam: cam)
|
||
resolved.pins = nf.pins // keep authored pins; only angles resolved
|
||
if pitch != 0 {
|
||
p = pose(resolved, prof: prof, cam: cam, camPitch: pitch)
|
||
}
|
||
|
||
let anchor = nf.rootPos
|
||
func scr(_ v: Vec3, _ limbOffset: CGPoint = .zero) -> CGPoint {
|
||
CGPoint(x: anchor.x + v.x + limbOffset.x, y: anchor.y - v.y + limbOffset.y)
|
||
}
|
||
let pelvis = scr(p.pelvis), mid = scr(p.mid), neckB = scr(p.neckB)
|
||
let control = CGPoint(x: 2 * mid.x - (pelvis.x + neckB.x) / 2, y: 2 * mid.y - (pelvis.y + neckB.y) / 2)
|
||
|
||
var limbs: [FigureLimb: [CGPoint]] = [:]
|
||
var depths: [String: Double] = ["spine": chainDepth([p.pelvis, p.mid, p.neckB])]
|
||
for limb in FigureLimb.allCases {
|
||
guard let pts = p.limbs[limb] else { continue }
|
||
let limbOffset = shade[limb] == .far ? off : .zero
|
||
limbs[limb] = pts.map { scr($0, limbOffset) }
|
||
depths[limb.rawValue] = chainDepth(pts)
|
||
}
|
||
|
||
// Per-vertex depth shading: tone each drawn joint by its own camera depth
|
||
// relative to its pair's central plane, normalized by the profile half-width
|
||
// (× shadeSpanFrac), so the ink flows along a limb that reaches toward or away
|
||
// from the camera instead of the bone snapping to one flat tone. The per-vertex
|
||
// average reproduces the old flat per-limb tone. Kept 1:1 with render.py.
|
||
var nearness: [FigureLimb: [Double]] = [:]
|
||
for (right, left) in pairs {
|
||
guard let rp = p.limbs[right], let lp = p.limbs[left] else { continue }
|
||
let ref = (chainDepth(rp) + chainDepth(lp)) / 2
|
||
let half = (right == .armR ? prof.shoulderHalf : prof.hipHalf) * p.k
|
||
let span = half * shadeSpanFrac
|
||
func tone(_ z: Double) -> Double {
|
||
span != 0 ? 0.5 + 0.5 * max(-1, min(1, (z - ref) / span)) : 0.5
|
||
}
|
||
nearness[right] = rp.map { tone($0.z) }
|
||
nearness[left] = lp.map { tone($0.z) }
|
||
}
|
||
|
||
let head = scr(p.head)
|
||
var noseStart: CGPoint?, noseEnd: CGPoint?
|
||
let nose = p.noseDir
|
||
let mag = (nose.x * nose.x + nose.y * nose.y).squareRoot()
|
||
if mag > 0.3 {
|
||
let ux = nose.x / mag, uy = -nose.y / mag, r = prof.headR
|
||
noseStart = CGPoint(x: head.x + ux * r, y: head.y + uy * r)
|
||
noseEnd = CGPoint(x: head.x + ux * (r + 7 * mag), y: head.y + uy * (r + 7 * mag))
|
||
}
|
||
|
||
let order = depths.keys.sorted { lhs, rhs in
|
||
let bl = bucket(depths[lhs]!), br = bucket(depths[rhs]!)
|
||
if bl != br { return bl < br }
|
||
return (fixedRank[lhs] ?? 0) < (fixedRank[rhs] ?? 0)
|
||
} + ["head"]
|
||
|
||
func attachPoint(_ v: Vec3, _ limb: FigureLimb) -> CGPoint {
|
||
scr(v, shade[limb] == .far ? off : .zero)
|
||
}
|
||
// The exercise mat: a ground-plane quad rotating with the camera about the
|
||
// figure's vertical axis (`mat` = footprint bounds in the authored view).
|
||
var floor: [CGPoint]?
|
||
if let mat {
|
||
let yg = -(groundY + 4 - anchor.y)
|
||
let rc = Mat3.rotX(pitch).times(Mat3.rotY(-cam))
|
||
floor = [(mat.lo, floorHalfDepth), (mat.hi, floorHalfDepth),
|
||
(mat.hi, -floorHalfDepth), (mat.lo, -floorHalfDepth)].map { dx, dz in
|
||
scr(rc.apply(Vec3(dx - anchor.x, yg, dz)))
|
||
}
|
||
}
|
||
let geo = FigureGeometry(
|
||
headCenter: head, headRadius: prof.headR, noseStart: noseStart, noseEnd: noseEnd,
|
||
spineStart: pelvis, spineControl: control, spineEnd: neckB,
|
||
girdle: [attachPoint(p.shoulderL, .armL), neckB, attachPoint(p.shoulderR, .armR)],
|
||
pelvisBar: [attachPoint(p.hipL, .legL), pelvis, attachPoint(p.hipR, .legR)],
|
||
floor: floor, limbs: limbs, order: order, shade: shade, nearness: nearness)
|
||
return (resolved, geo)
|
||
}
|
||
|
||
// MARK: Props
|
||
|
||
/// Prop joint refs → (limb, chain index): extremities are index 2, mid joints
|
||
/// (elbows/knees) index 1, so equipment can ride either joint. Kept 1:1 with
|
||
/// the reference renderer's `JOINT_LIMB` (legs end in a foot bone, so a foot
|
||
/// prop rides the ankle at index 2).
|
||
private static let propJoints: [String: (limb: FigureLimb, index: Int)] = [
|
||
"hand_r": (.armR, 2), "elbow_r": (.armR, 1),
|
||
"hand_l": (.armL, 2), "elbow_l": (.armL, 1),
|
||
"foot_r": (.legR, 2), "knee_r": (.legR, 1),
|
||
"foot_l": (.legL, 2), "knee_l": (.legL, 1),
|
||
]
|
||
|
||
/// The joint point a prop follows (plus the unit direction of the bone ending
|
||
/// at the first joint, for perpendicular items). Nil when any referenced limb
|
||
/// isn't drawn this frame.
|
||
private static func jointAnchor(_ geo: FigureGeometry, _ ref: PropJointRef) -> (point: CGPoint, direction: CGVector)? {
|
||
var points: [CGPoint] = []
|
||
var direction: CGVector?
|
||
for name in ref.names {
|
||
guard let joint = propJoints[name],
|
||
let chain = geo.limbs[joint.limb], chain.count > joint.index else { return nil }
|
||
let a = chain[joint.index - 1]
|
||
let b = chain[joint.index]
|
||
points.append(b)
|
||
if direction == nil {
|
||
let d = max(hypot(b.x - a.x, b.y - a.y), 1)
|
||
direction = CGVector(dx: (b.x - a.x) / d, dy: (b.y - a.y) / d)
|
||
}
|
||
}
|
||
guard let direction, !points.isEmpty else { return nil }
|
||
let center = CGPoint(x: points.reduce(0) { $0 + $1.x } / CGFloat(points.count),
|
||
y: points.reduce(0) { $0 + $1.y } / CGFloat(points.count))
|
||
return (center, direction)
|
||
}
|
||
|
||
/// View-space rotation carrying authored-view prop geometry to the camera
|
||
/// `yawOffset` degrees past the authored yaw: a rotation about the world-vertical
|
||
/// axis through the root anchor, conjugated by the camera elevation. Nil (the
|
||
/// identity) at offset 0, so the authored view stays bit-exact.
|
||
static func propRotation(pitch: Double, yawOffset: Double) -> Mat3? {
|
||
guard yawOffset != 0 else { return nil }
|
||
return Mat3.rotX(pitch).times(Mat3.rotY(-yawOffset)).times(Mat3.rotX(-pitch))
|
||
}
|
||
|
||
/// Props → drawable primitives for one frame: (background, foreground).
|
||
///
|
||
/// `geo` is the frame's drawn (possibly orbit-rotated) geometry, `authored` the
|
||
/// same frame at the authored camera (pass `geo` itself when not orbiting),
|
||
/// `anchor` the frame's root canvas anchor, and `rotation` the `propRotation`
|
||
/// between them. Joint positions come from `geo`; everything authored — scene
|
||
/// points, cable anchors, bar angles, pad perpendiculars, roller offsets —
|
||
/// resolves against `authored` and rotates. `pitch` is the camera elevation,
|
||
/// needed by `axis` props whose world-space direction projects through it
|
||
/// (like the floor quad). Kept 1:1 with the reference renderer's
|
||
/// `resolve_props` — change them in lockstep.
|
||
static func resolveProps(_ props: [MotionProp], geo: FigureGeometry, authored: FigureGeometry,
|
||
anchor: CGPoint, rotation: Mat3?,
|
||
pitch: Double = MotionSolver.defaultPitch) -> (bg: [PropPrimitive], fg: [PropPrimitive]) {
|
||
/// Authored canvas point (+ depth toward the camera) → drawn canvas.
|
||
func place(_ x: Double, _ y: Double, _ z: Double) -> CGPoint {
|
||
guard let rotation else { return CGPoint(x: x, y: y) }
|
||
let v = rotation.apply(Vec3(x - anchor.x, anchor.y - y, z))
|
||
return CGPoint(x: anchor.x + v.x, y: anchor.y - v.y)
|
||
}
|
||
/// Authored canvas-space direction/offset → drawn canvas (y-down).
|
||
func swing(_ vec: CGVector) -> CGVector {
|
||
guard let rotation else { return vec }
|
||
let v = rotation.apply(Vec3(vec.dx, -vec.dy, 0))
|
||
return CGVector(dx: v.x, dy: -v.y)
|
||
}
|
||
|
||
var bg: [PropPrimitive] = []
|
||
var fg: [PropPrimitive] = []
|
||
for prop in props {
|
||
switch prop.type {
|
||
case "scene":
|
||
for shape in prop.shapes ?? [] {
|
||
let ink = PropInk(shape.color)
|
||
let z = shape.z ?? 0
|
||
switch shape.kind {
|
||
case "line":
|
||
guard let pts = shape.pts else { continue }
|
||
let lifted = pts.compactMap { pt -> (x: Double, y: Double, z: Double)? in
|
||
pt.count >= 2 ? (pt[0], pt[1], z + (pt.count > 2 ? pt[2] : 0)) : nil
|
||
}
|
||
if let depth = shape.depth, depth != 0 {
|
||
// An extruded slab: the polyline swept through ±depth,
|
||
// filled and outlined so it degenerates to the plain
|
||
// line whenever the sweep collapses edge-on.
|
||
let quad = lifted.map { place($0.x, $0.y, $0.z + depth) }
|
||
+ lifted.reversed().map { place($0.x, $0.y, $0.z - depth) }
|
||
bg.append(.poly(points: quad, width: shape.w ?? 4, ink: ink))
|
||
} else {
|
||
bg.append(.line(points: lifted.map { place($0.x, $0.y, $0.z) },
|
||
width: shape.w ?? 4, ink: ink))
|
||
}
|
||
case "circle":
|
||
guard let c = shape.c, c.count == 2, let r = shape.r else { continue }
|
||
bg.append(.circle(center: place(c[0], c[1], z), radius: r,
|
||
width: shape.w ?? 3, fill: shape.fill ?? false, ink: ink))
|
||
default:
|
||
break
|
||
}
|
||
}
|
||
case "cable":
|
||
guard let from = prop.from, from.count >= 2, let to = prop.to,
|
||
let end = jointAnchor(geo, to) else { continue }
|
||
let start = place(from[0], from[1], from.count > 2 ? from[2] : 0)
|
||
bg.append(.line(points: [start, end.point], width: prop.w ?? 2, ink: .equipment))
|
||
case "roller":
|
||
// A machine roller pad seen end-on: a disc riding the limb's lower
|
||
// bone near the joint, on the `side` (+1/−1) of the bone it presses.
|
||
// The offset resolves along the authored-view bone, then rotates.
|
||
guard let at = prop.at, let now = jointAnchor(geo, at),
|
||
let auth = jointAnchor(authored, at) else { continue }
|
||
let r = prop.r ?? 5
|
||
let back = prop.back ?? 0
|
||
let side = prop.side ?? 1
|
||
let d = auth.direction
|
||
let px = d.dy * side, py = -d.dx * side
|
||
let off = swing(CGVector(dx: -d.dx * back + px * (r + 3),
|
||
dy: -d.dy * back + py * (r + 3)))
|
||
fg.append(.circle(center: CGPoint(x: now.point.x + off.dx, y: now.point.y + off.dy),
|
||
radius: r, width: 3, fill: true, ink: .prop))
|
||
case "bar", "dumbbell", "pad":
|
||
let defaults: (halfLen: Double, width: Double, plateR: Double) = switch prop.type {
|
||
case "bar": (24, 4, 0)
|
||
case "dumbbell": (7, 3, 4.5)
|
||
default: (8, 7, 0)
|
||
}
|
||
guard let at = prop.at, let now = jointAnchor(geo, at),
|
||
let auth = jointAnchor(authored, at) else { continue }
|
||
let u: CGVector
|
||
if prop.axis == "z" {
|
||
// A cross-body rod (barbell, pull-up bar): its true 3D axis
|
||
// is the world left-right axis, projected through the camera
|
||
// elevation like the floor quad — a small vertical sliver
|
||
// end-on in a profile view (the plates read nearly
|
||
// concentric, seen from slightly above), full span face-on,
|
||
// swinging with the figure in between, symmetric at 0 and 180.
|
||
let pr = pitch * .pi / 180
|
||
let axis = Vec3(0, -sin(pr), cos(pr))
|
||
let v = rotation?.apply(axis) ?? axis
|
||
u = CGVector(dx: v.x, dy: -v.y)
|
||
} else if prop.type == "bar" || prop.angle != nil {
|
||
u = swing(direction(prop.angle ?? 0)) // fixed authored-view angle, foreshortens under orbit
|
||
} else {
|
||
u = swing(CGVector(dx: -auth.direction.dy, dy: auth.direction.dx))
|
||
}
|
||
let h = prop.halfLen ?? defaults.halfLen
|
||
let a = CGPoint(x: now.point.x - u.dx * h, y: now.point.y - u.dy * h)
|
||
let b = CGPoint(x: now.point.x + u.dx * h, y: now.point.y + u.dy * h)
|
||
fg.append(.line(points: [a, b], width: prop.w ?? defaults.width, ink: .prop))
|
||
let plateR = prop.plateR ?? defaults.plateR
|
||
if plateR > 0 {
|
||
for end in [a, b] {
|
||
fg.append(.circle(center: end, radius: plateR, width: 3, fill: true, ink: .prop))
|
||
}
|
||
}
|
||
default:
|
||
break
|
||
}
|
||
}
|
||
return (bg, fg)
|
||
}
|
||
}
|
||
|
||
/// The full looping animation for one motion: key frames resolved to anatomical angles,
|
||
/// then continuous-time sampling — hold at each key frame, then an eased anatomical-space
|
||
/// tween to the next; the last frame tweens back to the first.
|
||
struct MotionTimeline {
|
||
let resolved: [NormalizedFrame]
|
||
let profile: SkeletonProfile
|
||
let cam: Double
|
||
let pitch: Double
|
||
let duration: Double
|
||
/// The motion's footprint in the authored view (min/max screen x of every figure
|
||
/// point across the key frames, padded) — the exercise mat spans it.
|
||
let mat: (lo: Double, hi: Double)
|
||
|
||
init?(motion: ExerciseMotion, profile: SkeletonProfile) {
|
||
let cam = motion.camera?.yaw ?? 0
|
||
let pitch = motion.camera?.pitch ?? MotionSolver.defaultPitch
|
||
let norms = motion.frames.map { MotionSolver.normalize($0) }
|
||
guard !norms.isEmpty else { return nil }
|
||
var resolved: [NormalizedFrame] = []
|
||
var lo = Double.infinity, hi = -Double.infinity
|
||
for norm in norms {
|
||
let (frame, geo) = MotionSolver.frameGeometry(norm, prof: profile, cam: cam, pitch: pitch)
|
||
resolved.append(frame)
|
||
var xs: [Double] = [Double(geo.headCenter.x) - geo.headRadius,
|
||
Double(geo.headCenter.x) + geo.headRadius,
|
||
Double(geo.spineStart.x), Double(geo.spineControl.x),
|
||
Double(geo.spineEnd.x)]
|
||
xs += (geo.girdle + geo.pelvisBar).map { Double($0.x) }
|
||
xs += geo.limbs.values.flatMap { $0.map { Double($0.x) } }
|
||
lo = min(lo, xs.min() ?? lo)
|
||
hi = max(hi, xs.max() ?? hi)
|
||
}
|
||
let duration = resolved.reduce(0) { $0 + $1.hold + $1.tween }
|
||
guard duration > 0 else { return nil }
|
||
self.resolved = resolved
|
||
self.profile = profile
|
||
self.cam = cam
|
||
self.pitch = pitch
|
||
self.duration = duration
|
||
self.mat = (lo - 12, hi + 12)
|
||
}
|
||
|
||
/// The resolved frame (or eased tween) at wall-clock `time`, looping every
|
||
/// `duration` seconds.
|
||
func frame(at time: Double) -> NormalizedFrame {
|
||
var t = time.truncatingRemainder(dividingBy: duration)
|
||
if t < 0 { t += duration }
|
||
for (i, f) in resolved.enumerated() {
|
||
if t < f.hold { return f }
|
||
t -= f.hold
|
||
if t < f.tween {
|
||
let next = resolved[(i + 1) % resolved.count]
|
||
return MotionSolver.lerpFrames(f, next, MotionSolver.ease(t / f.tween))
|
||
}
|
||
t -= f.tween
|
||
}
|
||
return resolved[0]
|
||
}
|
||
|
||
/// The drawable geometry at wall-clock `time`. `yawOffset` turns the camera past
|
||
/// the exercise's authored yaw — the slow-orbit presentation. Pins are canvas
|
||
/// targets in the *authored* view, so the pose resolves there first and the posed
|
||
/// body is then rotated; re-pinning in a rotated view would glue hands to screen
|
||
/// points that no longer correspond to anything (arms visibly "stuck" mid-orbit).
|
||
/// `props` are resolved into the geometry's primitive layers the same way: joint
|
||
/// anchors follow the rotated figure, while authored constructs (scene points,
|
||
/// cable anchors, bar angles, pad perpendiculars) resolve in the authored view
|
||
/// and rotate about the root anchor's vertical axis.
|
||
func geometry(at time: Double, yawOffset: Double = 0, props: [MotionProp] = []) -> FigureGeometry {
|
||
let frame = frame(at: time)
|
||
var geo: FigureGeometry
|
||
let authored: FigureGeometry
|
||
if yawOffset == 0 {
|
||
geo = MotionSolver.frameGeometry(frame, prof: profile, cam: cam, pitch: pitch, mat: mat).1
|
||
authored = geo
|
||
} else {
|
||
let (posedFrame, authoredGeo) = MotionSolver.frameGeometry(frame, prof: profile, cam: cam, pitch: pitch)
|
||
authored = authoredGeo
|
||
var posed = posedFrame
|
||
posed.pins = [:]
|
||
geo = MotionSolver.frameGeometry(posed, prof: profile, cam: cam + yawOffset, pitch: pitch, mat: mat).1
|
||
}
|
||
guard !props.isEmpty else { return geo }
|
||
let rotation = MotionSolver.propRotation(pitch: pitch, yawOffset: yawOffset)
|
||
(geo.propsBackground, geo.propsForeground) = MotionSolver.resolveProps(
|
||
props, geo: geo, authored: authored, anchor: frame.rootPos, rotation: rotation,
|
||
pitch: pitch)
|
||
return geo
|
||
}
|
||
}
|