1 use alga::general::{RealField, SubsetOf, SupersetOf}; 2 use alga::linear::Rotation; 3 4 use crate::base::allocator::Allocator; 5 use crate::base::dimension::{DimMin, DimName, DimNameAdd, DimNameSum, U1}; 6 use crate::base::{DefaultAllocator, MatrixN}; 7 8 use crate::geometry::{Isometry, Point, Similarity, SuperTCategoryOf, TAffine, Transform, Translation}; 9 10 /* 11 * This file provides the following conversions: 12 * ============================================= 13 * 14 * Similarity -> Similarity 15 * Similarity -> Transform 16 * Similarity -> Matrix (homogeneous) 17 */ 18 19 impl<N1, N2, D: DimName, R1, R2> SubsetOf<Similarity<N2, D, R2>> for Similarity<N1, D, R1> 20 where 21 N1: RealField + SubsetOf<N2>, 22 N2: RealField + SupersetOf<N1>, 23 R1: Rotation<Point<N1, D>> + SubsetOf<R2>, 24 R2: Rotation<Point<N2, D>>, 25 DefaultAllocator: Allocator<N1, D> + Allocator<N2, D>, 26 { 27 #[inline] to_superset(&self) -> Similarity<N2, D, R2>28 fn to_superset(&self) -> Similarity<N2, D, R2> { 29 Similarity::from_isometry(self.isometry.to_superset(), self.scaling().to_superset()) 30 } 31 32 #[inline] is_in_subset(sim: &Similarity<N2, D, R2>) -> bool33 fn is_in_subset(sim: &Similarity<N2, D, R2>) -> bool { 34 crate::is_convertible::<_, Isometry<N1, D, R1>>(&sim.isometry) 35 && crate::is_convertible::<_, N1>(&sim.scaling()) 36 } 37 38 #[inline] from_superset_unchecked(sim: &Similarity<N2, D, R2>) -> Self39 unsafe fn from_superset_unchecked(sim: &Similarity<N2, D, R2>) -> Self { 40 Similarity::from_isometry( 41 sim.isometry.to_subset_unchecked(), 42 sim.scaling().to_subset_unchecked(), 43 ) 44 } 45 } 46 47 impl<N1, N2, D, R, C> SubsetOf<Transform<N2, D, C>> for Similarity<N1, D, R> 48 where 49 N1: RealField, 50 N2: RealField + SupersetOf<N1>, 51 C: SuperTCategoryOf<TAffine>, 52 R: Rotation<Point<N1, D>> 53 + SubsetOf<MatrixN<N1, DimNameSum<D, U1>>> 54 + SubsetOf<MatrixN<N2, DimNameSum<D, U1>>>, 55 D: DimNameAdd<U1> + DimMin<D, Output = D>, // needed by .determinant() 56 DefaultAllocator: Allocator<N1, D> 57 + Allocator<N1, D, D> 58 + Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>> 59 + Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>> 60 + Allocator<(usize, usize), D> 61 + Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>> 62 + Allocator<N2, D, D> 63 + Allocator<N2, D>, 64 { 65 #[inline] to_superset(&self) -> Transform<N2, D, C>66 fn to_superset(&self) -> Transform<N2, D, C> { 67 Transform::from_matrix_unchecked(self.to_homogeneous().to_superset()) 68 } 69 70 #[inline] is_in_subset(t: &Transform<N2, D, C>) -> bool71 fn is_in_subset(t: &Transform<N2, D, C>) -> bool { 72 <Self as SubsetOf<_>>::is_in_subset(t.matrix()) 73 } 74 75 #[inline] from_superset_unchecked(t: &Transform<N2, D, C>) -> Self76 unsafe fn from_superset_unchecked(t: &Transform<N2, D, C>) -> Self { 77 Self::from_superset_unchecked(t.matrix()) 78 } 79 } 80 81 impl<N1, N2, D, R> SubsetOf<MatrixN<N2, DimNameSum<D, U1>>> for Similarity<N1, D, R> 82 where 83 N1: RealField, 84 N2: RealField + SupersetOf<N1>, 85 R: Rotation<Point<N1, D>> 86 + SubsetOf<MatrixN<N1, DimNameSum<D, U1>>> 87 + SubsetOf<MatrixN<N2, DimNameSum<D, U1>>>, 88 D: DimNameAdd<U1> + DimMin<D, Output = D>, // needed by .determinant() 89 DefaultAllocator: Allocator<N1, D> 90 + Allocator<N1, D, D> 91 + Allocator<N1, DimNameSum<D, U1>, DimNameSum<D, U1>> 92 + Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>> 93 + Allocator<(usize, usize), D> 94 + Allocator<N2, DimNameSum<D, U1>, DimNameSum<D, U1>> 95 + Allocator<N2, D, D> 96 + Allocator<N2, D>, 97 { 98 #[inline] to_superset(&self) -> MatrixN<N2, DimNameSum<D, U1>>99 fn to_superset(&self) -> MatrixN<N2, DimNameSum<D, U1>> { 100 self.to_homogeneous().to_superset() 101 } 102 103 #[inline] is_in_subset(m: &MatrixN<N2, DimNameSum<D, U1>>) -> bool104 fn is_in_subset(m: &MatrixN<N2, DimNameSum<D, U1>>) -> bool { 105 let mut rot = m.fixed_slice::<D, D>(0, 0).clone_owned(); 106 if rot 107 .fixed_columns_mut::<U1>(0) 108 .try_normalize_mut(N2::zero()) 109 .is_some() 110 && rot 111 .fixed_columns_mut::<U1>(1) 112 .try_normalize_mut(N2::zero()) 113 .is_some() 114 && rot 115 .fixed_columns_mut::<U1>(2) 116 .try_normalize_mut(N2::zero()) 117 .is_some() 118 { 119 // FIXME: could we avoid explicit the computation of the determinant? 120 // (its sign is needed to see if the scaling factor is negative). 121 if rot.determinant() < N2::zero() { 122 rot.fixed_columns_mut::<U1>(0).neg_mut(); 123 rot.fixed_columns_mut::<U1>(1).neg_mut(); 124 rot.fixed_columns_mut::<U1>(2).neg_mut(); 125 } 126 127 let bottom = m.fixed_slice::<U1, D>(D::dim(), 0); 128 // Scalar types agree. 129 m.iter().all(|e| SupersetOf::<N1>::is_in_subset(e)) && 130 // The normalized block part is a rotation. 131 // rot.is_special_orthogonal(N2::default_epsilon().sqrt()) && 132 // The bottom row is (0, 0, ..., 1) 133 bottom.iter().all(|e| e.is_zero()) && m[(D::dim(), D::dim())] == N2::one() 134 } else { 135 false 136 } 137 } 138 139 #[inline] from_superset_unchecked(m: &MatrixN<N2, DimNameSum<D, U1>>) -> Self140 unsafe fn from_superset_unchecked(m: &MatrixN<N2, DimNameSum<D, U1>>) -> Self { 141 let mut mm = m.clone_owned(); 142 let na = mm.fixed_slice_mut::<D, U1>(0, 0).normalize_mut(); 143 let nb = mm.fixed_slice_mut::<D, U1>(0, 1).normalize_mut(); 144 let nc = mm.fixed_slice_mut::<D, U1>(0, 2).normalize_mut(); 145 146 let mut scale = (na + nb + nc) / crate::convert(3.0); // We take the mean, for robustness. 147 148 // FIXME: could we avoid the explicit computation of the determinant? 149 // (its sign is needed to see if the scaling factor is negative). 150 if mm.fixed_slice::<D, D>(0, 0).determinant() < N2::zero() { 151 mm.fixed_slice_mut::<D, U1>(0, 0).neg_mut(); 152 mm.fixed_slice_mut::<D, U1>(0, 1).neg_mut(); 153 mm.fixed_slice_mut::<D, U1>(0, 2).neg_mut(); 154 scale = -scale; 155 } 156 157 let t = m.fixed_slice::<D, U1>(0, D::dim()).into_owned(); 158 let t = Translation { 159 vector: crate::convert_unchecked(t), 160 }; 161 162 Self::from_parts(t, crate::convert_unchecked(mm), crate::convert_unchecked(scale)) 163 } 164 } 165 166 impl<N: RealField, D: DimName, R> From<Similarity<N, D, R>> for MatrixN<N, DimNameSum<D, U1>> 167 where 168 D: DimNameAdd<U1>, 169 R: SubsetOf<MatrixN<N, DimNameSum<D, U1>>>, 170 DefaultAllocator: Allocator<N, D> + Allocator<N, DimNameSum<D, U1>, DimNameSum<D, U1>>, 171 { 172 #[inline] from(sim: Similarity<N, D, R>) -> Self173 fn from(sim: Similarity<N, D, R>) -> Self { 174 sim.to_homogeneous() 175 } 176 } 177