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

glamx/
pose3.rs
