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

glamx/
pose2.rs
