qualia_core_db/render/physics/
joint.rs

1//! Kinematic joints as PGA motors animated over time `t` (Phase 2).
2//!
3//! "Joints as kinematic multivectors": a joint's pose at time `t` is a pure function returning a
4//! `render::pga::Motor` (the even-subalgebra multivector: rotation + translation). Identity at
5//! `t = 0`; deterministic (same `t` → same motor). Composing joints is motor multiplication, so a
6//! chain (arm → forearm → hand) is `motor_a ⊗ motor_b ⊗ …` with no heap.
7
8use crate::render::pga::{motor_mul, motor_translate, rotor_from_axis_angle, Motor};
9
10/// The kind of single-DOF joint.
11#[derive(Clone, Copy, Debug, PartialEq)]
12pub enum JointKind {
13    /// Rotation about a unit `axis` through the origin (`rate` in radians per unit time).
14    Revolute { axis: [f32; 3] },
15    /// Translation along a unit `axis` (`rate` in distance per unit time).
16    Prismatic { axis: [f32; 3] },
17}
18
19/// A single-DOF kinematic joint driven by a constant `rate`.
20#[derive(Clone, Copy, Debug)]
21pub struct Joint {
22    pub kind: JointKind,
23    pub rate: f32,
24}
25
26impl Joint {
27    #[inline]
28    pub fn revolute(axis: [f32; 3], rate: f32) -> Self {
29        Joint {
30            kind: JointKind::Revolute { axis },
31            rate,
32        }
33    }
34
35    #[inline]
36    pub fn prismatic(axis: [f32; 3], rate: f32) -> Self {
37        Joint {
38            kind: JointKind::Prismatic { axis },
39            rate,
40        }
41    }
42
43    /// The joint's motor at time `t`. Pure function of `t` (deterministic); identity at `t = 0`.
44    #[inline]
45    pub fn motor_at(&self, t: f32) -> Motor {
46        let q = self.rate * t;
47        match self.kind {
48            JointKind::Revolute { axis } => Motor::from_rotor(rotor_from_axis_angle(axis, q)),
49            JointKind::Prismatic { axis } => {
50                motor_translate([axis[0] * q, axis[1] * q, axis[2] * q])
51            }
52        }
53    }
54}
55
56/// Compose a chain of joint motors at time `t` (root-first): `m0 ⊗ m1 ⊗ … ⊗ mn`. Zero-alloc.
57pub fn chain_motor_at(joints: &[Joint], t: f32) -> Motor {
58    let mut m = Motor::identity();
59    for j in joints {
60        m = motor_mul(m, j.motor_at(t));
61    }
62    m
63}
64
65#[cfg(test)]
66mod tests {
67    use super::*;
68    use crate::render::pga::sandwich_point;
69
70    fn approx(a: [f32; 3], b: [f32; 3]) -> bool {
71        (0..3).all(|k| (a[k] - b[k]).abs() < 1e-5)
72    }
73
74    #[test]
75    fn identity_at_t_zero() {
76        let rev = Joint::revolute([0.0, 1.0, 0.0], 1.5);
77        let p = [0.7, -0.2, 0.4];
78        assert!(approx(sandwich_point(rev.motor_at(0.0), p), p));
79    }
80
81    #[test]
82    fn revolute_rotates_over_t() {
83        // 90° about Y at t·rate = π/2: (1,0,0) → (0,0,-1) in this right-handed convention.
84        let rev = Joint::revolute([0.0, 1.0, 0.0], std::f32::consts::FRAC_PI_2);
85        let out = sandwich_point(rev.motor_at(1.0), [1.0, 0.0, 0.0]);
86        assert!(out[0].abs() < 1e-5, "x≈0, got {out:?}");
87        assert!(out[1].abs() < 1e-5, "y≈0");
88        assert!(out[2].abs() > 0.9, "|z|≈1");
89    }
90
91    #[test]
92    fn prismatic_translates_over_t() {
93        let pri = Joint::prismatic([1.0, 0.0, 0.0], 2.0);
94        let out = sandwich_point(pri.motor_at(0.5), [0.0, 0.0, 0.0]);
95        assert!(approx(out, [1.0, 0.0, 0.0])); // 2.0 * 0.5
96    }
97
98    #[test]
99    fn motor_at_is_deterministic() {
100        let j = Joint::revolute([0.0, 0.0, 1.0], 0.9);
101        let a = sandwich_point(j.motor_at(0.33), [0.5, 0.1, 0.0]);
102        let b = sandwich_point(j.motor_at(0.33), [0.5, 0.1, 0.0]);
103        assert_eq!(a, b);
104    }
105
106    #[test]
107    fn chain_composes_two_joints() {
108        // base prismatic +x by 1, then revolute — the chain motor moves the origin to +1 x at least.
109        let chain = [
110            Joint::prismatic([1.0, 0.0, 0.0], 1.0),
111            Joint::revolute([0.0, 1.0, 0.0], 0.5),
112        ];
113        let out = sandwich_point(chain_motor_at(&chain, 1.0), [0.0, 0.0, 0.0]);
114        assert!(out[0] > 0.5);
115    }
116}
qualia_core_db/render/physics/joint.rs

qualia_core_db/render/physics/
joint.rs
