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

euclid/
rigid.rs
