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

euclid/
rigid.rs
