Rebuild the exercise figure on an anatomical 3D skeleton
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
This commit is contained in:
@@ -8,13 +8,83 @@
|
||||
import CoreGraphics
|
||||
import Foundation
|
||||
|
||||
/// Swift port of the Exercise Library's reference solver (`Exercise Library/render.py`):
|
||||
/// forward kinematics, analytic 2-bone IK, and angle-space tweening with shortest-path
|
||||
/// lerp and ease-in-out. The math is kept 1:1 with the Python so both renderers produce
|
||||
/// the same figure from the same data — change them in lockstep.
|
||||
/// 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.
|
||||
///
|
||||
/// Angles are absolute world angles in degrees, y-up (0 = right, 90 = up, −90 = down);
|
||||
/// points are y-down 320×180 canvas coordinates, so dir(θ) = (cos θ, −sin θ).
|
||||
/// 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 {
|
||||
@@ -23,8 +93,9 @@ enum FigureLimb: String, CaseIterable {
|
||||
case legR = "leg_r"
|
||||
case legL = "leg_l"
|
||||
|
||||
var isLeft: Bool { self == .armL || self == .legL }
|
||||
|
||||
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 {
|
||||
@@ -34,261 +105,402 @@ enum FigureLimb: String, CaseIterable {
|
||||
case .legL: "foot_l"
|
||||
}
|
||||
}
|
||||
|
||||
/// `[upper, lower]` bone lengths for this limb.
|
||||
func boneLengths(_ body: ExerciseBodyProfile) -> [Double] {
|
||||
switch self {
|
||||
case .armR, .armL: [body.upperArm, body.foreArm]
|
||||
case .legR, .legL: [body.thigh, body.shin]
|
||||
}
|
||||
}
|
||||
|
||||
/// The authored `[upper, lower]` angles for this limb, if present in the frame.
|
||||
func angles(in frame: MotionKeyFrame) -> [Double]? {
|
||||
switch self {
|
||||
case .armR: frame.armR
|
||||
case .armL: frame.armL
|
||||
case .legR: frame.legR
|
||||
case .legL: frame.legL
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// A key frame resolved to pure angles (pinned limbs replaced by their IK solution),
|
||||
/// so poses interpolate cleanly in angle space.
|
||||
struct FigurePose {
|
||||
var root: CGPoint
|
||||
var spine: [Double]
|
||||
var neck: Double
|
||||
/// Absent for figures facing the viewer — no nose tick.
|
||||
var gaze: Double?
|
||||
var limbs: [FigureLimb: [Double]]
|
||||
/// 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: Double
|
||||
var tween: Double
|
||||
}
|
||||
var hold, tween: Double
|
||||
|
||||
/// Trunk FK: pelvis, spine mid, neck joint, and the offset left-limb attachments.
|
||||
struct FigureAttachments {
|
||||
let pelvis: CGPoint
|
||||
let mid: CGPoint
|
||||
let neck: CGPoint
|
||||
let shoulderL: CGPoint
|
||||
let hipL: CGPoint
|
||||
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 }
|
||||
|
||||
func point(for limb: FigureLimb) -> CGPoint {
|
||||
switch limb {
|
||||
case .armR: neck
|
||||
case .armL: shoulderL
|
||||
case .legR: pelvis
|
||||
case .legL: hipL
|
||||
}
|
||||
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 }
|
||||
}
|
||||
}
|
||||
|
||||
/// A pose resolved to drawable points.
|
||||
/// 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 pose has no gaze (face-on view).
|
||||
var noseStart: CGPoint?
|
||||
var noseEnd: CGPoint?
|
||||
/// 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: CGPoint
|
||||
var spineControl: CGPoint
|
||||
var spineEnd: CGPoint
|
||||
/// Attachment → elbow/knee → extremity, per drawn limb.
|
||||
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 {
|
||||
/// Unit vector for a y-up angle, in y-down canvas coordinates.
|
||||
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))
|
||||
}
|
||||
|
||||
/// Chain FK: `[start, joint1, joint2, …]` for per-bone absolute angles.
|
||||
static func walk(from start: CGPoint, angles: [Double], lengths: [Double]) -> [CGPoint] {
|
||||
var points = [start]
|
||||
var current = start
|
||||
for (angle, length) in zip(angles, lengths) {
|
||||
let d = direction(angle)
|
||||
current = CGPoint(x: current.x + d.dx * length, y: current.y + d.dy * length)
|
||||
points.append(current)
|
||||
}
|
||||
return points
|
||||
}
|
||||
// MARK: Normalize / tween
|
||||
|
||||
/// Y-up world angle (degrees) of the segment a→b.
|
||||
static func angle(from a: CGPoint, to b: CGPoint) -> Double {
|
||||
atan2(-(b.y - a.y), b.x - a.x) * 180 / .pi
|
||||
}
|
||||
|
||||
/// Analytic 2-bone IK: `[upper, lower]` angles reaching from `start` toward
|
||||
/// `target`, choosing the elbow/knee solution nearest the authored guess.
|
||||
static func ik2(start: CGPoint, target: CGPoint, upper a: Double, lower b: Double, guess: [Double]) -> [Double] {
|
||||
let dx = target.x - start.x
|
||||
let dyUp = -(target.y - start.y)
|
||||
// Clamp the reach inside the chain's annulus so acos stays defined.
|
||||
let d = max(abs(a - b) + 0.5, min(a + b - 0.01, hypot(dx, dyUp)))
|
||||
let base = atan2(dyUp, dx) * 180 / .pi
|
||||
let alpha = acos((a * a + d * d - b * b) / (2 * a * d)) * 180 / .pi
|
||||
let guessElbow = walk(from: start, angles: guess, lengths: [a, b])[1]
|
||||
var best: (distance: Double, angles: [Double])?
|
||||
for sign in [1.0, -1.0] {
|
||||
let upperAngle = base + sign * alpha
|
||||
let elbow = walk(from: start, angles: [upperAngle], lengths: [a])[1]
|
||||
let distance = Double(hypot(elbow.x - guessElbow.x, elbow.y - guessElbow.y))
|
||||
if best == nil || distance < best!.distance {
|
||||
best = (distance, [upperAngle, angle(from: elbow, to: target)])
|
||||
}
|
||||
}
|
||||
return best!.angles
|
||||
}
|
||||
|
||||
static func attachments(root: CGPoint, spine: [Double], body: ExerciseBodyProfile) -> FigureAttachments {
|
||||
let mid = walk(from: root, angles: [spine[0]], lengths: [body.spine1])[1]
|
||||
let neck = walk(from: mid, angles: [spine[1]], lengths: [body.spine2])[1]
|
||||
let ox = body.leftOffset.count > 0 ? body.leftOffset[0] : 6
|
||||
let oy = body.leftOffset.count > 1 ? body.leftOffset[1] : 2
|
||||
return FigureAttachments(
|
||||
pelvis: root, mid: mid, neck: neck,
|
||||
shoulderL: CGPoint(x: neck.x + ox, y: neck.y + oy),
|
||||
hipL: CGPoint(x: root.x + ox, y: root.y + oy)
|
||||
)
|
||||
}
|
||||
|
||||
/// Resolve a key frame to pure angles: replace each pinned limb's authored angles
|
||||
/// with its IK solution (the authored angles only pick the bend direction).
|
||||
static func normalize(_ kf: MotionKeyFrame, body: ExerciseBodyProfile) -> FigurePose {
|
||||
let root = CGPoint(x: kf.root[0], y: kf.root[1])
|
||||
/// 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])
|
||||
}
|
||||
let at = attachments(root: root, spine: kf.spine, body: body)
|
||||
var limbs: [FigureLimb: [Double]] = [:]
|
||||
for limb in FigureLimb.allCases {
|
||||
guard let authored = limb.angles(in: kf) else { continue }
|
||||
if let pin = pins[limb.pinKey] {
|
||||
let lengths = limb.boneLengths(body)
|
||||
limbs[limb] = ik2(start: at.point(for: limb), target: pin,
|
||||
upper: lengths[0], lower: lengths[1], guess: authored)
|
||||
} else {
|
||||
limbs[limb] = authored
|
||||
}
|
||||
}
|
||||
return FigurePose(root: root, spine: kf.spine, neck: kf.neck, gaze: kf.gaze,
|
||||
limbs: limbs, pins: pins,
|
||||
hold: kf.hold ?? 0.5, tween: kf.tween ?? 0.6)
|
||||
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
|
||||
}
|
||||
static func ease(_ t: Double) -> Double { 3 * t * t - 2 * t * t * t }
|
||||
|
||||
/// Shortest-path angular interpolation, so limbs swing in natural arcs.
|
||||
static func lerpAngle(_ a: Double, _ b: Double, _ t: Double) -> Double {
|
||||
var delta = (b - a + 180).truncatingRemainder(dividingBy: 360)
|
||||
if delta < 0 { delta += 360 }
|
||||
return a + (delta - 180) * t
|
||||
}
|
||||
|
||||
/// Interpolate two normalized poses. A pin survives the tween only if planted in
|
||||
/// BOTH neighboring key frames (so planted limbs hold exactly and releasing pins
|
||||
/// release naturally); a limb absent from either side is dropped for the tween.
|
||||
static func lerp(_ a: FigurePose, _ b: FigurePose, _ t: Double) -> FigurePose {
|
||||
var limbs: [FigureLimb: [Double]] = [:]
|
||||
for (limb, va) in a.limbs {
|
||||
guard let vb = b.limbs[limb] else { continue }
|
||||
limbs[limb] = zip(va, vb).map { lerpAngle($0, $1, 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: pa.x + (pb.x - pa.x) * t, y: pa.y + (pb.y - pa.y) * t)
|
||||
pins[key] = CGPoint(x: n(pa.x, pb.x), y: n(pa.y, pb.y))
|
||||
}
|
||||
return FigurePose(
|
||||
root: CGPoint(x: a.root.x + (b.root.x - a.root.x) * t,
|
||||
y: a.root.y + (b.root.y - a.root.y) * t),
|
||||
spine: zip(a.spine, b.spine).map { lerpAngle($0, $1, t) },
|
||||
neck: lerpAngle(a.neck, b.neck, t),
|
||||
gaze: (a.gaze != nil && b.gaze != nil) ? lerpAngle(a.gaze!, b.gaze!, t) : nil,
|
||||
limbs: limbs, pins: pins, hold: a.hold, tween: a.tween
|
||||
)
|
||||
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)
|
||||
}
|
||||
|
||||
/// Normalized pose → drawable points. Limbs with an active pin are re-solved so
|
||||
/// planted hands/feet hold that point exactly through tweens.
|
||||
static func geometry(of pose: FigurePose, body: ExerciseBodyProfile, hide: Set<String>) -> FigureGeometry {
|
||||
let at = attachments(root: pose.root, spine: pose.spine, body: body)
|
||||
let head = walk(from: at.neck, angles: [pose.neck], lengths: [body.neck])[1]
|
||||
let r = body.headR
|
||||
// Nose tick: 7pt outward from the head rim along the gaze direction
|
||||
// (omitted for face-on poses without a gaze).
|
||||
var noseStart: CGPoint?
|
||||
var noseEnd: CGPoint?
|
||||
if let gaze = pose.gaze {
|
||||
let d = direction(gaze)
|
||||
noseStart = CGPoint(x: head.x + d.dx * r, y: head.y + d.dy * r)
|
||||
noseEnd = CGPoint(x: head.x + d.dx * (r + 7), y: head.y + d.dy * (r + 7))
|
||||
// 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]] = [:]
|
||||
for (limb, authored) in pose.limbs where !hide.contains(limb.rawValue) {
|
||||
let lengths = limb.boneLengths(body)
|
||||
var angles = authored
|
||||
if let pin = pose.pins[limb.pinKey] {
|
||||
angles = ik2(start: at.point(for: limb), target: pin,
|
||||
upper: lengths[0], lower: lengths[1], guess: angles)
|
||||
}
|
||||
limbs[limb] = walk(from: at.point(for: limb), angles: angles, lengths: lengths)
|
||||
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)
|
||||
}
|
||||
|
||||
return FigureGeometry(
|
||||
headCenter: head,
|
||||
headRadius: r,
|
||||
noseStart: noseStart,
|
||||
noseEnd: noseEnd,
|
||||
spineStart: at.pelvis,
|
||||
spineControl: CGPoint(x: 2 * at.mid.x - (at.pelvis.x + at.neck.x) / 2,
|
||||
y: 2 * at.mid.y - (at.pelvis.y + at.neck.y) / 2),
|
||||
spineEnd: at.neck,
|
||||
limbs: limbs
|
||||
)
|
||||
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: normalized key poses plus
|
||||
/// continuous-time sampling — hold at each key frame, then an eased angle-space tween
|
||||
/// to the next; the last frame tweens back to the first.
|
||||
/// 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 poses: [FigurePose]
|
||||
let resolved: [NormalizedFrame]
|
||||
let profile: SkeletonProfile
|
||||
let cam: Double
|
||||
let duration: Double
|
||||
|
||||
init?(motion: ExerciseMotion, body: ExerciseBodyProfile) {
|
||||
let poses = motion.frames.map { MotionSolver.normalize($0, body: body) }
|
||||
let duration = poses.reduce(0) { $0 + $1.hold + $1.tween }
|
||||
guard !poses.isEmpty, duration > 0 else { return nil }
|
||||
self.poses = poses
|
||||
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 pose at wall-clock `time`, looping every `duration` seconds.
|
||||
func pose(at time: Double) -> FigurePose {
|
||||
/// 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, pose) in poses.enumerated() {
|
||||
if t < pose.hold { return pose }
|
||||
t -= pose.hold
|
||||
if t < pose.tween {
|
||||
let next = poses[(i + 1) % poses.count]
|
||||
return MotionSolver.lerp(pose, next, MotionSolver.ease(t / pose.tween))
|
||||
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 -= pose.tween
|
||||
t -= f.tween
|
||||
}
|
||||
return poses[0]
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user