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

glam_ext/f32_ext/
isometry3a.rs
