Skip to main content

vibe/animation/
pga.rs

1//! 3D Projective Geometric Algebra (PGA 𝒢_{3,0,1}) / Dual Quaternion Motor Algebra.
2//!
3//! Represents rigid 3D spatial transformations (rotation + translation) as motors M = R + εD.
4//! Enables constant-speed Screw Linear Interpolation (ScLERP) along Riemannian geodesics
5//! in SE(3) without singularity, gimbal lock, or heap allocation.
6
7/// A 3D PGA Motor (even multivector / unit dual quaternion).
8/// Real part R = (r_w, r_x, r_y, r_z), Dual part D = (d_w, d_x, d_y, d_z).
9#[derive(Debug, Clone, Copy, PartialEq)]
10#[repr(C)]
11pub struct Motor {
12    pub r_w: f64,
13    pub r_x: f64,
14    pub r_y: f64,
15    pub r_z: f64,
16    pub d_w: f64,
17    pub d_x: f64,
18    pub d_y: f64,
19    pub d_z: f64,
20}
21
22/// Lie algebra se(3) screw bivector (rotation axis/angle + translation along screw axis).
23#[derive(Debug, Clone, Copy, PartialEq)]
24pub struct MotorBivector {
25    pub rx: f64,
26    pub ry: f64,
27    pub rz: f64,
28    pub dx: f64,
29    pub dy: f64,
30    pub dz: f64,
31}
32
33impl Motor {
34    /// Identity motor (no rotation, zero translation).
35    pub const fn identity() -> Self {
36        Self {
37            r_w: 1.0,
38            r_x: 0.0,
39            r_y: 0.0,
40            r_z: 0.0,
41            d_w: 0.0,
42            d_x: 0.0,
43            d_y: 0.0,
44            d_z: 0.0,
45        }
46    }
47
48    /// Construct a motor from a rotation quaternion [w, x, y, z] and translation [x, y, z].
49    pub fn from_rotation_translation(rot: [f64; 4], trans: [f64; 3]) -> Self {
50        let (rw, rx, ry, rz) = (rot[0], rot[1], rot[2], rot[3]);
51        let (tx, ty, tz) = (trans[0], trans[1], trans[2]);
52
53        // Dual part D = 0.5 * T * R
54        let dw = -0.5 * (tx * rx + ty * ry + tz * rz);
55        let dx = 0.5 * (tx * rw + ty * rz - tz * ry);
56        let dy = 0.5 * (-tx * rz + ty * rw + tz * rx);
57        let dz = 0.5 * (tx * ry - ty * rx + tz * rw);
58
59        Self {
60            r_w: rw,
61            r_x: rx,
62            r_y: ry,
63            r_z: rz,
64            d_w: dw,
65            d_x: dx,
66            d_y: dy,
67            d_z: dz,
68        }
69        .normalize()
70    }
71
72    /// Construct a pure translation motor.
73    pub fn from_translation(tx: f64, ty: f64, tz: f64) -> Self {
74        Self::from_rotation_translation([1.0, 0.0, 0.0, 0.0], [tx, ty, tz])
75    }
76    /// Converts to rotation-translation.
77    pub fn to_rotation_translation(&self) -> ([f64; 4], [f64; 3]) {
78        let rot = [self.r_w, self.r_x, self.r_y, self.r_z];
79        // Translation T = 2 * D * R^(-1)
80        let tx = 2.0
81            * (self.d_x * self.r_w - self.d_w * self.r_x + self.d_z * self.r_y
82                - self.d_y * self.r_z);
83        let ty = 2.0
84            * (self.d_y * self.r_w - self.d_z * self.r_x - self.d_w * self.r_y
85                + self.d_x * self.r_z);
86        let tz = 2.0
87            * (self.d_z * self.r_w + self.d_y * self.r_x
88                - self.d_x * self.r_y
89                - self.d_w * self.r_z);
90        let trans = [tx, ty, tz];
91        (rot, trans)
92    }
93
94    /// Geometric product (motor multiplication: M_a * M_b).
95    pub fn mul(&self, other: &Self) -> Self {
96        // Real part product (R1 * R2)
97        let rw = self.r_w * other.r_w
98            - self.r_x * other.r_x
99            - self.r_y * other.r_y
100            - self.r_z * other.r_z;
101        let rx = self.r_w * other.r_x + self.r_x * other.r_w + self.r_y * other.r_z
102            - self.r_z * other.r_y;
103        let ry = self.r_w * other.r_y - self.r_x * other.r_z
104            + self.r_y * other.r_w
105            + self.r_z * other.r_x;
106        let rz = self.r_w * other.r_z + self.r_x * other.r_y - self.r_y * other.r_x
107            + self.r_z * other.r_w;
108
109        // Dual part product (R1 * D2 + D1 * R2)
110        let dw = self.r_w * other.d_w
111            - self.r_x * other.d_x
112            - self.r_y * other.d_y
113            - self.r_z * other.d_z
114            + self.d_w * other.r_w
115            - self.d_x * other.r_x
116            - self.d_y * other.r_y
117            - self.d_z * other.r_z;
118
119        let dx = self.r_w * other.d_x + self.r_x * other.d_w + self.r_y * other.d_z
120            - self.r_z * other.d_y
121            + self.d_w * other.r_x
122            + self.d_x * other.r_w
123            + self.d_y * other.r_z
124            - self.d_z * other.r_y;
125
126        let dy = self.r_w * other.d_y - self.r_x * other.d_z
127            + self.r_y * other.d_w
128            + self.r_z * other.d_x
129            + self.d_w * other.r_y
130            - self.d_x * other.r_z
131            + self.d_y * other.r_w
132            + self.d_z * other.r_x;
133
134        let dz = self.r_w * other.d_z + self.r_x * other.d_y - self.r_y * other.d_x
135            + self.r_z * other.d_w
136            + self.d_w * other.r_z
137            + self.d_x * other.r_y
138            - self.d_y * other.r_x
139            + self.d_z * other.r_w;
140
141        Self {
142            r_w: rw,
143            r_x: rx,
144            r_y: ry,
145            r_z: rz,
146            d_w: dw,
147            d_x: dx,
148            d_y: dy,
149            d_z: dz,
150        }
151    }
152
153    /// Conjugate of motor (R*, D*).
154    pub fn conjugate(&self) -> Self {
155        Self {
156            r_w: self.r_w,
157            r_x: -self.r_x,
158            r_y: -self.r_y,
159            r_z: -self.r_z,
160            d_w: self.d_w,
161            d_x: -self.d_x,
162            d_y: -self.d_y,
163            d_z: -self.d_z,
164        }
165    }
166
167    /// Normalize motor so that |R| = 1 and R · D = 0.
168    pub fn normalize(&self) -> Self {
169        let mag_r_sq =
170            self.r_w * self.r_w + self.r_x * self.r_x + self.r_y * self.r_y + self.r_z * self.r_z;
171        if mag_r_sq < 1e-12 {
172            return Self::identity();
173        }
174        let inv_mag_r = 1.0 / mag_r_sq.sqrt();
175        let dot_rd =
176            self.r_w * self.d_w + self.r_x * self.d_x + self.r_y * self.d_y + self.r_z * self.d_z;
177
178        let rw = self.r_w * inv_mag_r;
179        let rx = self.r_x * inv_mag_r;
180        let ry = self.r_y * inv_mag_r;
181        let rz = self.r_z * inv_mag_r;
182
183        let dw = (self.d_w - rw * dot_rd) * inv_mag_r;
184        let dx = (self.d_x - rx * dot_rd) * inv_mag_r;
185        let dy = (self.d_y - ry * dot_rd) * inv_mag_r;
186        let dz = (self.d_z - rz * dot_rd) * inv_mag_r;
187
188        Self {
189            r_w: rw,
190            r_x: rx,
191            r_y: ry,
192            r_z: rz,
193            d_w: dw,
194            d_x: dx,
195            d_y: dy,
196            d_z: dz,
197        }
198    }
199
200    /// Logarithmic map: log(M) ∈ se(3).
201    pub fn log(&self) -> MotorBivector {
202        let m = if self.r_w < 0.0 {
203            Self {
204                r_w: -self.r_w,
205                r_x: -self.r_x,
206                r_y: -self.r_y,
207                r_z: -self.r_z,
208                d_w: -self.d_w,
209                d_x: -self.d_x,
210                d_y: -self.d_y,
211                d_z: -self.d_z,
212            }
213        } else {
214            *self
215        };
216
217        let sin_sq = m.r_x * m.r_x + m.r_y * m.r_y + m.r_z * m.r_z;
218        if sin_sq < 1e-12 {
219            // Pure translation (rotation angle ≈ 0)
220            return MotorBivector {
221                rx: 0.0,
222                ry: 0.0,
223                rz: 0.0,
224                dx: m.d_x,
225                dy: m.d_y,
226                dz: m.d_z,
227            };
228        }
229
230        let sin_theta = sin_sq.sqrt();
231        let theta = sin_theta.atan2(m.r_w);
232        let inv_sin = 1.0 / sin_theta;
233
234        let axis_x = m.r_x * inv_sin;
235        let axis_y = m.r_y * inv_sin;
236        let axis_z = m.r_z * inv_sin;
237
238        let pitch = -m.d_w * inv_sin;
239
240        let rx = theta * axis_x;
241        let ry = theta * axis_y;
242        let rz = theta * axis_z;
243
244        let dx = m.d_x - pitch * m.r_x;
245        let dy = m.d_y - pitch * m.r_y;
246        let dz = m.d_z - pitch * m.r_z;
247
248        MotorBivector {
249            rx,
250            ry,
251            rz,
252            dx,
253            dy,
254            dz,
255        }
256    }
257
258    /// Screw Linear Interpolation (ScLERP): Geodesic shortest-path SE(3) interpolation.
259    /// Returns M(t) = M0 * (M0^(-1) * M1)^t.
260    pub fn sclerp(m0: &Self, m1: &Self, t: f64) -> Self {
261        if t <= 0.0 {
262            return *m0;
263        }
264        if t >= 1.0 {
265            return *m1;
266        }
267
268        // Relative transform: delta = M0^(-1) * M1
269        let m0_inv = m0.conjugate();
270        let mut delta = m0_inv.mul(m1);
271
272        // Ensure shortest path in SO(3)
273        if delta.r_w < 0.0 {
274            delta = Self {
275                r_w: -delta.r_w,
276                r_x: -delta.r_x,
277                r_y: -delta.r_y,
278                r_z: -delta.r_z,
279                d_w: -delta.d_w,
280                d_x: -delta.d_x,
281                d_y: -delta.d_y,
282                d_z: -delta.d_z,
283            };
284        }
285
286        let bivector = delta.log();
287        let scaled_bivector = MotorBivector {
288            rx: bivector.rx * t,
289            ry: bivector.ry * t,
290            rz: bivector.rz * t,
291            dx: bivector.dx * t,
292            dy: bivector.dy * t,
293            dz: bivector.dz * t,
294        };
295
296        let delta_t = scaled_bivector.exp();
297        m0.mul(&delta_t).normalize()
298    }
299
300    /// Transform a 3D point using the motor: p' = M * p * M^~.
301    pub fn transform_point(&self, p: [f64; 3]) -> [f64; 3] {
302        let (rot, trans) = self.to_rotation_translation();
303        let (rw, rx, ry, rz) = (rot[0], rot[1], rot[2], rot[3]);
304        let (px, py, pz) = (p[0], p[1], p[2]);
305
306        // Rotate point: q * p * q*
307        let ix = rw * px + ry * pz - rz * py;
308        let iy = rw * py - rx * pz + rz * px;
309        let iz = rw * pz + rx * py - ry * px;
310        let iw = -rx * px - ry * py - rz * pz;
311
312        let rx_out = ix * rw + iw * -rx + iy * -rz - iz * -ry;
313        let ry_out = iy * rw + iw * -ry + iz * -rx - ix * -rz;
314        let rz_out = iz * rw + iw * -rz + ix * -ry - iy * -rx;
315
316        [rx_out + trans[0], ry_out + trans[1], rz_out + trans[2]]
317    }
318}
319
320impl MotorBivector {
321    /// Exponential map: exp(B) ∈ SE(3).
322    pub fn exp(&self) -> Motor {
323        let theta_sq = self.rx * self.rx + self.ry * self.ry + self.rz * self.rz;
324        if theta_sq < 1e-12 {
325            // Pure translation limit
326            return Motor {
327                r_w: 1.0,
328                r_x: 0.0,
329                r_y: 0.0,
330                r_z: 0.0,
331                d_w: 0.0,
332                d_x: self.dx,
333                d_y: self.dy,
334                d_z: self.dz,
335            };
336        }
337
338        let theta = theta_sq.sqrt();
339        let sin_theta = theta.sin();
340        let cos_theta = theta.cos();
341        let inv_theta = 1.0 / theta;
342
343        let rw = cos_theta;
344        let rx = self.rx * inv_theta * sin_theta;
345        let ry = self.ry * inv_theta * sin_theta;
346        let rz = self.rz * inv_theta * sin_theta;
347
348        let dw = 0.0;
349        let dx = self.dx;
350        let dy = self.dy;
351        let dz = self.dz;
352
353        Motor {
354            r_w: rw,
355            r_x: rx,
356            r_y: ry,
357            r_z: rz,
358            d_w: dw,
359            d_x: dx,
360            d_y: dy,
361            d_z: dz,
362        }
363        .normalize()
364    }
365}
366
367#[cfg(test)]
368mod tests {
369    use super::*;
370
371    #[test]
372    fn motor_identity_transforms_point_invariant() {
373        let m = Motor::identity();
374        let p = [10.0, -5.0, 3.5];
375        let p_out = m.transform_point(p);
376        for i in 0..3 {
377            assert!((p[i] - p_out[i]).abs() < 1e-6);
378        }
379    }
380
381    #[test]
382    fn motor_pure_translation() {
383        let m = Motor::from_rotation_translation([1.0, 0.0, 0.0, 0.0], [5.0, 10.0, -2.0]);
384        let p = [1.0, 2.0, 3.0];
385        let p_out = m.transform_point(p);
386        assert!((p_out[0] - 6.0).abs() < 1e-5);
387        assert!((p_out[1] - 12.0).abs() < 1e-5);
388        assert!((p_out[2] - 1.0).abs() < 1e-5);
389    }
390
391    #[test]
392    fn sclerp_halfway_interpolation() {
393        let m0 = Motor::identity();
394        let m1 = Motor::from_rotation_translation([1.0, 0.0, 0.0, 0.0], [10.0, 20.0, 30.0]);
395        let mid = Motor::sclerp(&m0, &m1, 0.5);
396
397        let (_, trans) = mid.to_rotation_translation();
398        assert!((trans[0] - 5.0).abs() < 1e-4);
399        assert!((trans[1] - 10.0).abs() < 1e-4);
400        assert!((trans[2] - 15.0).abs() < 1e-4);
401    }
402}