glam_ext/f32_ext/
isometry3a.rs

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

glam_ext/f32_ext/
isometry3a.rs
