glam_ext/f64_ext/
isometry3a.rs

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

glam_ext/f64_ext/
isometry3a.rs
