rapier3d/utils/
pos_ops.rs

1//! SimdPose trait for pose (isometry) operations.
2
3#[cfg(feature = "simd-is-enabled")]
4use crate::math::SimdReal;
5use crate::math::{AngVector, Pose, Real, Rotation, Vector};
6use crate::utils::ScalarType;
7
8/// Trait for pose types (isometry) providing access to rotation and translation.
9pub trait PoseOps<N: ScalarType>: Copy {
10    /// Get the rotation component of this pose.
11    fn rotation(&self) -> N::Rotation;
12    /// Get the translation component of this pose as a vector.
13    fn translation(&self) -> N::Vector;
14    /// Set the translation component of this pose.
15    fn set_translation(&mut self, tra: N::Vector);
16    /// Prepend a translation to this pose.
17    fn prepend_translation(&self, translation: N::Vector) -> Self;
18    /// Append a translation to this pose.
19    fn append_translation(&self, translation: N::Vector) -> Self;
20    /// Prepend a rotation (as an axis-angle) to this pose.
21    fn prepend_rotation(&self, axisangle: N::AngVector) -> Self;
22    /// Append a rotation (as an axis-angle) to this pose.
23    fn append_rotation(&self, axisangle: N::AngVector) -> Self;
24}
25
26#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
27impl PoseOps<SimdReal> for na::Isometry3<SimdReal> {
28    #[inline]
29    fn rotation(&self) -> na::UnitQuaternion<SimdReal> {
30        self.rotation
31    }
32    #[inline]
33    fn translation(&self) -> na::Vector3<SimdReal> {
34        self.translation.vector
35    }
36
37    #[inline]
38    fn set_translation(&mut self, tra: na::Vector3<SimdReal>) {
39        self.translation.vector = tra;
40    }
41
42    #[inline]
43    fn prepend_translation(&self, translation: na::Vector3<SimdReal>) -> Self {
44        self * na::Translation3::from(translation)
45    }
46
47    #[inline]
48    fn append_translation(&self, translation: na::Vector3<SimdReal>) -> Self {
49        na::Translation3::from(translation) * self
50    }
51
52    #[inline]
53    fn prepend_rotation(&self, rotation: na::Vector3<SimdReal>) -> Self {
54        self * na::UnitQuaternion::new(rotation)
55    }
56
57    #[inline]
58    fn append_rotation(&self, rotation: na::Vector3<SimdReal>) -> Self {
59        na::UnitQuaternion::new(rotation) * self
60    }
61}
62
63#[cfg(all(feature = "dim2", feature = "simd-is-enabled"))]
64impl PoseOps<SimdReal> for na::Isometry2<SimdReal> {
65    #[inline]
66    fn rotation(&self) -> na::UnitComplex<SimdReal> {
67        self.rotation
68    }
69    #[inline]
70    fn translation(&self) -> na::Vector2<SimdReal> {
71        self.translation.vector
72    }
73
74    #[inline]
75    fn set_translation(&mut self, tra: na::Vector2<SimdReal>) {
76        self.translation.vector = tra;
77    }
78
79    #[inline]
80    fn prepend_translation(&self, translation: na::Vector2<SimdReal>) -> Self {
81        self * na::Translation2::from(translation)
82    }
83
84    #[inline]
85    fn append_translation(&self, translation: na::Vector2<SimdReal>) -> Self {
86        na::Translation2::from(translation) * self
87    }
88
89    #[inline]
90    fn prepend_rotation(&self, rotation: SimdReal) -> Self {
91        self * na::UnitComplex::new(rotation)
92    }
93
94    #[inline]
95    fn append_rotation(&self, rotation: SimdReal) -> Self {
96        na::UnitComplex::new(rotation) * self
97    }
98}
99
100impl PoseOps<Real> for Pose {
101    #[inline]
102    fn rotation(&self) -> Rotation {
103        self.rotation
104    }
105    #[inline]
106    fn translation(&self) -> Vector {
107        self.translation
108    }
109
110    #[inline]
111    fn set_translation(&mut self, tra: Vector) {
112        self.translation = tra;
113    }
114
115    #[inline]
116    fn prepend_translation(&self, translation: Vector) -> Self {
117        (*self).prepend_translation(translation)
118    }
119
120    #[inline]
121    fn append_translation(&self, translation: Vector) -> Self {
122        (*self).append_translation(translation)
123    }
124
125    #[inline]
126    fn prepend_rotation(&self, rotation: AngVector) -> Self {
127        #[cfg(feature = "dim2")]
128        return *self * Rotation::from_angle(rotation);
129        #[cfg(feature = "dim3")]
130        return *self * Rotation::from_scaled_axis(rotation);
131    }
132
133    #[inline]
134    fn append_rotation(&self, rotation: AngVector) -> Self {
135        #[cfg(feature = "dim2")]
136        return Rotation::from_angle(rotation) * *self;
137        #[cfg(feature = "dim3")]
138        return Rotation::from_scaled_axis(rotation) * *self;
139    }
140}
rapier3d/utils/pos_ops.rs

rapier3d/utils/
pos_ops.rs
