glam_ext/f64_ext/
isometry3a.rs

1use core::ops::{Mul, MulAssign};
2use glam::{DAffine3, DMat3, DMat4, DQuat, DVec3};
3
4#[repr(C)]
5#[derive(Debug, Default, Clone, Copy, PartialEq)]
6#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
7#[cfg_attr(feature = "rkyv", derive(rkyv::Archive, rkyv::Serialize, rkyv::Deserialize))]
8pub struct DIsometry3 {
9    pub translation: DVec3,
10    pub rotation: DQuat,
11}
12
13impl DIsometry3 {
14    /// The degenerate zero isometry.
15    ///
16    /// This isometrys any finite vector and point to zero.
17    /// The zero isometry is non-invertible.
18    pub const ZERO: Self = Self {
19        translation: DVec3::ZERO,
20        rotation: DQuat::IDENTITY,
21    };
22
23    /// The identity isometry.
24    ///
25    /// Multiplying a vector with this returns the same vector.
26    pub const IDENTITY: Self = Self {
27        translation: DVec3::ZERO,
28        rotation: DQuat::IDENTITY,
29    };
30
31    /// All NAN:s.
32    pub const NAN: Self = Self {
33        translation: DVec3::NAN,
34        rotation: DQuat::NAN,
35    };
36
37    /// Creates a new isometry.
38    #[inline]
39    #[must_use]
40    pub fn new(translation: DVec3, rotation: DQuat) -> Self {
41        Self {
42            translation,
43            rotation,
44        }
45    }
46
47    /// Creates a isometry isometry from the given `rotation` quaternion.
48    #[inline]
49    #[must_use]
50    pub fn from_quat(rotation: DQuat) -> Self {
51        Self {
52            translation: DVec3::ZERO,
53            rotation,
54        }
55    }
56
57    /// Creates a isometry isometry containing a 3D rotation around a normalized
58    /// rotation `axis` of `angle` (in radians).
59    #[inline]
60    #[must_use]
61    pub fn from_axis_angle(axis: DVec3, angle: f64) -> Self {
62        Self {
63            translation: DVec3::ZERO,
64            rotation: DQuat::from_axis_angle(axis, angle),
65        }
66    }
67
68    /// Creates a isometry isometry containing a 3D rotation around the x axis of
69    /// `angle` (in radians).
70    #[inline]
71    #[must_use]
72    pub fn from_rotation_x(angle: f64) -> Self {
73        Self {
74            translation: DVec3::ZERO,
75            rotation: DQuat::from_rotation_x(angle),
76        }
77    }
78
79    /// Creates a isometry isometry containing a 3D rotation around the y axis of
80    /// `angle` (in radians).
81    #[inline]
82    #[must_use]
83    pub fn from_rotation_y(angle: f64) -> Self {
84        Self {
85            translation: DVec3::ZERO,
86            rotation: DQuat::from_rotation_y(angle),
87        }
88    }
89
90    /// Creates a isometry isometry containing a 3D rotation around the z axis of
91    /// `angle` (in radians).
92    #[inline]
93    #[must_use]
94    pub fn from_rotation_z(angle: f64) -> Self {
95        Self {
96            translation: DVec3::ZERO,
97            rotation: DQuat::from_rotation_z(angle),
98        }
99    }
100
101    /// Creates a isometry isometryation from the given 3D `translation`.
102    #[inline]
103    #[must_use]
104    pub fn from_translation(translation: DVec3) -> Self {
105        Self {
106            translation,
107            rotation: DQuat::IDENTITY,
108        }
109    }
110
111    /// Creates a isometry from the given 3D `rotation` and `translation`.
112    #[inline]
113    #[must_use]
114    pub fn from_rotation_translation(rotation: DQuat, translation: DVec3) -> Self {
115        Self {
116            translation,
117            rotation,
118        }
119    }
120
121    /// Creates a isometry from a 3x3 matrix (expressing scale and rotation)
122    #[inline]
123    #[must_use]
124    pub fn from_mat3(mat3: DMat3) -> Self {
125        Self {
126            translation: DVec3::ZERO,
127            rotation: DQuat::from_mat3(&mat3),
128        }
129    }
130
131    /// Creates a isometry from a 3x3 matrix (expressing scale and rotation)
132    #[inline]
133    #[must_use]
134    pub fn from_mat3_translation(mat3: DMat3, translation: DVec3) -> Self {
135        Self {
136            translation,
137            rotation: DQuat::from_mat3(&mat3),
138        }
139    }
140
141    /// Creates a isometry from a 4x4 matrix.
142    #[inline]
143    #[must_use]
144    pub fn from_mat4(mat4: DMat4) -> Self {
145        Self {
146            translation: mat4.w_axis.truncate(),
147            rotation: DQuat::from_mat4(&mat4),
148        }
149    }
150
151    /// Extracts `scale`, `rotation` and `translation` from `self`.
152    #[inline]
153    #[must_use]
154    pub fn to_rotation_translation(&self) -> (DQuat, DVec3) {
155        (self.rotation, self.translation)
156    }
157
158    /// Transforms the given 3D points, applying rotation and translation.
159    #[inline]
160    #[must_use]
161    pub fn transform_point3(&self, rhs: DVec3) -> DVec3 {
162        let translation: DVec3 = self.translation;
163        self.rotation * rhs + translation
164    }
165
166    /// Transforms the given 3D vector, applying rotation (but NOT translation).
167    #[inline]
168    #[must_use]
169    pub fn transform_vector3(&self, rhs: DVec3) -> DVec3 {
170        self.rotation * rhs
171    }
172
173    /// Returns `true` if, and only if, all elements are finite.
174    ///
175    /// If any element is either `NaN`, positive or negative infinity, this will return `false`.
176    #[inline]
177    #[must_use]
178    pub fn is_finite(&self) -> bool {
179        self.translation.is_finite() && self.rotation.is_finite()
180    }
181
182    /// Returns `true` if any elements are `NaN`.
183    #[inline]
184    #[must_use]
185    pub fn is_nan(&self) -> bool {
186        self.translation.is_nan() && self.rotation.is_nan()
187    }
188
189    /// Returns true if the absolute difference of all elements between `self` and `rhs`
190    /// is less than or equal to `max_abs_diff`.
191    #[inline]
192    #[must_use]
193    pub fn abs_diff_eq(self, rhs: Self, max_abs_diff: f64) -> bool {
194        self.translation.abs_diff_eq(rhs.translation, max_abs_diff)
195            && self.rotation.abs_diff_eq(rhs.rotation, max_abs_diff)
196    }
197
198    /// Return the inverse of this isometry.
199    ///
200    /// Note that if the isometry is not invertible the result will be invalid.
201    #[inline]
202    #[must_use]
203    pub fn inverse(&self) -> Self {
204        let rotation = self.rotation.inverse();
205        Self {
206            translation: -(rotation * self.translation),
207            rotation,
208        }
209    }
210}
211
212impl From<DIsometry3> for DMat4 {
213    #[inline]
214    fn from(i: DIsometry3) -> DMat4 {
215        let mat3 = DMat3::from_quat(i.rotation);
216        DMat4::from_cols(
217            mat3.x_axis.extend(0.0),
218            mat3.y_axis.extend(0.0),
219            mat3.z_axis.extend(0.0),
220            i.translation.extend(1.0),
221        )
222    }
223}
224
225impl From<DIsometry3> for DAffine3 {
226    #[inline]
227    fn from(i: DIsometry3) -> DAffine3 {
228        DAffine3::from_scale_rotation_translation(DVec3::ONE, i.rotation, i.translation)
229    }
230}
231
232impl Mul for DIsometry3 {
233    type Output = DIsometry3;
234
235    #[inline]
236    fn mul(self, rhs: DIsometry3) -> Self::Output {
237        DIsometry3 {
238            translation: self.rotation * rhs.translation + self.translation,
239            rotation: self.rotation * rhs.rotation,
240        }
241    }
242}
243
244impl MulAssign for DIsometry3 {
245    #[inline]
246    fn mul_assign(&mut self, rhs: DIsometry3) {
247        *self = self.mul(rhs);
248    }
249}
250
251impl Mul<DMat4> for DIsometry3 {
252    type Output = DMat4;
253
254    #[inline]
255    fn mul(self, rhs: DMat4) -> Self::Output {
256        DMat4::from(self) * rhs
257    }
258}
259
260impl Mul<DIsometry3> for DMat4 {
261    type Output = DMat4;
262
263    #[inline]
264    fn mul(self, rhs: DIsometry3) -> Self::Output {
265        self * DMat4::from(rhs)
266    }
267}
268
269#[cfg(test)]
270mod test {
271    use super::*;
272
273    #[test]
274    fn test_from_mat4() {
275        let rot = DQuat::from_rotation_y(0.6);
276        let pos = DVec3::new(1.0, 2.0, 3.0);
277        let mat = DMat4::from_rotation_translation(rot, pos);
278        let is = DIsometry3::from_mat4(mat);
279        assert!(DVec3::abs_diff_eq(is.translation, pos, 1e-6));
280        assert!(DQuat::abs_diff_eq(is.rotation, rot, 1e-6));
281    }
282
283    #[test]
284    fn test_transform_point3() {
285        let rot = DQuat::from_rotation_x(0.6);
286        let pos = DVec3::new(1.0, 2.0, 3.0);
287        let mat = DMat4::from_rotation_translation(rot, pos);
288        let is = DIsometry3::from_mat4(mat);
289
290        let point = DVec3::new(5.0, -5.0, 5.0);
291        let p1 = mat.project_point3(point);
292        let p2 = is.transform_point3(point);
293        assert!(DVec3::abs_diff_eq(p1, p2, 1e-6));
294    }
295
296    #[test]
297    fn test_transform_vec3() {
298        let rot = DQuat::from_rotation_z(-0.5);
299        let pos = DVec3::new(1.5, -2.0, -2.0);
300        let mat = DMat4::from_rotation_translation(rot, pos);
301        let is = DIsometry3::from_mat4(mat);
302
303        let vec = DVec3::new(1.0, 0.0, 0.7);
304        let v1 = mat.transform_vector3(vec);
305        let v2 = is.transform_vector3(vec);
306        assert!(DVec3::abs_diff_eq(v1, v2, 1e-6));
307    }
308
309    #[test]
310    fn test_inverse() {
311        let rot = DQuat::from_rotation_z(1.5) * DQuat::from_rotation_x(1.0);
312        let pos = DVec3::new(1.99, 0.77, -1.55);
313        let mat = DMat4::from_rotation_translation(rot, pos);
314        let mat_inv = mat.inverse();
315        let is1 = DIsometry3::from_mat4(mat).inverse();
316        let is2 = DIsometry3::from_mat4(mat_inv);
317        assert!(DIsometry3::abs_diff_eq(is1, is2, 1e-6));
318    }
319
320    #[test]
321    fn test_mat4_from() {
322        let rot = DQuat::from_rotation_y(-2.0);
323        let pos = DVec3::new(3.0, 3.3, 3.33);
324        let mat = DMat4::from_rotation_translation(rot, pos);
325        let is = DIsometry3::from_mat4(mat);
326        let mat2 = DMat4::from(is);
327        assert!(DMat4::abs_diff_eq(&mat, mat2, 1e-6));
328    }
329
330    #[test]
331    fn test_isometry_mul() {
332        let rot1 = DQuat::from_rotation_x(0.77);
333        let pos1 = DVec3::new(6.6, -6.6, 3.3);
334        let mat1 = DMat4::from_rotation_translation(rot1, pos1);
335        let is1 = DIsometry3::from_rotation_translation(rot1, pos1);
336
337        let rot2 = DQuat::from_rotation_z(-0.44);
338        let pos2 = DVec3::new(-1.1, -2.2, -3.3);
339        let mat2 = DMat4::from_rotation_translation(rot2, pos2);
340        let is2 = DIsometry3::from_rotation_translation(rot2, pos2);
341
342        let mat = mat1 * mat2;
343        let is = is1 * is2;
344        let is_mat = DIsometry3::from_mat4(mat);
345        assert!(DIsometry3::abs_diff_eq(is, is_mat, 1e-6));
346    }
347}
glam_ext/f64_ext/isometry3a.rs

glam_ext/f64_ext/
isometry3a.rs
