euclid/
rigid.rs

1//! All matrix multiplication in this module is in row-vector notation,
2//! i.e. a vector `v` is transformed with `v * T`, and if you want to apply `T1`
3//! before `T2` you use `T1 * T2`
4
5use crate::approxeq::ApproxEq;
6use crate::trig::Trig;
7use crate::{Rotation3D, Transform3D, UnknownUnit, Vector3D};
8
9use core::{fmt, hash};
10
11#[cfg(feature = "bytemuck")]
12use bytemuck::{Pod, Zeroable};
13use num_traits::real::Real;
14#[cfg(feature = "serde")]
15use serde::{Deserialize, Serialize};
16
17/// A rigid transformation. All lengths are preserved under such a transformation.
18///
19///
20/// Internally, this is a rotation and a translation, with the rotation
21/// applied first (i.e. `Rotation * Translation`, in row-vector notation)
22///
23/// This can be more efficient to use over full matrices, especially if you
24/// have to deal with the decomposed quantities often.
25#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
26#[repr(C)]
27pub struct RigidTransform3D<T, Src, Dst> {
28    pub rotation: Rotation3D<T, Src, Dst>,
29    pub translation: Vector3D<T, Dst>,
30}
31
32impl<T, Src, Dst> RigidTransform3D<T, Src, Dst> {
33    /// Construct a new rigid transformation, where the `rotation` applies first
34    #[inline]
35    pub const fn new(rotation: Rotation3D<T, Src, Dst>, translation: Vector3D<T, Dst>) -> Self {
36        Self {
37            rotation,
38            translation,
39        }
40    }
41}
42
43impl<T: Copy, Src, Dst> RigidTransform3D<T, Src, Dst> {
44    pub fn cast_unit<Src2, Dst2>(&self) -> RigidTransform3D<T, Src2, Dst2> {
45        RigidTransform3D {
46            rotation: self.rotation.cast_unit(),
47            translation: self.translation.cast_unit(),
48        }
49    }
50}
51
52impl<T: Real + ApproxEq<T>, Src, Dst> RigidTransform3D<T, Src, Dst> {
53    /// Construct an identity transform
54    #[inline]
55    pub fn identity() -> Self {
56        Self {
57            rotation: Rotation3D::identity(),
58            translation: Vector3D::zero(),
59        }
60    }
61
62    /// Construct a new rigid transformation, where the `translation` applies first
63    #[inline]
64    pub fn new_from_reversed(
65        translation: Vector3D<T, Src>,
66        rotation: Rotation3D<T, Src, Dst>,
67    ) -> Self {
68        // T * R
69        //   = (R * R^-1) * T * R
70        //   = R * (R^-1 * T * R)
71        //   = R * T'
72        //
73        // T' = (R^-1 * T * R) is also a translation matrix
74        // It is equivalent to the translation matrix obtained by rotating the
75        // translation by R
76
77        let translation = rotation.transform_vector3d(translation);
78        Self {
79            rotation,
80            translation,
81        }
82    }
83
84    #[inline]
85    pub fn from_rotation(rotation: Rotation3D<T, Src, Dst>) -> Self {
86        Self {
87            rotation,
88            translation: Vector3D::zero(),
89        }
90    }
91
92    #[inline]
93    pub fn from_translation(translation: Vector3D<T, Dst>) -> Self {
94        Self {
95            translation,
96            rotation: Rotation3D::identity(),
97        }
98    }
99
100    /// Decompose this into a translation and an rotation to be applied in the opposite order
101    ///
102    /// i.e., the translation is applied _first_
103    #[inline]
104    pub fn decompose_reversed(&self) -> (Vector3D<T, Src>, Rotation3D<T, Src, Dst>) {
105        // self = R * T
106        //      = R * T * (R^-1 * R)
107        //      = (R * T * R^-1) * R)
108        //      = T' * R
109        //
110        // T' = (R^ * T * R^-1) is T rotated by R^-1
111
112        let translation = self.rotation.inverse().transform_vector3d(self.translation);
113        (translation, self.rotation)
114    }
115
116    /// Returns the multiplication of the two transforms such that
117    /// other's transformation applies after self's transformation.
118    ///
119    /// i.e., this produces `self * other` in row-vector notation
120    #[inline]
121    pub fn then<Dst2>(
122        &self,
123        other: &RigidTransform3D<T, Dst, Dst2>,
124    ) -> RigidTransform3D<T, Src, Dst2> {
125        // self = R1 * T1
126        // other = R2 * T2
127        // result = R1 * T1 * R2 * T2
128        //        = R1 * (R2 * R2^-1) * T1 * R2 * T2
129        //        = (R1 * R2) * (R2^-1 * T1 * R2) * T2
130        //        = R' * T' * T2
131        //        = R' * T''
132        //
133        // (R2^-1 * T2 * R2^) = T' = T2 rotated by R2
134        // R1 * R2  = R'
135        // T' * T2 = T'' = vector addition of translations T2 and T'
136
137        let t_prime = other.rotation.transform_vector3d(self.translation);
138        let r_prime = self.rotation.then(&other.rotation);
139        let t_prime2 = t_prime + other.translation;
140        RigidTransform3D {
141            rotation: r_prime,
142            translation: t_prime2,
143        }
144    }
145
146    /// Inverts the transformation
147    #[inline]
148    pub fn inverse(&self) -> RigidTransform3D<T, Dst, Src> {
149        // result = (self)^-1
150        //        = (R * T)^-1
151        //        = T^-1 * R^-1
152        //        = (R^-1 * R) * T^-1 * R^-1
153        //        = R^-1 * (R * T^-1 * R^-1)
154        //        = R' * T'
155        //
156        // T' = (R * T^-1 * R^-1) = (-T) rotated by R^-1
157        // R' = R^-1
158        //
159        // An easier way of writing this is to use new_from_reversed() with R^-1 and T^-1
160
161        RigidTransform3D::new_from_reversed(-self.translation, self.rotation.inverse())
162    }
163
164    pub fn to_transform(&self) -> Transform3D<T, Src, Dst>
165    where
166        T: Trig,
167    {
168        self.rotation
169            .to_transform()
170            .then(&self.translation.to_transform())
171    }
172
173    /// Drop the units, preserving only the numeric value.
174    #[inline]
175    pub fn to_untyped(&self) -> RigidTransform3D<T, UnknownUnit, UnknownUnit> {
176        RigidTransform3D {
177            rotation: self.rotation.to_untyped(),
178            translation: self.translation.to_untyped(),
179        }
180    }
181
182    /// Tag a unitless value with units.
183    #[inline]
184    pub fn from_untyped(transform: &RigidTransform3D<T, UnknownUnit, UnknownUnit>) -> Self {
185        RigidTransform3D {
186            rotation: Rotation3D::from_untyped(&transform.rotation),
187            translation: Vector3D::from_untyped(transform.translation),
188        }
189    }
190}
191
192impl<T: fmt::Debug, Src, Dst> fmt::Debug for RigidTransform3D<T, Src, Dst> {
193    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
194        f.debug_struct("RigidTransform3D")
195            .field("rotation", &self.rotation)
196            .field("translation", &self.translation)
197            .finish()
198    }
199}
200
201impl<T: PartialEq, Src, Dst> PartialEq for RigidTransform3D<T, Src, Dst> {
202    fn eq(&self, other: &Self) -> bool {
203        self.rotation == other.rotation && self.translation == other.translation
204    }
205}
206impl<T: Eq, Src, Dst> Eq for RigidTransform3D<T, Src, Dst> {}
207
208impl<T: hash::Hash, Src, Dst> hash::Hash for RigidTransform3D<T, Src, Dst> {
209    fn hash<H: hash::Hasher>(&self, state: &mut H) {
210        self.rotation.hash(state);
211        self.translation.hash(state);
212    }
213}
214
215impl<T: Copy, Src, Dst> Copy for RigidTransform3D<T, Src, Dst> {}
216
217impl<T: Clone, Src, Dst> Clone for RigidTransform3D<T, Src, Dst> {
218    fn clone(&self) -> Self {
219        RigidTransform3D {
220            rotation: self.rotation.clone(),
221            translation: self.translation.clone(),
222        }
223    }
224}
225
226#[cfg(feature = "arbitrary")]
227impl<'a, T, Src, Dst> arbitrary::Arbitrary<'a> for RigidTransform3D<T, Src, Dst>
228where
229    T: arbitrary::Arbitrary<'a>,
230{
231    fn arbitrary(u: &mut arbitrary::Unstructured<'a>) -> arbitrary::Result<Self> {
232        Ok(RigidTransform3D {
233            rotation: arbitrary::Arbitrary::arbitrary(u)?,
234            translation: arbitrary::Arbitrary::arbitrary(u)?,
235        })
236    }
237}
238
239#[cfg(feature = "bytemuck")]
240unsafe impl<T: Zeroable, Src, Dst> Zeroable for RigidTransform3D<T, Src, Dst> {}
241
242#[cfg(feature = "bytemuck")]
243unsafe impl<T: Pod, Src: 'static, Dst: 'static> Pod for RigidTransform3D<T, Src, Dst> {}
244
245impl<T: Real + ApproxEq<T>, Src, Dst> From<Rotation3D<T, Src, Dst>>
246    for RigidTransform3D<T, Src, Dst>
247{
248    fn from(rot: Rotation3D<T, Src, Dst>) -> Self {
249        Self::from_rotation(rot)
250    }
251}
252
253impl<T: Real + ApproxEq<T>, Src, Dst> From<Vector3D<T, Dst>> for RigidTransform3D<T, Src, Dst> {
254    fn from(t: Vector3D<T, Dst>) -> Self {
255        Self::from_translation(t)
256    }
257}
258
259#[cfg(test)]
260mod test {
261    use super::RigidTransform3D;
262    use crate::default::{Rotation3D, Transform3D, Vector3D};
263
264    #[test]
265    fn test_rigid_construction() {
266        let translation = Vector3D::new(12.1, 17.8, -5.5);
267        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
268
269        let rigid = RigidTransform3D::new(rotation, translation);
270        assert!(rigid
271            .to_transform()
272            .approx_eq(&rotation.to_transform().then(&translation.to_transform())));
273
274        let rigid = RigidTransform3D::new_from_reversed(translation, rotation);
275        assert!(rigid
276            .to_transform()
277            .approx_eq(&translation.to_transform().then(&rotation.to_transform())));
278    }
279
280    #[test]
281    fn test_rigid_decomposition() {
282        let translation = Vector3D::new(12.1, 17.8, -5.5);
283        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
284
285        let rigid = RigidTransform3D::new(rotation, translation);
286        let (t2, r2) = rigid.decompose_reversed();
287        assert!(rigid
288            .to_transform()
289            .approx_eq(&t2.to_transform().then(&r2.to_transform())));
290    }
291
292    #[test]
293    fn test_rigid_inverse() {
294        let translation = Vector3D::new(12.1, 17.8, -5.5);
295        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
296
297        let rigid = RigidTransform3D::new(rotation, translation);
298        let inverse = rigid.inverse();
299        assert!(rigid
300            .then(&inverse)
301            .to_transform()
302            .approx_eq(&Transform3D::identity()));
303        assert!(inverse
304            .to_transform()
305            .approx_eq(&rigid.to_transform().inverse().unwrap()));
306    }
307
308    #[test]
309    fn test_rigid_multiply() {
310        let translation = Vector3D::new(12.1, 17.8, -5.5);
311        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
312        let translation2 = Vector3D::new(9.3, -3.9, 1.1);
313        let rotation2 = Rotation3D::unit_quaternion(0.1, 0.2, 0.3, -0.4);
314        let rigid = RigidTransform3D::new(rotation, translation);
315        let rigid2 = RigidTransform3D::new(rotation2, translation2);
316
317        assert!(rigid
318            .then(&rigid2)
319            .to_transform()
320            .approx_eq(&rigid.to_transform().then(&rigid2.to_transform())));
321        assert!(rigid2
322            .then(&rigid)
323            .to_transform()
324            .approx_eq(&rigid2.to_transform().then(&rigid.to_transform())));
325    }
326}
euclid/rigid.rs

euclid/
rigid.rs
