1#![cfg_attr(feature = "rkyv-serialize", allow(unsafe_op_in_unsafe_fn))]
3
4use approx::{AbsDiffEq, RelativeEq, UlpsEq};
5use num::Zero;
6use std::fmt;
7use std::hash;
8
9#[cfg(feature = "serde-serialize-no-std")]
10use serde::{Deserialize, Serialize};
11
12use simba::scalar::{RealField, SubsetOf};
13use simba::simd::SimdRealField;
14
15use crate::base::allocator::Allocator;
16use crate::base::dimension::{DimNameAdd, DimNameSum, U1};
17use crate::base::storage::Owned;
18use crate::base::{Const, DefaultAllocator, OMatrix, SVector, Scalar};
19use crate::geometry::{AbstractRotation, Isometry, Point, Translation};
20
21#[cfg(feature = "rkyv-serialize")]
22use rkyv::bytecheck;
23
24#[repr(C)]
26#[derive(Debug, Copy, Clone)]
27#[cfg_attr(feature = "serde-serialize-no-std", derive(Serialize, Deserialize))]
28#[cfg_attr(
29 feature = "serde-serialize-no-std",
30 serde(bound(serialize = "T: Scalar + Serialize,
31 R: Serialize,
32 DefaultAllocator: Allocator<Const<D>>,
33 Owned<T, Const<D>>: Serialize"))
34)]
35#[cfg_attr(
36 feature = "serde-serialize-no-std",
37 serde(bound(deserialize = "T: Scalar + Deserialize<'de>,
38 R: Deserialize<'de>,
39 DefaultAllocator: Allocator<Const<D>>,
40 Owned<T, Const<D>>: Deserialize<'de>"))
41)]
42#[cfg_attr(feature = "rkyv-serialize", derive(bytecheck::CheckBytes))]
43#[cfg_attr(
44 feature = "rkyv-serialize-no-std",
45 derive(rkyv::Archive, rkyv::Serialize, rkyv::Deserialize),
46 archive(
47 as = "Similarity<T::Archived, R::Archived, D>",
48 bound(archive = "
49 T: rkyv::Archive,
50 R: rkyv::Archive,
51 Isometry<T, R, D>: rkyv::Archive<Archived = Isometry<T::Archived, R::Archived, D>>
52 ")
53 )
54)]
55#[cfg_attr(feature = "defmt", derive(defmt::Format))]
56pub struct Similarity<T, R, const D: usize> {
57 pub isometry: Isometry<T, R, D>,
59 scaling: T,
60}
61
62impl<T: Scalar + hash::Hash, R: hash::Hash, const D: usize> hash::Hash for Similarity<T, R, D>
63where
64 Owned<T, Const<D>>: hash::Hash,
65{
66 fn hash<H: hash::Hasher>(&self, state: &mut H) {
67 self.isometry.hash(state);
68 self.scaling.hash(state);
69 }
70}
71
72#[cfg(feature = "bytemuck")]
73unsafe impl<T: Scalar, R, const D: usize> bytemuck::Zeroable for Similarity<T, R, D> where
74 Isometry<T, R, D>: bytemuck::Zeroable
75{
76}
77
78#[cfg(feature = "bytemuck")]
79unsafe impl<T: Scalar, R, const D: usize> bytemuck::Pod for Similarity<T, R, D>
80where
81 Isometry<T, R, D>: bytemuck::Pod,
82 R: Copy,
83 T: Copy,
84{
85}
86
87impl<T: Scalar + Zero, R, const D: usize> Similarity<T, R, D>
88where
89 R: AbstractRotation<T, D>,
90{
91 #[inline]
93 pub fn from_parts(translation: Translation<T, D>, rotation: R, scaling: T) -> Self {
94 Self::from_isometry(Isometry::from_parts(translation, rotation), scaling)
95 }
96
97 #[inline]
99 pub fn from_isometry(isometry: Isometry<T, R, D>, scaling: T) -> Self {
100 assert!(!scaling.is_zero(), "The scaling factor must not be zero.");
101
102 Self { isometry, scaling }
103 }
104
105 #[inline]
107 pub fn set_scaling(&mut self, scaling: T) {
108 assert!(
109 !scaling.is_zero(),
110 "The similarity scaling factor must not be zero."
111 );
112
113 self.scaling = scaling;
114 }
115}
116
117impl<T: Scalar, R, const D: usize> Similarity<T, R, D> {
118 #[inline]
120 #[must_use]
121 pub fn scaling(&self) -> T {
122 self.scaling.clone()
123 }
124}
125
126impl<T: SimdRealField, R, const D: usize> Similarity<T, R, D>
127where
128 T::Element: SimdRealField,
129 R: AbstractRotation<T, D>,
130{
131 #[inline]
133 pub fn from_scaling(scaling: T) -> Self {
134 Self::from_isometry(Isometry::identity(), scaling)
135 }
136
137 #[inline]
139 #[must_use = "Did you mean to use inverse_mut()?"]
140 pub fn inverse(&self) -> Self {
141 let mut res = self.clone();
142 res.inverse_mut();
143 res
144 }
145
146 #[inline]
148 pub fn inverse_mut(&mut self) {
149 self.scaling = T::one() / self.scaling.clone();
150 self.isometry.inverse_mut();
151 self.isometry.translation.vector *= self.scaling.clone();
152 }
153
154 #[inline]
156 #[must_use = "Did you mean to use prepend_scaling_mut()?"]
157 pub fn prepend_scaling(&self, scaling: T) -> Self {
158 assert!(
159 !scaling.is_zero(),
160 "The similarity scaling factor must not be zero."
161 );
162
163 Self::from_isometry(self.isometry.clone(), self.scaling.clone() * scaling)
164 }
165
166 #[inline]
168 #[must_use = "Did you mean to use append_scaling_mut()?"]
169 pub fn append_scaling(&self, scaling: T) -> Self {
170 assert!(
171 !scaling.is_zero(),
172 "The similarity scaling factor must not be zero."
173 );
174
175 Self::from_parts(
176 Translation::from(&self.isometry.translation.vector * scaling.clone()),
177 self.isometry.rotation.clone(),
178 self.scaling.clone() * scaling,
179 )
180 }
181
182 #[inline]
184 pub fn prepend_scaling_mut(&mut self, scaling: T) {
185 assert!(
186 !scaling.is_zero(),
187 "The similarity scaling factor must not be zero."
188 );
189
190 self.scaling *= scaling
191 }
192
193 #[inline]
195 pub fn append_scaling_mut(&mut self, scaling: T) {
196 assert!(
197 !scaling.is_zero(),
198 "The similarity scaling factor must not be zero."
199 );
200
201 self.isometry.translation.vector *= scaling.clone();
202 self.scaling *= scaling;
203 }
204
205 #[inline]
207 pub fn append_translation_mut(&mut self, t: &Translation<T, D>) {
208 self.isometry.append_translation_mut(t)
209 }
210
211 #[inline]
213 pub fn append_rotation_mut(&mut self, r: &R) {
214 self.isometry.append_rotation_mut(r)
215 }
216
217 #[inline]
220 pub fn append_rotation_wrt_point_mut(&mut self, r: &R, p: &Point<T, D>) {
221 self.isometry.append_rotation_wrt_point_mut(r, p)
222 }
223
224 #[inline]
227 pub fn append_rotation_wrt_center_mut(&mut self, r: &R) {
228 self.isometry.append_rotation_wrt_center_mut(r)
229 }
230
231 #[inline]
247 #[must_use]
248 pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
249 self * pt
250 }
251
252 #[inline]
269 #[must_use]
270 pub fn transform_vector(&self, v: &SVector<T, D>) -> SVector<T, D> {
271 self * v
272 }
273
274 #[inline]
290 #[must_use]
291 pub fn inverse_transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
292 self.isometry.inverse_transform_point(pt) / self.scaling()
293 }
294
295 #[inline]
311 #[must_use]
312 pub fn inverse_transform_vector(&self, v: &SVector<T, D>) -> SVector<T, D> {
313 self.isometry.inverse_transform_vector(v) / self.scaling()
314 }
315}
316
317impl<T: SimdRealField, R, const D: usize> Similarity<T, R, D> {
322 #[inline]
324 #[must_use]
325 pub fn to_homogeneous(&self) -> OMatrix<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>
326 where
327 Const<D>: DimNameAdd<U1>,
328 R: SubsetOf<OMatrix<T, DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>>,
329 DefaultAllocator: Allocator<DimNameSum<Const<D>, U1>, DimNameSum<Const<D>, U1>>,
330 {
331 let mut res = self.isometry.to_homogeneous();
332
333 for e in res.fixed_view_mut::<D, D>(0, 0).iter_mut() {
334 *e *= self.scaling.clone()
335 }
336
337 res
338 }
339}
340
341impl<T: SimdRealField, R, const D: usize> Eq for Similarity<T, R, D> where
342 R: AbstractRotation<T, D> + Eq
343{
344}
345
346impl<T: SimdRealField, R, const D: usize> PartialEq for Similarity<T, R, D>
347where
348 R: AbstractRotation<T, D> + PartialEq,
349{
350 #[inline]
351 fn eq(&self, right: &Self) -> bool {
352 self.isometry == right.isometry && self.scaling == right.scaling
353 }
354}
355
356impl<T: RealField, R, const D: usize> AbsDiffEq for Similarity<T, R, D>
357where
358 R: AbstractRotation<T, D> + AbsDiffEq<Epsilon = T::Epsilon>,
359 T::Epsilon: Clone,
360{
361 type Epsilon = T::Epsilon;
362
363 #[inline]
364 fn default_epsilon() -> Self::Epsilon {
365 T::default_epsilon()
366 }
367
368 #[inline]
369 fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
370 self.isometry.abs_diff_eq(&other.isometry, epsilon.clone())
371 && self.scaling.abs_diff_eq(&other.scaling, epsilon)
372 }
373}
374
375impl<T: RealField, R, const D: usize> RelativeEq for Similarity<T, R, D>
376where
377 R: AbstractRotation<T, D> + RelativeEq<Epsilon = T::Epsilon>,
378 T::Epsilon: Clone,
379{
380 #[inline]
381 fn default_max_relative() -> Self::Epsilon {
382 T::default_max_relative()
383 }
384
385 #[inline]
386 fn relative_eq(
387 &self,
388 other: &Self,
389 epsilon: Self::Epsilon,
390 max_relative: Self::Epsilon,
391 ) -> bool {
392 self.isometry
393 .relative_eq(&other.isometry, epsilon.clone(), max_relative.clone())
394 && self
395 .scaling
396 .relative_eq(&other.scaling, epsilon, max_relative)
397 }
398}
399
400impl<T: RealField, R, const D: usize> UlpsEq for Similarity<T, R, D>
401where
402 R: AbstractRotation<T, D> + UlpsEq<Epsilon = T::Epsilon>,
403 T::Epsilon: Clone,
404{
405 #[inline]
406 fn default_max_ulps() -> u32 {
407 T::default_max_ulps()
408 }
409
410 #[inline]
411 fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool {
412 self.isometry
413 .ulps_eq(&other.isometry, epsilon.clone(), max_ulps)
414 && self.scaling.ulps_eq(&other.scaling, epsilon, max_ulps)
415 }
416}
417
418impl<T, R, const D: usize> fmt::Display for Similarity<T, R, D>
424where
425 T: RealField + fmt::Display,
426 R: AbstractRotation<T, D> + fmt::Display,
427{
428 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
429 let precision = f.precision().unwrap_or(3);
430
431 writeln!(f, "Similarity {{")?;
432 write!(f, "{:.*}", precision, self.isometry)?;
433 write!(f, "Scaling: {:.*}", precision, self.scaling)?;
434 writeln!(f, "}}")
435 }
436}