rapier2d/utils/
rotation_ops.rs

1//! RotationOps trait for quaternion operations.
2
3#[cfg(feature = "dim3")]
4use crate::math::Mat3;
5#[cfg(feature = "simd-is-enabled")]
6use crate::math::SimdReal;
7use crate::math::{Matrix, Real, Rotation, Vector};
8#[cfg(feature = "dim3")]
9use crate::utils::CrossProductMatrix;
10use crate::utils::ScalarType;
11use core::fmt::Debug;
12use core::ops::Mul;
13#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
14use na::{Matrix3, UnitQuaternion};
15
16/// Trait implemented by quaternions.
17#[cfg(feature = "dim3")]
18pub trait RotationOps<N: ScalarType>:
19    Copy + Debug + Mul<N::Rotation, Output = N::Rotation> + Mul<N::Vector, Output = N::Vector>
20{
21    /// Converts this rotation to a rotation matrix.
22    fn to_mat(self) -> N::Matrix;
23    /// Returns the inverse of this rotation.
24    fn inverse(self) -> Self;
25    /// Compute the differential of `inv(q1) * q2`.
26    fn diff_conj1_2(&self, rhs: &Self) -> N::Matrix;
27    /// Compute the transposed differential of `inv(q1) * q2`.
28    fn diff_conj1_2_tr(&self, rhs: &Self) -> N::Matrix;
29    /// Compute the dot product of two quaternions.
30    fn dot(&self, rhs: &Self) -> N;
31    /// The imaginary part of the quaternion.
32    fn imag(&self) -> N::Vector;
33    /// Multiply this quaternion by a scalar without renormalizing.
34    fn mul_assign_unchecked(&mut self, rhs: N);
35}
36
37/// Trait implemented by 2D rotation types (UnitComplex).
38#[cfg(feature = "dim2")]
39pub trait RotationOps<N: ScalarType>:
40    Copy + Debug + Mul<N::Rotation, Output = N::Rotation> + Mul<N::Vector, Output = N::Vector>
41{
42    /// Converts this rotation to a rotation matrix.
43    fn to_mat(self) -> N::Matrix;
44    /// Returns the inverse of this rotation.
45    fn inverse(self) -> Self;
46    /// The imaginary part of the complex rotation.
47    fn imag(&self) -> N;
48    /// The angle of the rotation.
49    fn angle(&self) -> N;
50}
51
52#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))]
53impl RotationOps<SimdReal> for UnitQuaternion<SimdReal> {
54    #[inline]
55    fn to_mat(self) -> na::Matrix3<SimdReal> {
56        self.to_rotation_matrix().into_inner()
57    }
58
59    #[inline]
60    fn inverse(self) -> Self {
61        self.conjugate()
62    }
63
64    #[inline]
65    fn diff_conj1_2(&self, rhs: &Self) -> Matrix3<SimdReal> {
66        use crate::na::SimdValue;
67        let half = SimdReal::splat(0.5);
68        let v1 = self.imag();
69        let v2 = rhs.imag();
70        let w1 = self.w;
71        let w2 = rhs.w;
72
73        // TODO: this can probably be optimized a lot by unrolling the ops.
74        (v1 * v2.transpose() + Matrix3::from_diagonal_element(w1 * w2)
75            - (v1 * w2 + v2 * w1).cross_matrix()
76            + v1.cross_matrix() * v2.cross_matrix())
77            * half
78    }
79
80    #[inline]
81    fn diff_conj1_2_tr(&self, rhs: &Self) -> Matrix3<SimdReal> {
82        self.diff_conj1_2(rhs).transpose()
83    }
84
85    #[inline]
86    fn dot(&self, rhs: &Self) -> SimdReal {
87        self.coords.dot(&rhs.coords)
88    }
89
90    #[inline]
91    fn imag(&self) -> na::Vector3<SimdReal> {
92        (**self).imag()
93    }
94
95    #[inline]
96    fn mul_assign_unchecked(&mut self, rhs: SimdReal) {
97        *self.as_mut_unchecked() *= rhs;
98    }
99}
100
101#[cfg(feature = "dim3")]
102impl RotationOps<Real> for Rotation {
103    #[inline]
104    fn to_mat(self) -> Mat3 {
105        Matrix::from_quat(self)
106    }
107
108    #[inline]
109    fn inverse(self) -> Self {
110        self.inverse()
111    }
112
113    fn diff_conj1_2(&self, rhs: &Self) -> Mat3 {
114        use parry::math::VectorExt;
115
116        let half = 0.5;
117        let v1 = self.xyz();
118        let v2 = rhs.xyz();
119        let w1 = self.w;
120        let w2 = rhs.w;
121
122        // TODO: this can probably be optimized a lot by unrolling the ops.
123        (v1.kronecker(v2) + Matrix::from_diagonal(Vector::splat(w1 * w2))
124            - (v1 * w2 + v2 * w1).gcross_matrix()
125            + v1.gcross_matrix() * v2.gcross_matrix())
126            * half
127    }
128
129    #[inline]
130    fn diff_conj1_2_tr(&self, rhs: &Self) -> Mat3 {
131        self.diff_conj1_2(rhs).transpose()
132    }
133
134    #[inline]
135    fn dot(&self, rhs: &Self) -> Real {
136        (*self).dot(*rhs)
137    }
138
139    #[inline]
140    fn imag(&self) -> Vector {
141        self.xyz()
142    }
143
144    #[inline]
145    fn mul_assign_unchecked(&mut self, rhs: Real) {
146        *self *= rhs;
147    }
148}
149
150#[cfg(feature = "dim2")]
151impl RotationOps<Real> for Rotation {
152    #[inline]
153    fn to_mat(self) -> Matrix {
154        Matrix::from_cols(
155            Vector::new(self.re, self.im),
156            Vector::new(-self.im, self.re),
157        )
158    }
159
160    #[inline]
161    fn inverse(self) -> Self {
162        self.inverse()
163    }
164
165    #[inline]
166    fn imag(&self) -> Real {
167        self.im
168    }
169
170    #[inline]
171    fn angle(&self) -> Real {
172        (*self).angle()
173    }
174}
175
176#[cfg(all(feature = "dim2", feature = "simd-is-enabled"))]
177impl RotationOps<SimdReal> for na::UnitComplex<SimdReal> {
178    #[inline]
179    fn to_mat(self) -> na::Matrix2<SimdReal> {
180        self.to_rotation_matrix().into_inner()
181    }
182
183    #[inline]
184    fn inverse(self) -> Self {
185        self.conjugate()
186    }
187
188    #[inline]
189    fn imag(&self) -> SimdReal {
190        self.im
191    }
192
193    #[inline]
194    fn angle(&self) -> SimdReal {
195        (*self).angle()
196    }
197}
rapier2d/utils/rotation_ops.rs

rapier2d/utils/
rotation_ops.rs
