1use approx::{AbsDiffEq, RelativeEq, UlpsEq};
2use num::Zero;
3use std::fmt;
4use std::hash;
5
6#[cfg(feature = "abomonation-serialize")]
7use std::io::{Result as IOResult, Write};
8
9#[cfg(feature = "serde-serialize-no-std")]
10use serde::{Deserialize, Serialize};
11
12#[cfg(feature = "abomonation-serialize")]
13use abomonation::Abomonation;
14
15use simba::scalar::{RealField, SubsetOf};
16use simba::simd::SimdRealField;
17
18use crate::base::allocator::Allocator;
19use crate::base::dimension::{DimNameAdd, DimNameSum, U1};
20use crate::base::storage::Owned;
21use crate::base::{Const, DefaultAllocator, OMatrix, SVector, Scalar};
22use crate::geometry::{AbstractRotation, Isometry, Point, Translation};
23
24#[repr(C)]
26#[derive(Debug, Copy, Clone)]
27#[cfg_attr(
28 all(not(target_os = "cuda"), feature = "cuda"),
29 derive(cust::DeviceCopy)
30)]
31#[cfg_attr(feature = "serde-serialize-no-std", derive(Serialize, Deserialize))]
32#[cfg_attr(
33 feature = "serde-serialize-no-std",
34 serde(bound(serialize = "T: Scalar + Serialize,
35 R: Serialize,
36 DefaultAllocator: Allocator<T, Const<D>>,
37 Owned<T, Const<D>>: Serialize"))
38)]
39#[cfg_attr(
40 feature = "serde-serialize-no-std",
41 serde(bound(deserialize = "T: Scalar + Deserialize<'de>,
42 R: Deserialize<'de>,
43 DefaultAllocator: Allocator<T, Const<D>>,
44 Owned<T, Const<D>>: Deserialize<'de>"))
45)]
46pub struct Similarity<T, R, const D: usize> {
47 pub isometry: Isometry<T, R, D>,
49 scaling: T,
50}
51
52#[cfg(feature = "abomonation-serialize")]
53impl<T: Scalar, R, const D: usize> Abomonation for Similarity<T, R, D>
54where
55 Isometry<T, R, D>: Abomonation,
56{
57 unsafe fn entomb<W: Write>(&self, writer: &mut W) -> IOResult<()> {
58 self.isometry.entomb(writer)
59 }
60
61 fn extent(&self) -> usize {
62 self.isometry.extent()
63 }
64
65 unsafe fn exhume<'a, 'b>(&'a mut self, bytes: &'b mut [u8]) -> Option<&'b mut [u8]> {
66 self.isometry.exhume(bytes)
67 }
68}
69
70impl<T: Scalar + hash::Hash, R: hash::Hash, const D: usize> hash::Hash for Similarity<T, R, D>
71where
72 Owned<T, Const<D>>: hash::Hash,
73{
74 fn hash<H: hash::Hasher>(&self, state: &mut H) {
75 self.isometry.hash(state);
76 self.scaling.hash(state);
77 }
78}
79
80impl<T: Scalar + Zero, R, const D: usize> Similarity<T, R, D>
81where
82 R: AbstractRotation<T, D>,
83{
84 #[inline]
86 pub fn from_parts(translation: Translation<T, D>, rotation: R, scaling: T) -> Self {
87 Self::from_isometry(Isometry::from_parts(translation, rotation), scaling)
88 }
89
90 #[inline]
92 pub fn from_isometry(isometry: Isometry<T, R, D>, scaling: T) -> Self {
93 assert!(!scaling.is_zero(), "The scaling factor must not be zero.");
94
95 Self { isometry, scaling }
96 }
97
98 #[inline]
100 pub fn set_scaling(&mut self, scaling: T) {
101 assert!(
102 !scaling.is_zero(),
103 "The similarity scaling factor must not be zero."
104 );
105
106 self.scaling = scaling;
107 }
108}
109
110impl<T: Scalar, R, const D: usize> Similarity<T, R, D> {
111 #[inline]
113 #[must_use]
114 pub fn scaling(&self) -> T {
115 self.scaling.clone()
116 }
117}
118
119impl<T: SimdRealField, R, const D: usize> Similarity<T, R, D>
120where
121 T::Element: SimdRealField,
122 R: AbstractRotation<T, D>,
123{
124 #[inline]
126 pub fn from_scaling(scaling: T) -> Self {
127 Self::from_isometry(Isometry::identity(), scaling)
128 }
129
130 #[inline]
132 #[must_use = "Did you mean to use inverse_mut()?"]
133 pub fn inverse(&self) -> Self {
134 let mut res = self.clone();
135 res.inverse_mut();
136 res
137 }
138
139 #[inline]
141 pub fn inverse_mut(&mut self) {
142 self.scaling = T::one() / self.scaling.clone();
143 self.isometry.inverse_mut();
144 self.isometry.translation.vector *= self.scaling.clone();
145 }
146
147 #[inline]
149 #[must_use = "Did you mean to use prepend_scaling_mut()?"]
150 pub fn prepend_scaling(&self, scaling: T) -> Self {
151 assert!(
152 !scaling.is_zero(),
153 "The similarity scaling factor must not be zero."
154 );
155
156 Self::from_isometry(self.isometry.clone(), self.scaling.clone() * scaling)
157 }
158
159 #[inline]
161 #[must_use = "Did you mean to use append_scaling_mut()?"]
162 pub fn append_scaling(&self, scaling: T) -> Self {
163 assert!(
164 !scaling.is_zero(),
165 "The similarity scaling factor must not be zero."
166 );
167
168 Self::from_parts(
169 Translation::from(&self.isometry.translation.vector * scaling.clone()),
170 self.isometry.rotation.clone(),
171 self.scaling.clone() * scaling,
172 )
173 }
174
175 #[inline]
177 pub fn prepend_scaling_mut(&mut self, scaling: T) {
178 assert!(
179 !scaling.is_zero(),
180 "The similarity scaling factor must not be zero."
181 );
182
183 self.scaling *= scaling
184 }
185
186 #[inline]
188 pub fn append_scaling_mut(&mut self, scaling: T) {
189 assert!(
190 !scaling.is_zero(),
191 "The similarity scaling factor must not be zero."
192 );
193
194 self.isometry.translation.vector *= scaling.clone();
195 self.scaling *= scaling;
196 }
197
198 #[inline]
200 pub fn append_translation_mut(&mut self, t: &Translation<T, D>) {
201 self.isometry.append_translation_mut(t)
202 }
203
204 #[inline]
206 pub fn append_rotation_mut(&mut self, r: &R) {
207 self.isometry.append_rotation_mut(r)
208 }
209
210 #[inline]
213 pub fn append_rotation_wrt_point_mut(&mut self, r: &R, p: &Point<T, D>) {
214 self.isometry.append_rotation_wrt_point_mut(r, p)
215 }
216
217 #[inline]
220 pub fn append_rotation_wrt_center_mut(&mut self, r: &R) {
221 self.isometry.append_rotation_wrt_center_mut(r)
222 }
223
224 #[inline]
240 #[must_use]
241 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
242 self * pt
243 }
244
245 #[inline]
262 #[must_use]
263 pub fn transform_vector(&self, v: &SVector<T, D>) -> SVector<T, D> {
264 self * v
265 }
266
267 #[inline]
283 #[must_use]
284 pub fn inverse_transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
285 self.isometry.inverse_transform_point(pt) / self.scaling()
286 }
287
288 #[inline]
304 #[must_use]
305 pub fn inverse_transform_vector(&self, v: &SVector<T, D>) -> SVector<T, D> {
306 self.isometry.inverse_transform_vector(v) / self.scaling()
307 }
308}
309
310impl<T: SimdRealField, R, const D: usize> Similarity<T, R, D> {
315 #[inline]
317 #[must_use]
318 pub fn to_homogeneous(&self) -> OMatrix<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
319 where
320 Const<D>: DimNameAdd<U1>,
321 R: SubsetOf<OMatrix<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>,
322 DefaultAllocator: Allocator<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>,
323 {
324 let mut res = self.isometry.to_homogeneous();
325
326 for e in res.fixed_slice_mut::<D, D>(0, 0).iter_mut() {
327 *e *= self.scaling.clone()
328 }
329
330 res
331 }
332}
333
334impl<T: SimdRealField, R, const D: usize> Eq for Similarity<T, R, D> where
335 R: AbstractRotation<T, D> + Eq
336{
337}
338
339impl<T: SimdRealField, R, const D: usize> PartialEq for Similarity<T, R, D>
340where
341 R: AbstractRotation<T, D> + PartialEq,
342{
343 #[inline]
344 fn eq(&self, right: &Self) -> bool {
345 self.isometry == right.isometry && self.scaling == right.scaling
346 }
347}
348
349impl<T: RealField, R, const D: usize> AbsDiffEq for Similarity<T, R, D>
350where
351 R: AbstractRotation<T, D> + AbsDiffEq<Epsilon = T::Epsilon>,
352 T::Epsilon: Clone,
353{
354 type Epsilon = T::Epsilon;
355
356 #[inline]
357 fn default_epsilon() -> Self::Epsilon {
358 T::default_epsilon()
359 }
360
361 #[inline]
362 fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
363 self.isometry.abs_diff_eq(&other.isometry, epsilon.clone())
364 && self.scaling.abs_diff_eq(&other.scaling, epsilon)
365 }
366}
367
368impl<T: RealField, R, const D: usize> RelativeEq for Similarity<T, R, D>
369where
370 R: AbstractRotation<T, D> + RelativeEq<Epsilon = T::Epsilon>,
371 T::Epsilon: Clone,
372{
373 #[inline]
374 fn default_max_relative() -> Self::Epsilon {
375 T::default_max_relative()
376 }
377
378 #[inline]
379 fn relative_eq(
380 &self,
381 other: &Self,
382 epsilon: Self::Epsilon,
383 max_relative: Self::Epsilon,
384 ) -> bool {
385 self.isometry
386 .relative_eq(&other.isometry, epsilon.clone(), max_relative.clone())
387 && self
388 .scaling
389 .relative_eq(&other.scaling, epsilon, max_relative)
390 }
391}
392
393impl<T: RealField, R, const D: usize> UlpsEq for Similarity<T, R, D>
394where
395 R: AbstractRotation<T, D> + UlpsEq<Epsilon = T::Epsilon>,
396 T::Epsilon: Clone,
397{
398 #[inline]
399 fn default_max_ulps() -> u32 {
400 T::default_max_ulps()
401 }
402
403 #[inline]
404 fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
405 self.isometry
406 .ulps_eq(&other.isometry, epsilon.clone(), max_ulps)
407 && self.scaling.ulps_eq(&other.scaling, epsilon, max_ulps)
408 }
409}
410
411impl<T, R, const D: usize> fmt::Display for Similarity<T, R, D>
417where
418 T: RealField + fmt::Display,
419 R: AbstractRotation<T, D> + fmt::Display,
420{
421 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
422 let precision = f.precision().unwrap_or(3);
423
424 writeln!(f, "Similarity {{")?;
425 write!(f, "{:.*}", precision, self.isometry)?;
426 write!(f, "Scaling: {:.*}", precision, self.scaling)?;
427 writeln!(f, "}}")
428 }
429}