glamx/
pose2.rs

1//! 2D pose types for rigid body transformations.
2
3#[cfg(feature = "f64")]
4use crate::rot2::DRot2;
5use crate::rot2::Rot2;
6use core::ops::{Mul, MulAssign};
7
8/// Macro to generate a 2D pose type for a specific scalar type.
9macro_rules! impl_pose2 {
10    ($Pose2:ident, $Rot2:ident, $Real:ty, $Vec2:ty, $Vec3:ty, $Mat2:ty, $Mat3:ty $(, #[$attr:meta])*) => {
11        #[doc = concat!("A 2D 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        #[repr(C)]
16        $(#[$attr])*
17        pub struct $Pose2 {
18            /// The rotational part of the pose.
19            pub rotation: $Rot2,
20            /// The translational part of the pose.
21            pub translation: $Vec2,
22        }
23
24        impl $Pose2 {
25            /// The identity pose (no rotation, no translation).
26            pub const IDENTITY: Self = Self {
27                rotation: $Rot2::IDENTITY,
28                translation: <$Vec2>::ZERO,
29            };
30
31            /// Creates the identity pose.
32            #[inline]
33            pub fn identity() -> Self {
34                Self::IDENTITY
35            }
36
37            /// Creates a pose from a translation vector.
38            #[inline]
39            pub fn from_translation(translation: $Vec2) -> Self {
40                Self {
41                    rotation: $Rot2::IDENTITY,
42                    translation,
43                }
44            }
45
46            /// Creates a pose from translation components.
47            #[inline]
48            pub fn translation(x: $Real, y: $Real) -> Self {
49                Self::from_translation(<$Vec2>::new(x, y))
50            }
51
52            /// Creates a pose from a rotation.
53            #[inline]
54            pub fn from_rotation(rotation: $Rot2) -> Self {
55                Self {
56                    rotation,
57                    translation: <$Vec2>::ZERO,
58                }
59            }
60
61            /// Creates a pose that is a pure rotation with the given angle (in radians).
62            #[inline]
63            pub fn rotation(angle: $Real) -> Self {
64                Self::from_rotation(<$Rot2>::new(angle))
65            }
66
67            /// Creates a pose from translation and rotation parts.
68            #[inline]
69            pub fn from_parts(translation: $Vec2, rotation: $Rot2) -> Self {
70                Self {
71                    rotation,
72                    translation,
73                }
74            }
75
76            /// Creates a pose from translation and angle.
77            #[inline]
78            pub fn new(translation: $Vec2, angle: $Real) -> Self {
79                Self {
80                    rotation: $Rot2::new(angle),
81                    translation,
82                }
83            }
84
85            /// Prepends a translation to this pose (applies translation in local frame).
86            #[inline]
87            pub fn prepend_translation(self, translation: $Vec2) -> Self {
88                Self {
89                    rotation: self.rotation,
90                    translation: self.translation + self.rotation * translation,
91                }
92            }
93
94            /// Appends a translation to this pose (applies translation in world frame).
95            #[inline]
96            pub fn append_translation(self, translation: $Vec2) -> Self {
97                Self {
98                    rotation: self.rotation,
99                    translation: self.translation + translation,
100                }
101            }
102
103            /// Returns the inverse of this pose.
104            #[inline]
105            pub fn inverse(&self) -> Self {
106                let inv_rot = self.rotation.inverse();
107                Self {
108                    rotation: inv_rot,
109                    translation: inv_rot.transform_vector(-self.translation),
110                }
111            }
112
113            /// Computes `self.inverse() * rhs`.
114            #[inline]
115            pub fn inv_mul(&self, rhs: &Self) -> Self {
116                self.inverse() * *rhs
117            }
118
119            /// Transforms a 2D point by this pose.
120            ///
121            /// This applies both the rotation and translation.
122            #[inline]
123            pub fn transform_point(&self, p: $Vec2) -> $Vec2 {
124                self.rotation.transform_vector(p) + self.translation
125            }
126
127            /// Transforms a 2D vector by this pose.
128            ///
129            /// This applies only the rotation (ignores translation).
130            #[inline]
131            pub fn transform_vector(&self, v: $Vec2) -> $Vec2 {
132                self.rotation.transform_vector(v)
133            }
134
135            /// Transforms a point by the inverse of this pose.
136            #[inline]
137            pub fn inverse_transform_point(&self, p: $Vec2) -> $Vec2 {
138                self.rotation.inverse_transform_vector(p - self.translation)
139            }
140
141            /// Transforms a vector by the inverse of this pose.
142            #[inline]
143            pub fn inverse_transform_vector(&self, v: $Vec2) -> $Vec2 {
144                self.rotation.inverse_transform_vector(v)
145            }
146
147            /// Linearly interpolates between two poses.
148            ///
149            /// Uses spherical linear interpolation for the rotation part.
150            #[inline]
151            pub fn lerp(&self, other: &Self, t: $Real) -> Self {
152                Self {
153                    rotation: self.rotation.slerp(&other.rotation, t),
154                    translation: self.translation.lerp(other.translation, t),
155                }
156            }
157
158            /// Returns `true` if, and only if, all elements are finite.
159            #[inline]
160            pub fn is_finite(&self) -> bool {
161                self.rotation.is_finite() && self.translation.is_finite()
162            }
163
164            /// Returns `true` if any element is `NaN`.
165            #[inline]
166            pub fn is_nan(&self) -> bool {
167                self.rotation.is_nan() || self.translation.is_nan()
168            }
169
170            /// Converts this pose to a homogeneous 3x3 matrix.
171            #[inline]
172            pub fn to_mat3(&self) -> $Mat3 {
173                let (sin, cos) = (self.rotation.sin(), self.rotation.cos());
174                <$Mat3>::from_cols(
175                    <$Vec3>::new(cos, sin, 0.0),
176                    <$Vec3>::new(-sin, cos, 0.0),
177                    <$Vec3>::new(self.translation.x, self.translation.y, 1.0),
178                )
179            }
180
181            /// Creates a pose from a homogeneous 3x3 matrix.
182            ///
183            /// The matrix is assumed to represent a rigid body transformation
184            /// (rotation + translation only, no scaling or shearing).
185            #[inline]
186            pub fn from_mat3(mat: $Mat3) -> Self {
187                let rotation = $Rot2::from_mat(<$Mat2>::from_cols(
188                    mat.x_axis.truncate(),
189                    mat.y_axis.truncate(),
190                ));
191                let translation = mat.z_axis.truncate();
192                Self { rotation, translation }
193            }
194        }
195
196        // Pose2 * Pose2
197        impl Mul<$Pose2> for $Pose2 {
198            type Output = Self;
199
200            #[inline]
201            fn mul(self, rhs: Self) -> Self::Output {
202                Self {
203                    rotation: self.rotation * rhs.rotation,
204                    translation: self.translation + self.rotation.transform_vector(rhs.translation),
205                }
206            }
207        }
208
209        impl MulAssign<$Pose2> for $Pose2 {
210            #[inline]
211            fn mul_assign(&mut self, rhs: Self) {
212                *self = *self * rhs;
213            }
214        }
215
216        // Pose2 * Vec2 (transform point - applies rotation and translation)
217        impl Mul<$Vec2> for $Pose2 {
218            type Output = $Vec2;
219
220            #[inline]
221            fn mul(self, rhs: $Vec2) -> Self::Output {
222                self.transform_point(rhs)
223            }
224        }
225
226        impl Mul<$Vec2> for &$Pose2 {
227            type Output = $Vec2;
228
229            #[inline]
230            fn mul(self, rhs: $Vec2) -> Self::Output {
231                self.transform_point(rhs)
232            }
233        }
234
235        impl Mul<&$Vec2> for $Pose2 {
236            type Output = $Vec2;
237
238            #[inline]
239            fn mul(self, rhs: &$Vec2) -> Self::Output {
240                self.transform_point(*rhs)
241            }
242        }
243
244        impl Default for $Pose2 {
245            fn default() -> Self {
246                Self::IDENTITY
247            }
248        }
249
250        // &Pose2 * &Pose2
251        impl Mul<&$Pose2> for &$Pose2 {
252            type Output = $Pose2;
253
254            #[inline]
255            fn mul(self, rhs: &$Pose2) -> Self::Output {
256                *self * *rhs
257            }
258        }
259
260        // Pose2 * &Pose2
261        impl Mul<&$Pose2> for $Pose2 {
262            type Output = $Pose2;
263
264            #[inline]
265            fn mul(self, rhs: &$Pose2) -> Self::Output {
266                self * *rhs
267            }
268        }
269
270        // &Pose2 * Pose2
271        impl Mul<$Pose2> for &$Pose2 {
272            type Output = $Pose2;
273
274            #[inline]
275            fn mul(self, rhs: $Pose2) -> Self::Output {
276                *self * rhs
277            }
278        }
279
280        impl Mul<$Pose2> for $Rot2 {
281            type Output = $Pose2;
282
283            #[inline]
284            fn mul(self, rhs: $Pose2) -> Self::Output {
285                $Pose2 {
286                    translation: self * rhs.translation,
287                    rotation: self * rhs.rotation,
288                }
289            }
290        }
291
292        impl Mul<$Rot2> for $Pose2 {
293            type Output = $Pose2;
294
295            #[inline]
296            fn mul(self, rhs: $Rot2) -> Self::Output {
297                $Pose2 {
298                    translation: self.translation,
299                    rotation: self.rotation * rhs,
300                }
301            }
302        }
303
304        impl MulAssign<$Rot2> for $Pose2 {
305            #[inline]
306            fn mul_assign(&mut self, rhs: $Rot2) {
307                *self = *self * rhs;
308            }
309        }
310
311        impl From<$Rot2> for $Pose2 {
312            #[inline]
313            fn from(rotation: $Rot2) -> Self {
314                Self::from_rotation(rotation)
315            }
316        }
317
318        impl From<($Vec2, $Real)> for $Pose2 {
319            #[inline]
320            fn from((translation, rotation): ($Vec2, $Real)) -> Self {
321                Self {
322                    translation,
323                    rotation: $Rot2::new(rotation),
324                }
325            }
326        }
327
328        impl From<$Pose2> for ($Vec2, $Real) {
329            #[inline]
330            fn from(pose: $Pose2) -> ($Vec2, $Real) {
331                (pose.translation, pose.rotation.angle())
332            }
333        }
334
335        #[cfg(feature = "approx")]
336        impl approx::AbsDiffEq for $Pose2 {
337            type Epsilon = $Real;
338
339            fn default_epsilon() -> Self::Epsilon {
340                <$Real>::EPSILON
341            }
342
343            fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
344                self.rotation.abs_diff_eq(&other.rotation, epsilon)
345                    && self.translation.abs_diff_eq(other.translation, epsilon)
346            }
347        }
348
349        #[cfg(feature = "approx")]
350        impl approx::RelativeEq for $Pose2 {
351            fn default_max_relative() -> Self::Epsilon {
352                <$Real>::EPSILON
353            }
354
355            fn relative_eq(
356                &self,
357                other: &Self,
358                epsilon: Self::Epsilon,
359                max_relative: Self::Epsilon,
360            ) -> bool {
361                self.rotation.relative_eq(&other.rotation, epsilon, max_relative)
362                    && self.translation.abs_diff_eq(other.translation, epsilon.max(max_relative))
363            }
364        }
365    };
366}
367
368impl_pose2!(
369    Pose2,
370    Rot2,
371    f32,
372    glam::Vec2,
373    glam::Vec3,
374    glam::Mat2,
375    glam::Mat3,
376    #[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))]
377);
378#[cfg(feature = "f64")]
379impl_pose2!(
380    DPose2,
381    DRot2,
382    f64,
383    glam::DVec2,
384    glam::DVec3,
385    glam::DMat2,
386    glam::DMat3
387);
388
389// f32 <-> f64 conversions
390#[cfg(feature = "f64")]
391impl From<Pose2> for DPose2 {
392    #[inline]
393    fn from(p: Pose2) -> Self {
394        Self {
395            rotation: p.rotation.into(),
396            translation: p.translation.into(),
397        }
398    }
399}
400
401#[cfg(feature = "f64")]
402impl From<DPose2> for Pose2 {
403    #[inline]
404    fn from(p: DPose2) -> Self {
405        Self {
406            rotation: p.rotation.into(),
407            translation: p.translation.as_vec2(),
408        }
409    }
410}
411
412// Nalgebra conversions
413#[cfg(feature = "nalgebra")]
414mod nalgebra_conv {
415    use super::*;
416
417    impl From<Pose2> for nalgebra::Isometry2<f32> {
418        fn from(p: Pose2) -> Self {
419            nalgebra::Isometry2 {
420                translation: p.translation.into(),
421                rotation: p.rotation.into(),
422            }
423        }
424    }
425
426    impl From<nalgebra::Isometry2<f32>> for Pose2 {
427        fn from(iso: nalgebra::Isometry2<f32>) -> Self {
428            Self {
429                rotation: Rot2::from_cos_sin_unchecked(iso.rotation.re, iso.rotation.im),
430                translation: glam::Vec2::new(iso.translation.x, iso.translation.y),
431            }
432        }
433    }
434
435    #[cfg(feature = "f64")]
436    impl From<DPose2> for nalgebra::Isometry2<f64> {
437        fn from(p: DPose2) -> Self {
438            nalgebra::Isometry2 {
439                translation: p.translation.into(),
440                rotation: p.rotation.into(),
441            }
442        }
443    }
444
445    #[cfg(feature = "f64")]
446    impl From<nalgebra::Isometry2<f64>> for DPose2 {
447        fn from(iso: nalgebra::Isometry2<f64>) -> Self {
448            Self {
449                rotation: DRot2::from_cos_sin_unchecked(iso.rotation.re, iso.rotation.im),
450                translation: glam::DVec2::new(iso.translation.x, iso.translation.y),
451            }
452        }
453    }
454}
455
456#[cfg(test)]
457mod tests {
458    use super::*;
459    use approx::assert_relative_eq;
460    use core::f32::consts::PI;
461
462    #[test]
463    fn test_pose2_identity() {
464        let p = Pose2::IDENTITY;
465        assert_eq!(p.rotation, Rot2::IDENTITY);
466        assert_eq!(p.translation, glam::Vec2::ZERO);
467        assert!(p.is_finite());
468    }
469
470    #[test]
471    fn test_pose2_from_translation() {
472        let t = glam::Vec2::new(1.0, 2.0);
473        let p = Pose2::from_translation(t);
474        assert_eq!(p.translation, t);
475        assert_eq!(p.rotation, Rot2::IDENTITY);
476    }
477
478    #[test]
479    fn test_pose2_from_rotation_translation() {
480        let t = glam::Vec2::new(1.0, 2.0);
481        let r = Rot2::new(PI / 4.0);
482        let p = Pose2::from_parts(t, r);
483        assert_eq!(p.translation, t);
484        assert_eq!(p.rotation, r);
485    }
486
487    #[test]
488    fn test_pose2_transform_point() {
489        let p = Pose2::new(glam::Vec2::new(1.0, 0.0), PI / 2.0);
490        let point = glam::Vec2::new(1.0, 0.0);
491        let result = p.transform_point(point);
492        assert_relative_eq!(result.x, 1.0, epsilon = 1e-6);
493        assert_relative_eq!(result.y, 1.0, epsilon = 1e-6);
494        // Operator should also work
495        let result2 = p * point;
496        assert_relative_eq!(result2.x, 1.0, epsilon = 1e-6);
497        assert_relative_eq!(result2.y, 1.0, epsilon = 1e-6);
498    }
499
500    #[test]
501    fn test_pose2_transform_vector() {
502        let p = Pose2::new(glam::Vec2::new(100.0, 200.0), PI / 2.0);
503        let vector = glam::Vec2::new(1.0, 0.0);
504        // Vector transformation ignores translation
505        let result = p.transform_vector(vector);
506        assert_relative_eq!(result.x, 0.0, epsilon = 1e-6);
507        assert_relative_eq!(result.y, 1.0, epsilon = 1e-6);
508    }
509
510    #[test]
511    fn test_pose2_inverse() {
512        let p = Pose2::new(glam::Vec2::new(1.0, 2.0), 0.5);
513        let inv = p.inverse();
514        let identity = p * inv;
515        assert_relative_eq!(identity.rotation.re, 1.0, epsilon = 1e-6);
516        assert_relative_eq!(identity.rotation.im, 0.0, epsilon = 1e-6);
517        assert_relative_eq!(identity.translation.x, 0.0, epsilon = 1e-6);
518        assert_relative_eq!(identity.translation.y, 0.0, epsilon = 1e-6);
519    }
520
521    #[test]
522    fn test_pose2_to_from_mat3() {
523        let p = Pose2::new(glam::Vec2::new(1.0, 2.0), PI / 4.0);
524        let m = p.to_mat3();
525        let p2 = Pose2::from_mat3(m);
526        assert_relative_eq!(p.rotation.re, p2.rotation.re, epsilon = 1e-6);
527        assert_relative_eq!(p.rotation.im, p2.rotation.im, epsilon = 1e-6);
528        assert_relative_eq!(p.translation.x, p2.translation.x, epsilon = 1e-6);
529        assert_relative_eq!(p.translation.y, p2.translation.y, epsilon = 1e-6);
530    }
531
532    #[test]
533    fn test_pose2_lerp_across_branch_cut() {
534        let p1 = Pose2::new(glam::Vec2::new(0.0, 0.0), 3.13);
535        let p2 = Pose2::new(glam::Vec2::new(2.0, 4.0), 3.15);
536        let mid = p1.lerp(&p2, 0.5);
537        assert_relative_eq!(mid.rotation.angle(), 3.14, epsilon = 1e-6);
538        assert_relative_eq!(mid.translation.x, 1.0, epsilon = 1e-6);
539        assert_relative_eq!(mid.translation.y, 2.0, epsilon = 1e-6);
540    }
541
542    #[cfg(feature = "f64")]
543    #[test]
544    fn test_dpose2_new() {
545        let p = DPose2::new(glam::DVec2::new(1.0, 2.0), core::f64::consts::PI / 4.0);
546        assert_relative_eq!(p.translation.x, 1.0, epsilon = 1e-10);
547        assert_relative_eq!(p.translation.y, 2.0, epsilon = 1e-10);
548    }
549
550    #[test]
551    fn test_convert_back_to_equal_pose2() {
552        let (t1, r1) = (glam::Vec2::new(1.0, 2.0), PI / 4.0);
553        let p: Pose2 = (t1, r1).into();
554        let (t2, r2) = p.into();
555
556        assert_relative_eq!(t1, t2, epsilon = 1e-6);
557        assert_relative_eq!(r1, r2, epsilon = 1e-6);
558    }
559}
glamx/pose2.rs

glamx/
pose2.rs
