rzen
3c7a790e9d
The library's planar world-angle rig becomes a genuine 3D anatomical model: skeleton.json holds bone-length profiles (real shoulder/pelvis widths, feet, neutral/female/male) and per-joint ROM; motions pose joints with anatomical angles (flexion/abduction/rotation from neutral standing) under a per-exercise orthographic camera, resolved by kinematics.py (3D FK, analytic two-bone IK with anatomical write-back) and validated against physiological ranges. All 20 sagittal motions were migrated by planar decomposition with 0.00 px golden parity against the old renderer — relabeled to true anatomy, since shading is now near-dark/far-light by camera depth rather than by limb suffix — and the face-on machines are re-authored honestly: Abductor/Adductor with real hip abduction (the foreshortened "frontal" profile is retired) and Rotary with genuine spine axial rotation. Figures gain articulated feet; profiles swap without touching a single motion script; --orbit sweeps the camera 360° while a motion loops. The in-app SwiftUI renderer (iOS + watch) is ported to the same model and consumes the exported motions verbatim; figure-fixtures.json pins its geometry to the Python pipeline within 0.5 px across every exercise, key frame, tween, and orbit sample. Also makes the watch bridge logger nonisolated for the newer SDK's stricter isolation checking. Claude-Session: https://claude.ai/code/session_01LEoff8bXGBS83tK1c55Mf7
507 lines
24 KiB
Swift
507 lines
24 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 }
|
||
|
||
/// 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
|
||
/// 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]
|
||
}
|
||
|
||
/// 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.
|
||
static func pose(_ nf: NormalizedFrame, prof: SkeletonProfile, cam: Double) -> FigurePose {
|
||
let fRoot = chain(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).
|
||
static func frameGeometry(_ nf: NormalizedFrame, prof: SkeletonProfile, cam: Double) -> (NormalizedFrame, FigureGeometry) {
|
||
let p0 = pose(nf, prof: prof, cam: cam)
|
||
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)
|
||
}
|
||
}
|
||
var (resolved, p) = resolve(work, prof: prof, cam: cam)
|
||
resolved.pins = nf.pins // keep authored pins; only angles resolved
|
||
|
||
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"]
|
||
|
||
let geo = FigureGeometry(
|
||
headCenter: head, headRadius: prof.headR, noseStart: noseStart, noseEnd: noseEnd,
|
||
spineStart: pelvis, spineControl: control, spineEnd: neckB,
|
||
limbs: limbs, order: order, shade: shade)
|
||
return (resolved, geo)
|
||
}
|
||
}
|
||
|
||
/// 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 duration: Double
|
||
|
||
init?(motion: ExerciseMotion, profile: SkeletonProfile) {
|
||
let cam = motion.camera?.yaw ?? 0
|
||
let norms = motion.frames.map { MotionSolver.normalize($0) }
|
||
guard !norms.isEmpty else { return nil }
|
||
let resolved = norms.map { MotionSolver.frameGeometry($0, prof: profile, cam: cam).0 }
|
||
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.duration = duration
|
||
}
|
||
|
||
/// 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`.
|
||
func geometry(at time: Double) -> FigureGeometry {
|
||
MotionSolver.frameGeometry(frame(at: time), prof: profile, cam: cam).1
|
||
}
|
||
}
|