rapier2d/utils/
rotation_ops.rs

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

rapier2d/utils/
rotation_ops.rs
