rzen
81186c51b1
Scene shapes, cable anchors, bar angles, pad perpendiculars, and roller offsets all resolve in the authored view exactly as before, then rotate about the world-vertical axis through the root anchor - the same resolve-then-rotate pattern as the figure's pins and the mat - so at the authored yaw every exercise renders bit-identically to today, and under an orbiting camera the equipment turns with the figure while staying welded to its hands and feet. Scene lines gain an optional depth plane (z) and slab extrusion (depth) so seats, backrests, and platforms keep form edge-on; the rect shape is retired (re-authored as slab lines). All 14 machines' props re-authored with depths and verified at eight orbit angles. The fixture snapshots move into the pipeline as render.py --fixtures and now cover orbit-presentation samples with resolved prop primitives for a spread of prop flavors; the in-app renderer resolves props in MotionSolver (lockstep with resolve_props) and the view just draws primitives. Claude-Session: https://claude.ai/code/session_01HJDQQDA9QdP8zByg43H5v3
771 lines
39 KiB
Swift
771 lines
39 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]
|
||
/// 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)]
|
||
/// 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.
|
||
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)) }
|
||
}
|
||
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()
|
||
var best: (distance: Double, mid: Vec3)?
|
||
for sign in [1.0, -1.0] {
|
||
let mid = attach + (dirTarget.scaled(along) + perp.scaled(sign * h))
|
||
let distance = (mid - guessMid).length
|
||
if best == nil || distance < best!.distance { best = (distance, mid) }
|
||
}
|
||
let mid = best!.mid
|
||
let end = mid + (target - mid).normalized.scaled(b)
|
||
return invertLimb(limb, attach: attach, mid: mid, end: end, parent: parent)
|
||
}
|
||
|
||
/// 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
|
||
let rotation: Double
|
||
if limb.isArm {
|
||
rotation = bend > 0.5 ? sigma * atan2(w.z, w.x) * 180 / .pi : 0
|
||
} else {
|
||
rotation = bend > 0.5 ? 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
|
||
let (upper, lower) = solveLimb(limb, attach: attach, target: target, guessMid: chainPts[1], lengths: lengths, parent: parent)
|
||
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)
|
||
}
|
||
|
||
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)
|
||
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. 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?) -> (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 axis: CGVector
|
||
if prop.type == "bar" || prop.angle != nil {
|
||
axis = direction(prop.angle ?? 0) // fixed authored-view angle
|
||
} else {
|
||
axis = CGVector(dx: -auth.direction.dy, dy: auth.direction.dx)
|
||
}
|
||
let u = swing(axis) // foreshortens under orbit
|
||
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)
|
||
return geo
|
||
}
|
||
}
|