euclid/
rigid.rs

1use approxeq::ApproxEq;
2use num_traits::Float;
3use trig::Trig;
4use {TypedRotation3D, TypedTransform3D, TypedVector3D, UnknownUnit};
5
6/// A rigid transformation. All lengths are preserved under such a transformation.
7///
8///
9/// Internally, this is a rotation and a translation, with the rotation
10/// applied first (i.e. `Rotation * Translation`, in row-vector notation)
11///
12/// This can be more efficient to use over full matrices, especially if you
13/// have to deal with the decomposed quantities often.
14#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
15#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
16#[repr(C)]
17pub struct TypedRigidTransform3D<T, Src, Dst> {
18    pub rotation: TypedRotation3D<T, Src, Dst>,
19    pub translation: TypedVector3D<T, Dst>,
20}
21
22pub type RigidTransform3D<T> = TypedRigidTransform3D<T, UnknownUnit, UnknownUnit>;
23
24// All matrix multiplication in this file is in row-vector notation,
25// i.e. a vector `v` is transformed with `v * T`, and if you want to apply `T1`
26// before `T2` you use `T1 * T2`
27
28impl<T: Float + ApproxEq<T>, Src, Dst> TypedRigidTransform3D<T, Src, Dst> {
29    /// Construct a new rigid transformation, where the `rotation` applies first
30    #[inline]
31    pub fn new(rotation: TypedRotation3D<T, Src, Dst>, translation: TypedVector3D<T, Dst>) -> Self {
32        Self {
33            rotation,
34            translation,
35        }
36    }
37
38    /// Construct an identity transform
39    #[inline]
40    pub fn identity() -> Self {
41        Self {
42            rotation: TypedRotation3D::identity(),
43            translation: TypedVector3D::zero(),
44        }
45    }
46
47    /// Construct a new rigid transformation, where the `translation` applies first
48    #[inline]
49    pub fn new_from_reversed(
50        translation: TypedVector3D<T, Src>,
51        rotation: TypedRotation3D<T, Src, Dst>,
52    ) -> Self {
53        // T * R
54        //   = (R * R^-1) * T * R
55        //   = R * (R^-1 * T * R)
56        //   = R * T'
57        //
58        // T' = (R^-1 * T * R) is also a translation matrix
59        // It is equivalent to the translation matrix obtained by rotating the
60        // translation by R
61
62        let translation = rotation.rotate_vector3d(&translation);
63        Self {
64            rotation,
65            translation,
66        }
67    }
68
69    #[inline]
70    pub fn from_rotation(rotation: TypedRotation3D<T, Src, Dst>) -> Self {
71        Self {
72            rotation,
73            translation: TypedVector3D::zero(),
74        }
75    }
76
77    #[inline]
78    pub fn from_translation(translation: TypedVector3D<T, Dst>) -> Self {
79        Self {
80            translation,
81            rotation: TypedRotation3D::identity(),
82        }
83    }
84
85    /// Decompose this into a translation and an rotation to be applied in the opposite order
86    ///
87    /// i.e., the translation is applied _first_
88    #[inline]
89    pub fn decompose_reversed(&self) -> (TypedVector3D<T, Src>, TypedRotation3D<T, Src, Dst>) {
90        // self = R * T
91        //      = R * T * (R^-1 * R)
92        //      = (R * T * R^-1) * R)
93        //      = T' * R
94        //
95        // T' = (R^ * T * R^-1) is T rotated by R^-1
96
97        let translation = self.rotation.inverse().rotate_vector3d(&self.translation);
98        (translation, self.rotation)
99    }
100
101    /// Returns the multiplication of the two transforms such that
102    /// other's transformation applies after self's transformation.
103    ///
104    /// i.e., this produces `self * other` in row-vector notation
105    #[inline]
106    pub fn post_mul<Dst2>(
107        &self,
108        other: &TypedRigidTransform3D<T, Dst, Dst2>,
109    ) -> TypedRigidTransform3D<T, Src, Dst2> {
110        // self = R1 * T1
111        // other = R2 * T2
112        // result = R1 * T1 * R2 * T2
113        //        = R1 * (R2 * R2^-1) * T1 * R2 * T2
114        //        = (R1 * R2) * (R2^-1 * T1 * R2) * T2
115        //        = R' * T' * T2
116        //        = R' * T''
117        //
118        // (R2^-1 * T2 * R2^) = T' = T2 rotated by R2
119        // R1 * R2  = R'
120        // T' * T2 = T'' = vector addition of translations T2 and T'
121
122        let t_prime = other
123            .rotation
124            .rotate_vector3d(&self.translation);
125        let r_prime = self.rotation.post_rotate(&other.rotation);
126        let t_prime2 = t_prime + other.translation;
127        TypedRigidTransform3D {
128            rotation: r_prime,
129            translation: t_prime2,
130        }
131    }
132
133    /// Returns the multiplication of the two transforms such that
134    /// self's transformation applies after other's transformation.
135    ///
136    /// i.e., this produces `other * self` in row-vector notation
137    #[inline]
138    pub fn pre_mul<Src2>(
139        &self,
140        other: &TypedRigidTransform3D<T, Src2, Src>,
141    ) -> TypedRigidTransform3D<T, Src2, Dst> {
142        other.post_mul(&self)
143    }
144
145    /// Inverts the transformation
146    #[inline]
147    pub fn inverse(&self) -> TypedRigidTransform3D<T, Dst, Src> {
148        // result = (self)^-1
149        //        = (R * T)^-1
150        //        = T^-1 * R^-1
151        //        = (R^-1 * R) * T^-1 * R^-1
152        //        = R^-1 * (R * T^-1 * R^-1)
153        //        = R' * T'
154        //
155        // T' = (R * T^-1 * R^-1) = (-T) rotated by R^-1
156        // R' = R^-1
157        //
158        // An easier way of writing this is to use new_from_reversed() with R^-1 and T^-1
159
160        TypedRigidTransform3D::new_from_reversed(
161            -self.translation,
162            self.rotation.inverse(),
163        )
164    }
165
166    pub fn to_transform(&self) -> TypedTransform3D<T, Src, Dst>
167    where
168        T: Trig,
169    {
170        self.translation
171            .to_transform()
172            .pre_mul(&self.rotation.to_transform())
173    }
174}
175
176impl<T: Float + ApproxEq<T>, Src, Dst> From<TypedRotation3D<T, Src, Dst>>
177    for TypedRigidTransform3D<T, Src, Dst>
178{
179    fn from(rot: TypedRotation3D<T, Src, Dst>) -> Self {
180        Self::from_rotation(rot)
181    }
182}
183
184impl<T: Float + ApproxEq<T>, Src, Dst> From<TypedVector3D<T, Dst>>
185    for TypedRigidTransform3D<T, Src, Dst>
186{
187    fn from(t: TypedVector3D<T, Dst>) -> Self {
188        Self::from_translation(t)
189    }
190}
191
192#[cfg(test)]
193mod test {
194    use super::RigidTransform3D;
195    use {Rotation3D, TypedTransform3D, Vector3D};
196
197    #[test]
198    fn test_rigid_construction() {
199        let translation = Vector3D::new(12.1, 17.8, -5.5);
200        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
201
202        let rigid = RigidTransform3D::new(rotation, translation);
203        assert!(rigid
204            .to_transform()
205            .approx_eq(&translation.to_transform().pre_mul(&rotation.to_transform())));
206
207        let rigid = RigidTransform3D::new_from_reversed(translation, rotation);
208        assert!(rigid.to_transform().approx_eq(
209            &translation
210                .to_transform()
211                .post_mul(&rotation.to_transform())
212        ));
213    }
214
215    #[test]
216    fn test_rigid_decomposition() {
217        let translation = Vector3D::new(12.1, 17.8, -5.5);
218        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
219
220        let rigid = RigidTransform3D::new(rotation, translation);
221        let (t2, r2) = rigid.decompose_reversed();
222        assert!(rigid
223            .to_transform()
224            .approx_eq(&t2.to_transform().post_mul(&r2.to_transform())));
225    }
226
227    #[test]
228    fn test_rigid_inverse() {
229        let translation = Vector3D::new(12.1, 17.8, -5.5);
230        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
231
232        let rigid = RigidTransform3D::new(rotation, translation);
233        let inverse = rigid.inverse();
234        assert!(rigid
235            .post_mul(&inverse)
236            .to_transform()
237            .approx_eq(&TypedTransform3D::identity()));
238        assert!(inverse
239            .to_transform()
240            .approx_eq(&rigid.to_transform().inverse().unwrap()));
241    }
242
243    #[test]
244    fn test_rigid_multiply() {
245        let translation = Vector3D::new(12.1, 17.8, -5.5);
246        let rotation = Rotation3D::unit_quaternion(0.5, -7.8, 2.2, 4.3);
247        let translation2 = Vector3D::new(9.3, -3.9, 1.1);
248        let rotation2 = Rotation3D::unit_quaternion(0.1, 0.2, 0.3, -0.4);
249        let rigid = RigidTransform3D::new(rotation, translation);
250        let rigid2 = RigidTransform3D::new(rotation2, translation2);
251
252        assert!(rigid
253            .post_mul(&rigid2)
254            .to_transform()
255            .approx_eq(&rigid.to_transform().post_mul(&rigid2.to_transform())));
256        assert!(rigid
257            .pre_mul(&rigid2)
258            .to_transform()
259            .approx_eq(&rigid.to_transform().pre_mul(&rigid2.to_transform())));
260    }
261}
euclid/rigid.rs

euclid/
rigid.rs
