symtropy_math/
transform.rs

1// Copyright (C) 2024-2026 Tristan Stoltz / Luminous Dynamics
2// SPDX-License-Identifier: AGPL-3.0-or-later
3// Commercial licensing: see COMMERCIAL_LICENSE.md at repository root
4use crate::point::Point;
5use crate::rotor::Rotor;
6use nalgebra::SVector;
7
8/// N-dimensional rigid body transform: rotation + translation.
9/// Applied as: p' = R(p) + t
10#[derive(Clone, Debug)]
11pub struct Transform<const D: usize> {
12    pub translation: Point<D>,
13    pub rotation: Rotor<D>,
14}
15
16impl<const D: usize> Transform<D> {
17    pub fn identity() -> Self {
18        Self {
19            translation: Point::origin(),
20            rotation: Rotor::identity(),
21        }
22    }
23
24    pub fn from_translation(t: Point<D>) -> Self {
25        Self {
26            translation: t,
27            rotation: Rotor::identity(),
28        }
29    }
30
31    pub fn from_rotation(r: Rotor<D>) -> Self {
32        Self {
33            translation: Point::origin(),
34            rotation: r,
35        }
36    }
37
38    #[inline]
39    pub fn transform_point(&self, point: &Point<D>) -> Point<D> {
40        let rotated = self.rotation.rotate_point(point);
41        Point(rotated.0 + self.translation.0)
42    }
43
44    #[inline]
45    pub fn transform_vector(&self, v: &SVector<f64, D>) -> SVector<f64, D> {
46        self.rotation.rotate_vector(v)
47    }
48
49    pub fn compose(&self, other: &Self) -> Self {
50        let rotation = self.rotation.compose(&other.rotation);
51        let t = self.rotation.rotate_vector(&other.translation.0);
52        Self {
53            translation: Point(self.translation.0 + t),
54            rotation,
55        }
56    }
57
58    pub fn inverse(&self) -> Self {
59        let inv_rot = self.rotation.reverse();
60        let neg_t = inv_rot.rotate_vector(&self.translation.0) * -1.0;
61        Self {
62            translation: Point(neg_t),
63            rotation: inv_rot,
64        }
65    }
66
67    pub fn interpolate(&self, other: &Self, t: f64) -> Self {
68        let translation = self.translation.lerp(&other.translation, t);
69        let relative = other.rotation.compose(&self.rotation.reverse());
70        let rotation = relative.slerp(t).compose(&self.rotation);
71        Self {
72            translation,
73            rotation,
74        }
75    }
76}
77
78impl<const D: usize> Default for Transform<D> {
79    fn default() -> Self {
80        Self::identity()
81    }
82}
83
84#[cfg(test)]
85mod tests {
86    use super::*;
87    use crate::bivector::Bivector;
88    use std::f64::consts::FRAC_PI_2;
89
90    fn approx_vec<const N: usize>(a: &SVector<f64, N>, b: &SVector<f64, N>) -> bool {
91        a.iter().zip(b.iter()).all(|(x, y)| (x - y).abs() < 1e-10)
92    }
93
94    #[test]
95    fn identity_preserves() {
96        let t = Transform::<3>::identity();
97        let p = Point::new([1.0, 2.0, 3.0]);
98        assert!(approx_vec(&p.0, &t.transform_point(&p).0));
99    }
100
101    #[test]
102    fn pure_translation() {
103        let t = Transform::from_translation(Point::new([10.0, 20.0]));
104        let p = Point::new([1.0, 2.0]);
105        let q = t.transform_point(&p);
106        assert!((q.coord(0) - 11.0).abs() < 1e-10);
107        assert!((q.coord(1) - 22.0).abs() < 1e-10);
108    }
109
110    #[test]
111    fn inverse_roundtrip() {
112        let plane = Bivector::<4>::unit_plane(0, 2);
113        let t = Transform {
114            translation: Point::new([1.0, 2.0, 3.0, 4.0]),
115            rotation: Rotor::from_plane_angle(&plane, 1.0),
116        };
117        let p = Point::new([5.0, 6.0, 7.0, 8.0]);
118        let back = t.inverse().transform_point(&t.transform_point(&p));
119        assert!(approx_vec(&p.0, &back.0));
120    }
121
122    #[test]
123    fn compose_equals_sequential() {
124        let t1 = Transform {
125            translation: Point::new([1.0, 0.0, 0.0]),
126            rotation: Rotor::from_plane_angle(&Bivector::<3>::unit_plane(0, 1), 0.5),
127        };
128        let t2 = Transform {
129            translation: Point::new([0.0, 2.0, 0.0]),
130            rotation: Rotor::from_plane_angle(&Bivector::<3>::unit_plane(1, 2), 0.3),
131        };
132        let p = Point::new([1.0, 1.0, 1.0]);
133        let seq = t2.transform_point(&t1.transform_point(&p));
134        let composed = t2.compose(&t1).transform_point(&p);
135        assert!(approx_vec(&seq.0, &composed.0));
136    }
137
138    #[test]
139    fn rotate_then_translate() {
140        let t = Transform {
141            translation: Point::new([5.0, 0.0, 0.0]),
142            rotation: Rotor::from_plane_angle(&Bivector::<3>::unit_plane(0, 1), FRAC_PI_2),
143        };
144        let p = Point::new([1.0, 0.0, 0.0]);
145        let q = t.transform_point(&p);
146        assert!((q.coord(0) - 5.0).abs() < 1e-10);
147        assert!((q.coord(1) - 1.0).abs() < 1e-10);
148    }
149}
symtropy_math/transform.rs

symtropy_math/
transform.rs
