glam_ext/f32_ext/
isometry3a.rs

1use core::ops::{Mul, MulAssign};
2use glam::{Affine3A, Mat3, Mat4, Quat, Vec3, Vec3A};
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 Isometry3A {
9    pub translation: Vec3A,
10    pub rotation: Quat,
11}
12
13impl Isometry3A {
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: Vec3A::ZERO,
20        rotation: Quat::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: Vec3A::ZERO,
28        rotation: Quat::IDENTITY,
29    };
30
31    /// All NAN:s.
32    pub const NAN: Self = Self {
33        translation: Vec3A::NAN,
34        rotation: Quat::NAN,
35    };
36
37    /// Creates a new isometry.
38    #[inline]
39    #[must_use]
40    pub fn new(translation: Vec3, rotation: Quat) -> Self {
41        Self {
42            translation: translation.into(),
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: Quat) -> Self {
51        Self {
52            translation: Vec3A::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: Vec3, angle: f32) -> Self {
62        Self {
63            translation: Vec3A::ZERO,
64            rotation: Quat::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: f32) -> Self {
73        Self {
74            translation: Vec3A::ZERO,
75            rotation: Quat::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: f32) -> Self {
84        Self {
85            translation: Vec3A::ZERO,
86            rotation: Quat::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: f32) -> Self {
95        Self {
96            translation: Vec3A::ZERO,
97            rotation: Quat::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: Vec3) -> Self {
105        Self {
106            translation: translation.into(),
107            rotation: Quat::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: Quat, translation: Vec3) -> Self {
115        Self {
116            translation: translation.into(),
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: Mat3) -> Self {
125        Self {
126            translation: Vec3A::ZERO,
127            rotation: Quat::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: Mat3, translation: Vec3) -> Self {
135        Self {
136            translation: translation.into(),
137            rotation: Quat::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: Mat4) -> Self {
145        Self {
146            translation: mat4.w_axis.truncate().into(),
147            rotation: Quat::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) -> (Quat, Vec3) {
155        (self.rotation, self.translation.into())
156    }
157
158    /// Transforms the given 3D points, applying rotation and translation.
159    #[inline]
160    #[must_use]
161    pub fn transform_point3(&self, rhs: Vec3) -> Vec3 {
162        let translation: Vec3 = self.translation.into();
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: Vec3) -> Vec3 {
170        self.rotation * rhs
171    }
172
173    /// Transforms the given [`Vec3A`], applying rotation and translation.
174    #[inline]
175    #[must_use]
176    pub fn transform_point3a(&self, rhs: Vec3A) -> Vec3A {
177        self.rotation * rhs + self.translation
178    }
179
180    /// Transforms the given [`Vec3A`], applying rotation (but NOT translation).
181    #[inline]
182    #[must_use]
183    pub fn transform_vector3a(&self, rhs: Vec3A) -> Vec3A {
184        self.rotation * rhs
185    }
186
187    /// Returns `true` if, and only if, all elements are finite.
188    ///
189    /// If any element is either `NaN`, positive or negative infinity, this will return `false`.
190    #[inline]
191    #[must_use]
192    pub fn is_finite(&self) -> bool {
193        self.translation.is_finite() && self.rotation.is_finite()
194    }
195
196    /// Returns `true` if any elements are `NaN`.
197    #[inline]
198    #[must_use]
199    pub fn is_nan(&self) -> bool {
200        self.translation.is_nan() && self.rotation.is_nan()
201    }
202
203    /// Returns true if the absolute difference of all elements between `self` and `rhs`
204    /// is less than or equal to `max_abs_diff`.
205    #[inline]
206    #[must_use]
207    pub fn abs_diff_eq(self, rhs: Self, max_abs_diff: f32) -> bool {
208        self.translation.abs_diff_eq(rhs.translation, max_abs_diff)
209            && self.rotation.abs_diff_eq(rhs.rotation, max_abs_diff)
210    }
211
212    /// Return the inverse of this isometry.
213    ///
214    /// Note that if the isometry is not invertible the result will be invalid.
215    #[inline]
216    #[must_use]
217    pub fn inverse(&self) -> Self {
218        let rotation = self.rotation.inverse();
219        Self {
220            translation: -(rotation * self.translation),
221            rotation,
222        }
223    }
224}
225
226impl From<Isometry3A> for Mat4 {
227    #[inline]
228    fn from(i: Isometry3A) -> Mat4 {
229        let mat3 = Mat3::from_quat(i.rotation);
230        Mat4::from_cols(
231            mat3.x_axis.extend(0.0),
232            mat3.y_axis.extend(0.0),
233            mat3.z_axis.extend(0.0),
234            i.translation.extend(1.0),
235        )
236    }
237}
238
239impl From<Isometry3A> for Affine3A {
240    #[inline]
241    fn from(i: Isometry3A) -> Affine3A {
242        Affine3A::from_scale_rotation_translation(Vec3::ONE, i.rotation, i.translation.into())
243    }
244}
245
246impl Mul for Isometry3A {
247    type Output = Isometry3A;
248
249    #[inline]
250    fn mul(self, rhs: Isometry3A) -> Self::Output {
251        Isometry3A {
252            translation: self.rotation * rhs.translation + self.translation,
253            rotation: self.rotation * rhs.rotation,
254        }
255    }
256}
257
258impl MulAssign for Isometry3A {
259    #[inline]
260    fn mul_assign(&mut self, rhs: Isometry3A) {
261        *self = self.mul(rhs);
262    }
263}
264
265impl Mul<Mat4> for Isometry3A {
266    type Output = Mat4;
267
268    #[inline]
269    fn mul(self, rhs: Mat4) -> Self::Output {
270        Mat4::from(self) * rhs
271    }
272}
273
274impl Mul<Isometry3A> for Mat4 {
275    type Output = Mat4;
276
277    #[inline]
278    fn mul(self, rhs: Isometry3A) -> Self::Output {
279        self * Mat4::from(rhs)
280    }
281}
282
283#[cfg(test)]
284mod test {
285    use super::*;
286
287    #[test]
288    fn test_from_mat4() {
289        let rot = Quat::from_rotation_y(0.6);
290        let pos = Vec3::new(1.0, 2.0, 3.0);
291        let mat = Mat4::from_rotation_translation(rot, pos);
292        let is = Isometry3A::from_mat4(mat);
293        assert!(Vec3::abs_diff_eq(is.translation.into(), pos, 1e-6));
294        assert!(Quat::abs_diff_eq(is.rotation, rot, 1e-6));
295    }
296
297    #[test]
298    fn test_transform_point3() {
299        let rot = Quat::from_rotation_x(0.6);
300        let pos = Vec3::new(1.0, 2.0, 3.0);
301        let mat = Mat4::from_rotation_translation(rot, pos);
302        let is = Isometry3A::from_mat4(mat);
303
304        let point = Vec3::new(5.0, -5.0, 5.0);
305        let p1 = mat.project_point3(point);
306        let p2 = is.transform_point3(point);
307        assert!(Vec3::abs_diff_eq(p1, p2, 1e-6));
308
309        let point = Vec3A::new(3.3, 4.4, 5.5);
310        let p1 = mat.project_point3a(point);
311        let p2 = is.transform_point3a(point);
312        assert!(Vec3A::abs_diff_eq(p1, p2, 1e-6));
313    }
314
315    #[test]
316    fn test_transform_vec3() {
317        let rot = Quat::from_rotation_z(-0.5);
318        let pos = Vec3::new(1.5, -2.0, -2.0);
319        let mat = Mat4::from_rotation_translation(rot, pos);
320        let is = Isometry3A::from_mat4(mat);
321
322        let vec = Vec3::new(1.0, 0.0, 0.7);
323        let v1 = mat.transform_vector3(vec);
324        let v2 = is.transform_vector3(vec);
325        assert!(Vec3::abs_diff_eq(v1, v2, 1e-6));
326
327        let vec = Vec3A::new(-0.5, 1.0, 0.0);
328        let v1 = mat.transform_vector3a(vec);
329        let v2 = is.transform_vector3a(vec);
330        assert!(Vec3A::abs_diff_eq(v1, v2, 1e-6));
331    }
332
333    #[test]
334    fn test_inverse() {
335        let rot = Quat::from_rotation_z(1.5) * Quat::from_rotation_x(1.0);
336        let pos = Vec3::new(1.99, 0.77, -1.55);
337        let mat = Mat4::from_rotation_translation(rot, pos);
338        let mat_inv = mat.inverse();
339        let is1 = Isometry3A::from_mat4(mat).inverse();
340        let is2 = Isometry3A::from_mat4(mat_inv);
341        assert!(Isometry3A::abs_diff_eq(is1, is2, 1e-6));
342    }
343
344    #[test]
345    fn test_mat4_from() {
346        let rot = Quat::from_rotation_y(-2.0);
347        let pos = Vec3::new(3.0, 3.3, 3.33);
348        let mat = Mat4::from_rotation_translation(rot, pos);
349        let is = Isometry3A::from_mat4(mat);
350        let mat2 = Mat4::from(is);
351        assert!(Mat4::abs_diff_eq(&mat, mat2, 1e-6));
352    }
353
354    #[test]
355    fn test_isometry_mul() {
356        let rot1 = Quat::from_rotation_x(0.77);
357        let pos1 = Vec3::new(6.6, -6.6, 3.3);
358        let mat1 = Mat4::from_rotation_translation(rot1, pos1);
359        let is1 = Isometry3A::from_rotation_translation(rot1, pos1);
360
361        let rot2 = Quat::from_rotation_z(-0.44);
362        let pos2 = Vec3::new(-1.1, -2.2, -3.3);
363        let mat2 = Mat4::from_rotation_translation(rot2, pos2);
364        let is2 = Isometry3A::from_rotation_translation(rot2, pos2);
365
366        let mat = mat1 * mat2;
367        let is = is1 * is2;
368        let is_mat = Isometry3A::from_mat4(mat);
369        assert!(Isometry3A::abs_diff_eq(is, is_mat, 1e-6));
370    }
371}
glam_ext/f32_ext/isometry3a.rs

glam_ext/f32_ext/
isometry3a.rs
