1use num::Zero;
2
3use simba::scalar::{RealField, SubsetOf, SupersetOf};
4use simba::simd::{PrimitiveSimdValue, SimdRealField, SimdValue};
5
6use crate::base::allocator::Allocator;
7use crate::base::dimension::{DimMin, DimNameAdd, DimNameSum, U1};
8use crate::base::{Const, DefaultAllocator, OMatrix, Scalar};
9
10use crate::geometry::{
11 AbstractRotation, Isometry, Similarity, SuperTCategoryOf, TAffine, Transform, Translation,
12};
13
14impl<T1, T2, R1, R2, const D: usize> SubsetOf<Similarity<T2, R2, D>> for Similarity<T1, R1, D>
24where
25 T1: RealField + SubsetOf<T2>,
26 T2: RealField + SupersetOf<T1>,
27 R1: AbstractRotation<T1, D> + SubsetOf<R2>,
28 R2: AbstractRotation<T2, D>,
29{
30 #[inline]
31 fn to_superset(&self) -> Similarity<T2, R2, D> {
32 Similarity::from_isometry(self.isometry.to_superset(), self.scaling().to_superset())
33 }
34
35 #[inline]
36 fn is_in_subset(sim: &Similarity<T2, R2, D>) -> bool {
37 crate::is_convertible::<_, Isometry<T1, R1, D>>(&sim.isometry)
38 && crate::is_convertible::<_, T1>(&sim.scaling())
39 }
40
41 #[inline]
42 fn from_superset_unchecked(sim: &Similarity<T2, R2, D>) -> Self {
43 Similarity::from_isometry(
44 sim.isometry.to_subset_unchecked(),
45 sim.scaling().to_subset_unchecked(),
46 )
47 }
48}
49
50impl<T1, T2, R, C, const D: usize> SubsetOf<Transform<T2, C, D>> for Similarity<T1, R, D>
51where
52 T1: RealField,
53 T2: RealField + SupersetOf<T1>,
54 C: SuperTCategoryOf<TAffine>,
55 R: AbstractRotation<T1, D>
56 + SubsetOf<OMatrix<T1, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>
57 + SubsetOf<OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>,
58 Const<D>: DimNameAdd<U1> + DimMin<Const<D>, Output = Const<D>>, DefaultAllocator: Allocator<T1, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
60 + Allocator<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
61 + Allocator<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>,
62 {
68 #[inline]
69 fn to_superset(&self) -> Transform<T2, C, D> {
70 Transform::from_matrix_unchecked(self.to_homogeneous().to_superset())
71 }
72
73 #[inline]
74 fn is_in_subset(t: &Transform<T2, C, D>) -> bool {
75 <Self as SubsetOf<_>>::is_in_subset(t.matrix())
76 }
77
78 #[inline]
79 fn from_superset_unchecked(t: &Transform<T2, C, D>) -> Self {
80 Self::from_superset_unchecked(t.matrix())
81 }
82}
83
84impl<T1, T2, R, const D: usize>
85 SubsetOf<OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>
86 for Similarity<T1, R, D>
87where
88 T1: RealField,
89 T2: RealField + SupersetOf<T1>,
90 R: AbstractRotation<T1, D>
91 + SubsetOf<OMatrix<T1, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>
92 + SubsetOf<OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>,
93 Const<D>: DimNameAdd<U1> + DimMin<Const<D>, Output = Const<D>>, DefaultAllocator: Allocator<T1, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
95 + Allocator<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
96 + Allocator<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>, {
102 #[inline]
103 fn to_superset(&self) -> OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>> {
104 self.to_homogeneous().to_superset()
105 }
106
107 #[inline]
108 fn is_in_subset(m: &OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>) -> bool {
109 let mut rot = m.fixed_slice::<D, D>(0, 0).clone_owned();
110 if rot
111 .fixed_columns_mut::<1>(0)
112 .try_normalize_mut(T2::zero())
113 .is_some()
114 && rot
115 .fixed_columns_mut::<1>(1)
116 .try_normalize_mut(T2::zero())
117 .is_some()
118 && rot
119 .fixed_columns_mut::<1>(2)
120 .try_normalize_mut(T2::zero())
121 .is_some()
122 {
123 if rot.determinant() < T2::zero() {
126 rot.fixed_columns_mut::<1>(0).neg_mut();
127 rot.fixed_columns_mut::<1>(1).neg_mut();
128 rot.fixed_columns_mut::<1>(2).neg_mut();
129 }
130
131 let bottom = m.fixed_slice::<1, D>(D, 0);
132 m.iter().all(|e| SupersetOf::<T1>::is_in_subset(e)) &&
134 bottom.iter().all(|e| e.is_zero()) && m[(D, D)] == T2::one()
138 } else {
139 false
140 }
141 }
142
143 #[inline]
144 fn from_superset_unchecked(
145 m: &OMatrix<T2, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>,
146 ) -> Self {
147 let mut mm = m.clone_owned();
148 let na = mm.fixed_slice_mut::<D, 1>(0, 0).normalize_mut();
149 let nb = mm.fixed_slice_mut::<D, 1>(0, 1).normalize_mut();
150 let nc = mm.fixed_slice_mut::<D, 1>(0, 2).normalize_mut();
151
152 let mut scale = (na + nb + nc) / crate::convert(3.0); if mm.fixed_slice::<D, D>(0, 0).determinant() < T2::zero() {
157 mm.fixed_slice_mut::<D, 1>(0, 0).neg_mut();
158 mm.fixed_slice_mut::<D, 1>(0, 1).neg_mut();
159 mm.fixed_slice_mut::<D, 1>(0, 2).neg_mut();
160 scale = -scale;
161 }
162
163 let t = m.fixed_slice::<D, 1>(0, D).into_owned();
164 let t = Translation {
165 vector: crate::convert_unchecked(t),
166 };
167
168 Self::from_parts(
169 t,
170 crate::convert_unchecked(mm),
171 crate::convert_unchecked(scale),
172 )
173 }
174}
175
176impl<T: SimdRealField, R, const D: usize> From<Similarity<T, R, D>>
177 for OMatrix<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
178where
179 Const<D>: DimNameAdd<U1>,
180 R: SubsetOf<OMatrix<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>,
181 DefaultAllocator: Allocator<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>, {
183 #[inline]
184 fn from(sim: Similarity<T, R, D>) -> Self {
185 sim.to_homogeneous()
186 }
187}
188
189impl<T: Scalar + Zero + PrimitiveSimdValue, R, const D: usize>
190 From<[Similarity<T::Element, R::Element, D>; 2]> for Similarity<T, R, D>
191where
192 T: From<[<T as SimdValue>::Element; 2]>,
193 R: SimdValue + AbstractRotation<T, D> + From<[<R as SimdValue>::Element; 2]>,
194 R::Element: AbstractRotation<T::Element, D>,
195 T::Element: Scalar + Zero + Copy,
196 R::Element: Scalar + Zero + Copy,
197{
198 #[inline]
199 fn from(arr: [Similarity<T::Element, R::Element, D>; 2]) -> Self {
200 let iso = Isometry::from([arr[0].isometry, arr[1].isometry]);
201 let scale = T::from([arr[0].scaling(), arr[1].scaling()]);
202
203 Self::from_isometry(iso, scale)
204 }
205}
206
207impl<T: Scalar + Zero + PrimitiveSimdValue, R, const D: usize>
208 From<[Similarity<T::Element, R::Element, D>; 4]> for Similarity<T, R, D>
209where
210 T: From<[<T as SimdValue>::Element; 4]>,
211 R: SimdValue + AbstractRotation<T, D> + From<[<R as SimdValue>::Element; 4]>,
212 R::Element: AbstractRotation<T::Element, D>,
213 T::Element: Scalar + Zero + Copy,
214 R::Element: Scalar + Zero + Copy,
215{
216 #[inline]
217 fn from(arr: [Similarity<T::Element, R::Element, D>; 4]) -> Self {
218 let iso = Isometry::from([
219 arr[0].isometry,
220 arr[1].isometry,
221 arr[2].isometry,
222 arr[3].isometry,
223 ]);
224 let scale = T::from([
225 arr[0].scaling(),
226 arr[1].scaling(),
227 arr[2].scaling(),
228 arr[3].scaling(),
229 ]);
230
231 Self::from_isometry(iso, scale)
232 }
233}
234
235impl<T: Scalar + Zero + PrimitiveSimdValue, R, const D: usize>
236 From<[Similarity<T::Element, R::Element, D>; 8]> for Similarity<T, R, D>
237where
238 T: From<[<T as SimdValue>::Element; 8]>,
239 R: SimdValue + AbstractRotation<T, D> + From<[<R as SimdValue>::Element; 8]>,
240 R::Element: AbstractRotation<T::Element, D>,
241 T::Element: Scalar + Zero + Copy,
242 R::Element: Scalar + Zero + Copy,
243{
244 #[inline]
245 fn from(arr: [Similarity<T::Element, R::Element, D>; 8]) -> Self {
246 let iso = Isometry::from([
247 arr[0].isometry,
248 arr[1].isometry,
249 arr[2].isometry,
250 arr[3].isometry,
251 arr[4].isometry,
252 arr[5].isometry,
253 arr[6].isometry,
254 arr[7].isometry,
255 ]);
256 let scale = T::from([
257 arr[0].scaling(),
258 arr[1].scaling(),
259 arr[2].scaling(),
260 arr[3].scaling(),
261 arr[4].scaling(),
262 arr[5].scaling(),
263 arr[6].scaling(),
264 arr[7].scaling(),
265 ]);
266
267 Self::from_isometry(iso, scale)
268 }
269}
270
271impl<T: Scalar + Zero + PrimitiveSimdValue, R, const D: usize>
272 From<[Similarity<T::Element, R::Element, D>; 16]> for Similarity<T, R, D>
273where
274 T: From<[<T as SimdValue>::Element; 16]>,
275 R: SimdValue + AbstractRotation<T, D> + From<[<R as SimdValue>::Element; 16]>,
276 R::Element: AbstractRotation<T::Element, D>,
277 T::Element: Scalar + Zero + Copy,
278 R::Element: Scalar + Zero + Copy,
279{
280 #[inline]
281 fn from(arr: [Similarity<T::Element, R::Element, D>; 16]) -> Self {
282 let iso = Isometry::from([
283 arr[0].isometry,
284 arr[1].isometry,
285 arr[2].isometry,
286 arr[3].isometry,
287 arr[4].isometry,
288 arr[5].isometry,
289 arr[6].isometry,
290 arr[7].isometry,
291 arr[8].isometry,
292 arr[9].isometry,
293 arr[10].isometry,
294 arr[11].isometry,
295 arr[12].isometry,
296 arr[13].isometry,
297 arr[14].isometry,
298 arr[15].isometry,
299 ]);
300 let scale = T::from([
301 arr[0].scaling(),
302 arr[1].scaling(),
303 arr[2].scaling(),
304 arr[3].scaling(),
305 arr[4].scaling(),
306 arr[5].scaling(),
307 arr[6].scaling(),
308 arr[7].scaling(),
309 arr[8].scaling(),
310 arr[9].scaling(),
311 arr[10].scaling(),
312 arr[11].scaling(),
313 arr[12].scaling(),
314 arr[13].scaling(),
315 arr[14].scaling(),
316 arr[15].scaling(),
317 ]);
318
319 Self::from_isometry(iso, scale)
320 }
321}