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