glamx/
pose3.rs

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

glamx/
pose3.rs
