glamx/
pose2.rs

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

glamx/
pose2.rs
