nalgebra/geometry/
rotation_conversion.rs

1use num::Zero;
2
3use crate::ArrayStorage;
4use simba::scalar::{RealField, SubsetOf, SupersetOf};
5use simba::simd::{PrimitiveSimdValue, SimdValue};
6
7use crate::base::allocator::Allocator;
8use crate::base::dimension::{DimMin, DimNameAdd, DimNameSum, U1};
9use crate::base::{Const, DefaultAllocator, Matrix2, Matrix3, Matrix4, OMatrix, SMatrix, Scalar};
10
11use crate::geometry::{
12    AbstractRotation, Isometry, Rotation, Rotation2, Rotation3, Similarity, SuperTCategoryOf,
13    TAffine, Transform, Translation, UnitComplex, UnitDualQuaternion, UnitQuaternion,
14};
15
16/*
17 * This file provides the following conversions:
18 * =============================================
19 *
20 * Rotation  -> Rotation
21 * Rotation3 -> UnitQuaternion
22 * Rotation3 -> UnitDualQuaternion
23 * Rotation2 -> UnitComplex
24 * Rotation  -> Isometry
25 * Rotation  -> Similarity
26 * Rotation  -> Transform
27 * Rotation  -> Matrix (homogeneous)
28
29*/
30
31impl<T1, T2, const D: usize> SubsetOf<Rotation<T2, D>> for Rotation<T1, D>
32where
33    T1: RealField,
34    T2: RealField + SupersetOf<T1>,
35{
36    #[inline]
37    fn to_superset(&self) -> Rotation<T2, D> {
38        Rotation::from_matrix_unchecked(self.matrix().to_superset())
39    }
40
41    #[inline]
42    fn is_in_subset(rot: &Rotation<T2, D>) -> bool {
43        crate::is_convertible::<_, SMatrix<T1, D, D>>(rot.matrix())
44    }
45
46    #[inline]
47    fn from_superset_unchecked(rot: &Rotation<T2, D>) -> Self {
48        Rotation::from_matrix_unchecked(rot.matrix().to_subset_unchecked())
49    }
50}
51
52impl<T1, T2> SubsetOf<UnitQuaternion<T2>> for Rotation3<T1>
53where
54    T1: RealField,
55    T2: RealField + SupersetOf<T1>,
56{
57    #[inline]
58    fn to_superset(&self) -> UnitQuaternion<T2> {
59        let q = UnitQuaternion::<T1>::from_rotation_matrix(self);
60        q.to_superset()
61    }
62
63    #[inline]
64    fn is_in_subset(q: &UnitQuaternion<T2>) -> bool {
65        crate::is_convertible::<_, UnitQuaternion<T1>>(q)
66    }
67
68    #[inline]
69    fn from_superset_unchecked(q: &UnitQuaternion<T2>) -> Self {
70        let q: UnitQuaternion<T1> = crate::convert_ref_unchecked(q);
71        q.to_rotation_matrix()
72    }
73}
74
75impl<T1, T2> SubsetOf<UnitDualQuaternion<T2>> for Rotation3<T1>
76where
77    T1: RealField,
78    T2: RealField + SupersetOf<T1>,
79{
80    #[inline]
81    fn to_superset(&self) -> UnitDualQuaternion<T2> {
82        let q = UnitQuaternion::<T1>::from_rotation_matrix(self);
83        let dq = UnitDualQuaternion::from_rotation(q);
84        dq.to_superset()
85    }
86
87    #[inline]
88    fn is_in_subset(dq: &UnitDualQuaternion<T2>) -> bool {
89        crate::is_convertible::<_, UnitQuaternion<T1>>(&dq.rotation())
90            && dq.translation().vector.is_zero()
91    }
92
93    #[inline]
94    fn from_superset_unchecked(dq: &UnitDualQuaternion<T2>) -> Self {
95        let dq: UnitDualQuaternion<T1> = crate::convert_ref_unchecked(dq);
96        dq.rotation().to_rotation_matrix()
97    }
98}
99
100impl<T1, T2> SubsetOf<UnitComplex<T2>> for Rotation2<T1>
101where
102    T1: RealField,
103    T2: RealField + SupersetOf<T1>,
104{
105    #[inline]
106    fn to_superset(&self) -> UnitComplex<T2> {
107        let q = UnitComplex::<T1>::from_rotation_matrix(self);
108        q.to_superset()
109    }
110
111    #[inline]
112    fn is_in_subset(q: &UnitComplex<T2>) -> bool {
113        crate::is_convertible::<_, UnitComplex<T1>>(q)
114    }
115
116    #[inline]
117    fn from_superset_unchecked(q: &UnitComplex<T2>) -> Self {
118        let q: UnitComplex<T1> = crate::convert_ref_unchecked(q);
119        q.to_rotation_matrix()
120    }
121}
122
123impl<T1, T2, R, const D: usize> SubsetOf<Isometry<T2, R, D>> for Rotation<T1, D>
124where
125    T1: RealField,
126    T2: RealField + SupersetOf<T1>,
127    R: AbstractRotation<T2, D> + SupersetOf<Self>,
128{
129    #[inline]
130    fn to_superset(&self) -> Isometry<T2, R, D> {
131        Isometry::from_parts(Translation::identity(), crate::convert_ref(self))
132    }
133
134    #[inline]
135    fn is_in_subset(iso: &Isometry<T2, R, D>) -> bool {
136        iso.translation.vector.is_zero()
137    }
138
139    #[inline]
140    fn from_superset_unchecked(iso: &Isometry<T2, R, D>) -> Self {
141        crate::convert_ref_unchecked(&iso.rotation)
142    }
143}
144
145impl<T1, T2, R, const D: usize> SubsetOf<Similarity<T2, R, D>> for Rotation<T1, D>
146where
147    T1: RealField,
148    T2: RealField + SupersetOf<T1>,
149    R: AbstractRotation<T2, D> + SupersetOf<Self>,
150{
151    #[inline]
152    fn to_superset(&self) -> Similarity<T2, R, D> {
153        Similarity::from_parts(Translation::identity(), crate::convert_ref(self), T2::one())
154    }
155
156    #[inline]
157    fn is_in_subset(sim: &Similarity<T2, R, D>) -> bool {
158        sim.isometry.translation.vector.is_zero() && sim.scaling() == T2::one()
159    }
160
161    #[inline]
162    fn from_superset_unchecked(sim: &Similarity<T2, R, D>) -> Self {
163        crate::convert_ref_unchecked(&sim.isometry.rotation)
164    }
165}
166
167impl<T1, T2, C, const D: usize> SubsetOf<Transform<T2, C, D>> for Rotation<T1, D>
168where
169    T1: RealField,
170    T2: RealField + SupersetOf<T1>,
171    C: SuperTCategoryOf<TAffine>,
172    Const<D>: DimNameAdd<U1> + DimMin<Const<D>, Output = Const<D>>, // needed by .is_special_orthogonal()
173    DefaultAllocator: Allocator<DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>,
174    // + Allocator<D>,
175    //     + Allocator<D, D>
176{
177    // needed by .is_special_orthogonal()
178    #[inline]
179    fn to_superset(&self) -> Transform<T2, C, D> {
180        Transform::from_matrix_unchecked(self.to_homogeneous().to_superset())
181    }
182
183    #[inline]
184    fn is_in_subset(t: &Transform<T2, C, D>) -> bool {
185        <Self as SubsetOf<_>>::is_in_subset(t.matrix())
186    }
187
188    #[inline]
189    fn from_superset_unchecked(t: &Transform<T2, C, D>) -> Self {
190        Self::from_superset_unchecked(t.matrix())
191    }
192}
193
194impl<T1, T2, const D: usize>
195    SubsetOf<OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>> for Rotation<T1, D>
196where
197    T1: RealField,
198    T2: RealField + SupersetOf<T1>,
199    Const<D>: DimNameAdd<U1> + DimMin<Const<D>, Output = Const<D>>, // needed by .is_special_orthogonal()
200    DefaultAllocator: Allocator<Const<D>, Const<D>, Buffer<T1> = ArrayStorage<T1, D, D>>
201        + Allocator<DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>, // + Allocator<D>,
202                                                                    // + Allocator<D, D>
203{
204    // needed by .is_special_orthogonal()
205    #[inline]
206    fn to_superset(&self) -> OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>> {
207        self.to_homogeneous().to_superset()
208    }
209
210    #[inline]
211    fn is_in_subset(m: &OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>) -> bool {
212        let rot = m.fixed_view::<D, D>(0, 0);
213        let bottom = m.fixed_view::<1, D>(D, 0);
214
215        // Scalar types agree.
216        m.iter().all(|e| SupersetOf::<T1>::is_in_subset(e)) &&
217        // The block part is a rotation.
218        rot.is_special_orthogonal(T2::default_epsilon() * crate::convert(100.0)) &&
219        // The bottom row is (0, 0, ..., 1)
220        bottom.iter().all(|e| e.is_zero()) && m[(D, D)] == T2::one()
221    }
222
223    #[inline]
224    fn from_superset_unchecked(
225        m: &OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>,
226    ) -> Self {
227        let r = m.fixed_view::<D, D>(0, 0);
228        Self::from_matrix_unchecked(crate::convert_unchecked(r.into_owned()))
229    }
230}
231
232impl<T: RealField> From<Rotation2<T>> for Matrix3<T> {
233    #[inline]
234    fn from(q: Rotation2<T>) -> Self {
235        q.to_homogeneous()
236    }
237}
238
239impl<T: RealField> From<Rotation2<T>> for Matrix2<T> {
240    #[inline]
241    fn from(q: Rotation2<T>) -> Self {
242        q.into_inner()
243    }
244}
245
246impl<T: RealField> From<Rotation3<T>> for Matrix4<T> {
247    #[inline]
248    fn from(q: Rotation3<T>) -> Self {
249        q.to_homogeneous()
250    }
251}
252
253impl<T: RealField> From<Rotation3<T>> for Matrix3<T> {
254    #[inline]
255    fn from(q: Rotation3<T>) -> Self {
256        q.into_inner()
257    }
258}
259
260impl<T: Scalar + PrimitiveSimdValue, const D: usize> From<[Rotation<T::Element, D>; 2]>
261    for Rotation<T, D>
262where
263    T: From<[<T as SimdValue>::Element; 2]>,
264    T::Element: Scalar + Copy,
265{
266    #[inline]
267    fn from(arr: [Rotation<T::Element, D>; 2]) -> Self {
268        Self::from_matrix_unchecked(OMatrix::from([arr[0].into_inner(), arr[1].into_inner()]))
269    }
270}
271
272impl<T: Scalar + PrimitiveSimdValue, const D: usize> From<[Rotation<T::Element, D>; 4]>
273    for Rotation<T, D>
274where
275    T: From<[<T as SimdValue>::Element; 4]>,
276    T::Element: Scalar + Copy,
277{
278    #[inline]
279    fn from(arr: [Rotation<T::Element, D>; 4]) -> Self {
280        Self::from_matrix_unchecked(OMatrix::from([
281            arr[0].into_inner(),
282            arr[1].into_inner(),
283            arr[2].into_inner(),
284            arr[3].into_inner(),
285        ]))
286    }
287}
288
289impl<T: Scalar + PrimitiveSimdValue, const D: usize> From<[Rotation<T::Element, D>; 8]>
290    for Rotation<T, D>
291where
292    T: From<[<T as SimdValue>::Element; 8]>,
293    T::Element: Scalar + Copy,
294{
295    #[inline]
296    fn from(arr: [Rotation<T::Element, D>; 8]) -> Self {
297        Self::from_matrix_unchecked(OMatrix::from([
298            arr[0].into_inner(),
299            arr[1].into_inner(),
300            arr[2].into_inner(),
301            arr[3].into_inner(),
302            arr[4].into_inner(),
303            arr[5].into_inner(),
304            arr[6].into_inner(),
305            arr[7].into_inner(),
306        ]))
307    }
308}
309
310impl<T: Scalar + PrimitiveSimdValue, const D: usize> From<[Rotation<T::Element, D>; 16]>
311    for Rotation<T, D>
312where
313    T: From<[<T as SimdValue>::Element; 16]>,
314    T::Element: Scalar + Copy,
315{
316    #[inline]
317    fn from(arr: [Rotation<T::Element, D>; 16]) -> Self {
318        Self::from_matrix_unchecked(OMatrix::from([
319            arr[0].into_inner(),
320            arr[1].into_inner(),
321            arr[2].into_inner(),
322            arr[3].into_inner(),
323            arr[4].into_inner(),
324            arr[5].into_inner(),
325            arr[6].into_inner(),
326            arr[7].into_inner(),
327            arr[8].into_inner(),
328            arr[9].into_inner(),
329            arr[10].into_inner(),
330            arr[11].into_inner(),
331            arr[12].into_inner(),
332            arr[13].into_inner(),
333            arr[14].into_inner(),
334            arr[15].into_inner(),
335        ]))
336    }
337}
nalgebra/geometry/rotation_conversion.rs

nalgebra/geometry/
rotation_conversion.rs
