glamx/
pose3.rs

1//! 3D pose types for rigid body transformations.
2
3use crate::rot3::{DRot3, Rot3};
4use core::ops::{Mul, MulAssign};
5
6/// Macro to generate a 3D pose type for a specific scalar type.
7macro_rules! impl_pose3 {
8    ($Pose3:ident, $Rot3:ident, $Real:ty, $Vec3:ty, $Mat4: ty $(, #[$attr:meta])*) => {
9        #[doc = concat!("A 3D pose (rotation + translation), representing a rigid body transformation (", stringify!($Real), " precision).")]
10        #[derive(Copy, Clone, Debug, PartialEq)]
11        #[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
12        #[cfg_attr(feature = "rkyv", derive(rkyv::Archive, rkyv::Serialize, rkyv::Deserialize))]
13        $(#[$attr])*
14        pub struct $Pose3 {
15            /// The rotational part of the pose.
16            pub rotation: $Rot3,
17            /// The translational part of the pose.
18            pub translation: $Vec3,
19        }
20
21        impl $Pose3 {
22            /// The identity pose (no rotation, no translation).
23            pub const IDENTITY: Self = Self {
24                rotation: $Rot3::IDENTITY,
25                translation: <$Vec3>::ZERO,
26            };
27
28            /// Creates the identity pose.
29            #[inline]
30            pub fn identity() -> Self {
31                Self::IDENTITY
32            }
33
34            /// Creates a pose from a translation vector.
35            #[inline]
36            pub fn from_translation(translation: $Vec3) -> Self {
37                Self {
38                    rotation: $Rot3::IDENTITY,
39                    translation,
40                }
41            }
42
43            /// Creates a pose from translation components.
44            #[inline]
45            pub fn translation(x: $Real, y: $Real, z: $Real) -> Self {
46                Self::from_translation(<$Vec3>::new(x, y, z))
47            }
48
49            /// Creates a pose from a rotation.
50            #[inline]
51            pub fn from_rotation(rotation: $Rot3) -> Self {
52                Self {
53                    rotation,
54                    translation: <$Vec3>::ZERO,
55                }
56            }
57
58            /// Creates a pose from translation and rotation parts.
59            #[inline]
60            pub fn from_parts(translation: $Vec3, rotation: $Rot3) -> Self {
61                Self {
62                    rotation,
63                    translation,
64                }
65            }
66
67            /// Creates a pose from translation and axis-angle rotation.
68            #[inline]
69            pub fn new(translation: $Vec3, axisangle: $Vec3) -> Self {
70                let rotation = $Rot3::from_scaled_axis(axisangle.into());
71                Self {
72                    rotation,
73                    translation,
74                }
75            }
76
77            /// Creates a pose from axis-angle rotation only (no translation).
78            #[inline]
79            pub fn rotation(axisangle: $Vec3) -> Self {
80                Self {
81                    rotation: $Rot3::from_scaled_axis(axisangle.into()),
82                    translation: <$Vec3>::ZERO,
83                }
84            }
85
86            /// Prepends a translation to this pose (applies translation in local frame).
87            #[inline]
88            pub fn prepend_translation(self, translation: $Vec3) -> Self {
89                Self {
90                    rotation: self.rotation,
91                    translation: self.translation + self.rotation * translation,
92                }
93            }
94
95            /// Appends a translation to this pose (applies translation in world frame).
96            #[inline]
97            pub fn append_translation(self, translation: $Vec3) -> Self {
98                Self {
99                    rotation: self.rotation,
100                    translation: self.translation + translation,
101                }
102            }
103
104            /// Returns the inverse of this pose.
105            #[inline]
106            pub fn inverse(&self) -> Self {
107                let inv_rot = self.rotation.inverse();
108                Self {
109                    rotation: inv_rot,
110                    translation: inv_rot * (-self.translation),
111                }
112            }
113
114            /// Computes `self.inverse() * rhs`.
115            #[inline]
116            pub fn inv_mul(&self, rhs: &Self) -> Self {
117                let inv_rot1 = self.rotation.inverse();
118                let tr_12 = rhs.translation - self.translation;
119                $Pose3 {
120                    translation: inv_rot1 * tr_12,
121                    rotation: inv_rot1 * rhs.rotation,
122                }
123            }
124
125            /// Transforms a 3D point by this pose.
126            ///
127            /// This applies both the rotation and translation.
128            #[inline]
129            pub fn transform_point(&self, p: $Vec3) -> $Vec3 {
130                self.rotation * p + self.translation
131            }
132
133            /// Transforms a 3D vector by this pose.
134            ///
135            /// This applies only the rotation (ignores translation).
136            #[inline]
137            pub fn transform_vector(&self, v: $Vec3) -> $Vec3 {
138                self.rotation * v
139            }
140
141            /// Transforms a point by the inverse of this pose.
142            #[inline]
143            pub fn inverse_transform_point(&self, p: $Vec3) -> $Vec3 {
144                self.rotation.inverse() * (p - self.translation)
145            }
146
147            /// Transforms a vector by the inverse of this pose.
148            #[inline]
149            pub fn inverse_transform_vector(&self, v: $Vec3) -> $Vec3 {
150                self.rotation.inverse() * v
151            }
152
153            /// Linearly interpolates between two poses.
154            ///
155            /// Uses spherical linear interpolation for the rotation part.
156            #[inline]
157            pub fn lerp(&self, other: &Self, t: $Real) -> Self {
158                Self {
159                    rotation: self.rotation.slerp(other.rotation, t),
160                    translation: self.translation.lerp(other.translation, t),
161                }
162            }
163
164            /// Returns `true` if, and only if, all elements are finite.
165            #[inline]
166            pub fn is_finite(&self) -> bool {
167                self.rotation.is_finite() && self.translation.is_finite()
168            }
169
170            /// Returns `true` if any element is `NaN`.
171            #[inline]
172            pub fn is_nan(&self) -> bool {
173                self.rotation.is_nan() || self.translation.is_nan()
174            }
175
176            /// Converts this pose to a homogeneous 4x4 matrix.
177            #[inline]
178            pub fn to_mat4(&self) -> $Mat4 {
179                <$Mat4>::from_rotation_translation(self.rotation, self.translation.into())
180            }
181
182            /// Creates a pose from a homogeneous 4x4 matrix.
183            ///
184            /// The matrix is assumed to represent a rigid body transformation
185            /// (rotation + translation only, no scaling or shearing).
186            #[inline]
187            pub fn from_mat4(mat: $Mat4) -> Self {
188                let (_, rotation, translation) = mat.to_scale_rotation_translation();
189                Self {
190                    rotation,
191                    translation: translation.into(),
192                }
193            }
194
195            /// Builds a right-handed look-at view matrix.
196            ///
197            /// It maps the view direction target - eye to the negative z axis to and the eye to the
198            /// origin. This conforms to the common notion of right-handed camera look-at view matrix from the computer graphics community, i.e. the camera is assumed to look toward its local -z axis.
199            ///
200            /// # Arguments
201            /// - `eye`: The eye position.
202            /// - `target`: The target position.
203            /// - `up`: A vector approximately aligned with required the vertical axis. The only
204            ///   requirement of this parameter is to not be collinear to target - eye.
205            #[inline]
206            pub fn look_at_rh(eye: $Vec3, target: $Vec3, up: $Vec3) -> $Pose3 {
207                let rotation = <$Rot3>::look_at_rh(eye.into(), target.into(), up.into());
208                let translation = rotation * (-eye);
209
210                $Pose3 {
211                    rotation,
212                    translation,
213                }
214            }
215
216            /// Creates a pose that corresponds to the local frame of an observer standing at the
217            /// point eye and looking toward target.
218            ///
219            /// It maps the `z` axis to the view direction target - eye and the origin to the eye.
220            ///
221            /// # Arguments
222            /// - `eye`: The observer position.
223            /// - `target`: The target position.
224            /// - `up`: Vertical direction. The only requirement of this parameter is to not be
225            ///   collinear to eye - at. Non-collinearity is not checked.
226            #[inline]
227            pub fn face_towards(eye: $Vec3, target: $Vec3, up: $Vec3) -> $Pose3 {
228                $Pose3 {
229                    rotation: <$Rot3>::look_at_lh(eye.into(), target.into(), up.into()).inverse(),
230                    translation: eye,
231                }
232            }
233        }
234
235        // Pose3 * Pose3
236        impl Mul<$Pose3> for $Pose3 {
237            type Output = Self;
238
239            #[inline]
240            fn mul(self, rhs: Self) -> Self::Output {
241                Self {
242                    rotation: self.rotation * rhs.rotation,
243                    translation: self.translation + self.rotation * rhs.translation,
244                }
245            }
246        }
247
248        impl MulAssign<$Pose3> for $Pose3 {
249            #[inline]
250            fn mul_assign(&mut self, rhs: Self) {
251                *self = *self * rhs;
252            }
253        }
254
255        // Pose3 * Vec3 (transform point - applies rotation and translation)
256        impl Mul<$Vec3> for $Pose3 {
257            type Output = $Vec3;
258
259            #[inline]
260            fn mul(self, rhs: $Vec3) -> Self::Output {
261                self.transform_point(rhs)
262            }
263        }
264
265        impl Mul<$Vec3> for &$Pose3 {
266            type Output = $Vec3;
267
268            #[inline]
269            fn mul(self, rhs: $Vec3) -> Self::Output {
270                self.transform_point(rhs)
271            }
272        }
273
274        impl Mul<&$Vec3> for $Pose3 {
275            type Output = $Vec3;
276
277            #[inline]
278            fn mul(self, rhs: &$Vec3) -> Self::Output {
279                self.transform_point(*rhs)
280            }
281        }
282
283        // &Pose3 * &Pose3
284        impl Mul<&$Pose3> for &$Pose3 {
285            type Output = $Pose3;
286
287            #[inline]
288            fn mul(self, rhs: &$Pose3) -> Self::Output {
289                *self * *rhs
290            }
291        }
292
293        // Pose3 * &Pose3
294        impl Mul<&$Pose3> for $Pose3 {
295            type Output = $Pose3;
296
297            #[inline]
298            fn mul(self, rhs: &$Pose3) -> Self::Output {
299                self * *rhs
300            }
301        }
302
303        // &Pose3 * Pose3
304        impl Mul<$Pose3> for &$Pose3 {
305            type Output = $Pose3;
306
307            #[inline]
308            fn mul(self, rhs: $Pose3) -> Self::Output {
309                *self * rhs
310            }
311        }
312
313        impl Default for $Pose3 {
314            fn default() -> Self {
315                Self::IDENTITY
316            }
317        }
318
319        impl Mul<$Pose3> for $Rot3 {
320            type Output = $Pose3;
321
322            #[inline]
323            fn mul(self, rhs: $Pose3) -> Self::Output {
324                $Pose3 {
325                    translation: self * rhs.translation,
326                    rotation: self * rhs.rotation,
327                }
328            }
329        }
330
331        impl Mul<$Rot3> for $Pose3 {
332            type Output = $Pose3;
333
334            #[inline]
335            fn mul(self, rhs: $Rot3) -> Self::Output {
336                $Pose3 {
337                    translation: self.translation,
338                    rotation: self.rotation * rhs,
339                }
340            }
341        }
342
343        impl MulAssign<$Rot3> for $Pose3 {
344            #[inline]
345            fn mul_assign(&mut self, rhs: $Rot3) {
346                *self = *self * rhs;
347            }
348        }
349
350        impl From<$Rot3> for $Pose3 {
351            #[inline]
352            fn from(rotation: $Rot3) -> Self {
353                Self::from_rotation(rotation)
354            }
355        }
356
357        #[cfg(feature = "approx")]
358        impl approx::AbsDiffEq for $Pose3 {
359            type Epsilon = $Real;
360
361            fn default_epsilon() -> Self::Epsilon {
362                <$Real>::EPSILON
363            }
364
365            fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
366                self.rotation.abs_diff_eq(other.rotation, epsilon)
367                    && self.translation.abs_diff_eq(other.translation, epsilon)
368            }
369        }
370
371        #[cfg(feature = "approx")]
372        impl approx::RelativeEq for $Pose3 {
373            fn default_max_relative() -> Self::Epsilon {
374                <$Real>::EPSILON
375            }
376
377            fn relative_eq(
378                &self,
379                other: &Self,
380                epsilon: Self::Epsilon,
381                max_relative: Self::Epsilon,
382            ) -> bool {
383                self.rotation.abs_diff_eq(other.rotation, epsilon.max(max_relative))
384                    && self.translation.abs_diff_eq(other.translation, epsilon.max(max_relative))
385            }
386        }
387    };
388}
389
390impl_pose3!(Pose3, Rot3, f32, glam::Vec3, glam::Mat4);
391impl_pose3!(Pose3A, Rot3, f32, glam::Vec3A, glam::Mat4);
392impl_pose3!(DPose3, DRot3, f64, glam::DVec3, glam::DMat4);
393
394// f32 <-> f64 conversions
395impl From<Pose3> for DPose3 {
396    #[inline]
397    fn from(p: Pose3) -> Self {
398        Self {
399            rotation: p.rotation.as_dquat(),
400            translation: p.translation.as_dvec3(),
401        }
402    }
403}
404
405impl From<DPose3> for Pose3 {
406    #[inline]
407    fn from(p: DPose3) -> Self {
408        Self {
409            rotation: p.rotation.as_quat(),
410            translation: p.translation.as_vec3(),
411        }
412    }
413}
414
415impl From<Pose3A> for DPose3 {
416    #[inline]
417    fn from(p: Pose3A) -> Self {
418        Self {
419            rotation: p.rotation.as_dquat(),
420            translation: p.translation.as_dvec3(),
421        }
422    }
423}
424
425impl From<DPose3> for Pose3A {
426    #[inline]
427    fn from(p: DPose3) -> Self {
428        Self {
429            rotation: p.rotation.as_quat(),
430            translation: p.translation.as_vec3a(),
431        }
432    }
433}
434
435// Pose3 <-> Pose3A conversions
436impl From<Pose3> for Pose3A {
437    #[inline]
438    fn from(p: Pose3) -> Self {
439        Self {
440            rotation: p.rotation,
441            translation: p.translation.into(),
442        }
443    }
444}
445
446impl From<Pose3A> for Pose3 {
447    #[inline]
448    fn from(p: Pose3A) -> Self {
449        Self {
450            rotation: p.rotation,
451            translation: p.translation.into(),
452        }
453    }
454}
455
456// Nalgebra conversions
457#[cfg(feature = "nalgebra")]
458mod nalgebra_conv {
459    use super::*;
460
461    impl From<Pose3> for nalgebra::Isometry3<f32> {
462        fn from(p: Pose3) -> Self {
463            nalgebra::Isometry3 {
464                translation: p.translation.into(),
465                rotation: p.rotation.into(),
466            }
467        }
468    }
469
470    impl From<nalgebra::Isometry3<f32>> for Pose3 {
471        fn from(iso: nalgebra::Isometry3<f32>) -> Self {
472            Self {
473                rotation: glam::Quat::from_xyzw(
474                    iso.rotation.i,
475                    iso.rotation.j,
476                    iso.rotation.k,
477                    iso.rotation.w,
478                ),
479                translation: glam::Vec3::new(
480                    iso.translation.x,
481                    iso.translation.y,
482                    iso.translation.z,
483                ),
484            }
485        }
486    }
487
488    impl From<DPose3> for nalgebra::Isometry3<f64> {
489        fn from(p: DPose3) -> Self {
490            nalgebra::Isometry3 {
491                translation: p.translation.into(),
492                rotation: p.rotation.into(),
493            }
494        }
495    }
496
497    impl From<nalgebra::Isometry3<f64>> for DPose3 {
498        fn from(iso: nalgebra::Isometry3<f64>) -> Self {
499            Self {
500                rotation: iso.rotation.into(),
501                translation: iso.translation.into(),
502            }
503        }
504    }
505}
506
507#[cfg(test)]
508mod tests {
509    use super::*;
510    use approx::assert_relative_eq;
511    use core::f32::consts::PI;
512
513    #[test]
514    fn test_pose3_identity() {
515        let p = Pose3::IDENTITY;
516        assert_eq!(p.rotation, Rot3::IDENTITY);
517        assert_eq!(p.translation, glam::Vec3::ZERO);
518        assert!(p.is_finite());
519    }
520
521    #[test]
522    fn test_pose3_from_translation() {
523        let t = glam::Vec3::new(1.0, 2.0, 3.0);
524        let p = Pose3::from_translation(t);
525        assert_eq!(p.translation, t);
526        assert_eq!(p.rotation, Rot3::IDENTITY);
527    }
528
529    #[test]
530    fn test_pose3_from_rotation_translation() {
531        let t = glam::Vec3::new(1.0, 2.0, 3.0);
532        let r = Rot3::from_rotation_z(PI / 4.0);
533        let p = Pose3::from_parts(t, r);
534        assert_eq!(p.translation, t);
535        assert_eq!(p.rotation, r);
536    }
537
538    #[test]
539    fn test_pose3_transform_point() {
540        let axisangle = glam::Vec3::new(0.0, 0.0, PI / 2.0);
541        let p = Pose3::new(glam::Vec3::new(1.0, 0.0, 0.0), axisangle);
542        let point = glam::Vec3::new(1.0, 0.0, 0.0);
543        let result = p.transform_point(point);
544        assert_relative_eq!(result.x, 1.0, epsilon = 1e-6);
545        assert_relative_eq!(result.y, 1.0, epsilon = 1e-6);
546        assert_relative_eq!(result.z, 0.0, epsilon = 1e-6);
547        // Operator should also work
548        let result2 = p * point;
549        assert_relative_eq!(result2.x, 1.0, epsilon = 1e-6);
550        assert_relative_eq!(result2.y, 1.0, epsilon = 1e-6);
551        assert_relative_eq!(result2.z, 0.0, epsilon = 1e-6);
552    }
553
554    #[test]
555    fn test_pose3_transform_vector() {
556        let axisangle = glam::Vec3::new(0.0, 0.0, PI / 2.0);
557        let p = Pose3::new(glam::Vec3::new(100.0, 200.0, 300.0), axisangle);
558        let vector = glam::Vec3::new(1.0, 0.0, 0.0);
559        // Vector transformation ignores translation
560        let result = p.transform_vector(vector);
561        assert_relative_eq!(result.x, 0.0, epsilon = 1e-6);
562        assert_relative_eq!(result.y, 1.0, epsilon = 1e-6);
563        assert_relative_eq!(result.z, 0.0, epsilon = 1e-6);
564    }
565
566    #[test]
567    fn test_pose3_inverse() {
568        let axisangle = glam::Vec3::new(0.1, 0.2, 0.3);
569        let p = Pose3::new(axisangle, glam::Vec3::new(1.0, 2.0, 3.0));
570        let inv = p.inverse();
571        let identity = p * inv;
572        assert_relative_eq!(identity.rotation, Rot3::IDENTITY, epsilon = 1e-6);
573        assert_relative_eq!(identity.translation.x, 0.0, epsilon = 1e-6);
574        assert_relative_eq!(identity.translation.y, 0.0, epsilon = 1e-6);
575        assert_relative_eq!(identity.translation.z, 0.0, epsilon = 1e-6);
576    }
577
578    #[test]
579    fn test_pose3_to_from_mat4() {
580        let axisangle = glam::Vec3::new(0.1, 0.2, 0.3);
581        let p = Pose3::new(axisangle, glam::Vec3::new(1.0, 2.0, 3.0));
582        let m = p.to_mat4();
583        let p2 = Pose3::from_mat4(m);
584        assert_relative_eq!(p.rotation.x, p2.rotation.x, epsilon = 1e-5);
585        assert_relative_eq!(p.rotation.y, p2.rotation.y, epsilon = 1e-5);
586        assert_relative_eq!(p.rotation.z, p2.rotation.z, epsilon = 1e-5);
587        assert_relative_eq!(p.rotation.w, p2.rotation.w, epsilon = 1e-5);
588        assert_relative_eq!(p.translation.x, p2.translation.x, epsilon = 1e-6);
589        assert_relative_eq!(p.translation.y, p2.translation.y, epsilon = 1e-6);
590        assert_relative_eq!(p.translation.z, p2.translation.z, epsilon = 1e-6);
591    }
592
593    #[test]
594    fn test_dpose3_new() {
595        let axisangle = glam::DVec3::new(0.1, 0.2, 0.3);
596        let p = DPose3::new(glam::DVec3::new(1.0, 2.0, 3.0), axisangle);
597        assert_relative_eq!(p.translation.x, 1.0, epsilon = 1e-10);
598        assert_relative_eq!(p.translation.y, 2.0, epsilon = 1e-10);
599        assert_relative_eq!(p.translation.z, 3.0, epsilon = 1e-10);
600    }
601}
glamx/pose3.rs

glamx/
pose3.rs
