rzen
b82054b81a
The default camera pitches down 10 degrees, so the floor reads as a plane (drawn as a rectangle) and near/far contacts straddle it. Elevation is pure presentation - IK pins solve in the flat authored view and the posed body tilts, the same pattern as the orbit, so authored canvas targets never go out of reach. The leg-extension roller moves up onto the shin above the ankle and the leg-curl roller tucks under the heel. Fixtures and reference test values regenerated for the pitched camera. Claude-Session: https://claude.ai/code/session_01LEoff8bXGBS83tK1c55Mf7
545 lines
27 KiB
Swift
545 lines
27 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
|
||
/// 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]
|
||
/// 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; `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
|
||
/// Floor plane half-depth; its projected height is `floorHalfDepth · sin(pitch)`.
|
||
static let floorHalfDepth: Double = 30
|
||
|
||
static func frameGeometry(_ nf: NormalizedFrame, prof: SkeletonProfile, cam: Double,
|
||
pitch: Double = MotionSolver.defaultPitch) -> (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)
|
||
}
|
||
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)],
|
||
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 pitch: Double
|
||
let duration: 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 }
|
||
let resolved = norms.map { MotionSolver.frameGeometry($0, prof: profile, cam: cam, pitch: pitch).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.pitch = pitch
|
||
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`. `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).
|
||
func geometry(at time: Double, yawOffset: Double = 0) -> FigureGeometry {
|
||
let frame = frame(at: time)
|
||
guard yawOffset != 0 else {
|
||
return MotionSolver.frameGeometry(frame, prof: profile, cam: cam, pitch: pitch).1
|
||
}
|
||
var posed = MotionSolver.frameGeometry(frame, prof: profile, cam: cam, pitch: pitch).0
|
||
posed.pins = [:]
|
||
return MotionSolver.frameGeometry(posed, prof: profile, cam: cam + yawOffset, pitch: pitch).1
|
||
}
|
||
}
|