nalgebra/geometry/
similarity.rs

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/// A similarity, i.e., an uniform scaling, followed by a rotation, followed by a translation.
25#[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    /// The part of this similarity that does not include the scaling factor.
48    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    /// Creates a new similarity from its rotational and translational parts.
85    #[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    /// Creates a new similarity from its rotational and translational parts.
91    #[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    /// The scaling factor of this similarity transformation.
99    #[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    /// The scaling factor of this similarity transformation.
112    #[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    /// Creates a new similarity that applies only a scaling factor.
125    #[inline]
126    pub fn from_scaling(scaling: T) -> Self {
127        Self::from_isometry(Isometry::identity(), scaling)
128    }
129
130    /// Inverts `self`.
131    #[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    /// Inverts `self` in-place.
140    #[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    /// The similarity transformation that applies a scaling factor `scaling` before `self`.
148    #[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    /// The similarity transformation that applies a scaling factor `scaling` after `self`.
160    #[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    /// Sets `self` to the similarity transformation that applies a scaling factor `scaling` before `self`.
176    #[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    /// Sets `self` to the similarity transformation that applies a scaling factor `scaling` after `self`.
187    #[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    /// Appends to `self` the given translation in-place.
199    #[inline]
200    pub fn append_translation_mut(&mut self, t: &Translation<T, D>) {
201        self.isometry.append_translation_mut(t)
202    }
203
204    /// Appends to `self` the given rotation in-place.
205    #[inline]
206    pub fn append_rotation_mut(&mut self, r: &R) {
207        self.isometry.append_rotation_mut(r)
208    }
209
210    /// Appends in-place to `self` a rotation centered at the point `p`, i.e., the rotation that
211    /// lets `p` invariant.
212    #[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    /// Appends in-place to `self` a rotation centered at the point with coordinates
218    /// `self.translation`.
219    #[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    /// Transform the given point by this similarity.
225    ///
226    /// This is the same as the multiplication `self * pt`.
227    ///
228    /// # Example
229    /// ```
230    /// # #[macro_use] extern crate approx;
231    /// # use std::f32;
232    /// # use nalgebra::{Point3, Similarity3, Vector3};
233    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
234    /// let translation = Vector3::new(1.0, 2.0, 3.0);
235    /// let sim = Similarity3::new(translation, axisangle, 3.0);
236    /// let transformed_point = sim.transform_point(&Point3::new(4.0, 5.0, 6.0));
237    /// assert_relative_eq!(transformed_point, Point3::new(19.0, 17.0, -9.0), epsilon = 1.0e-5);
238    /// ```
239    #[inline]
240    #[must_use]
241    pub fn transform_point(&self, pt: &Point<T, D>) -> Point<T, D> {
242        self * pt
243    }
244
245    /// Transform the given vector by this similarity, ignoring the translational
246    /// component.
247    ///
248    /// This is the same as the multiplication `self * t`.
249    ///
250    /// # Example
251    /// ```
252    /// # #[macro_use] extern crate approx;
253    /// # use std::f32;
254    /// # use nalgebra::{Similarity3, Vector3};
255    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
256    /// let translation = Vector3::new(1.0, 2.0, 3.0);
257    /// let sim = Similarity3::new(translation, axisangle, 3.0);
258    /// let transformed_vector = sim.transform_vector(&Vector3::new(4.0, 5.0, 6.0));
259    /// assert_relative_eq!(transformed_vector, Vector3::new(18.0, 15.0, -12.0), epsilon = 1.0e-5);
260    /// ```
261    #[inline]
262    #[must_use]
263    pub fn transform_vector(&self, v: &SVector<T, D>) -> SVector<T, D> {
264        self * v
265    }
266
267    /// Transform the given point by the inverse of this similarity. This may
268    /// be cheaper than inverting the similarity and then transforming the
269    /// given point.
270    ///
271    /// # Example
272    /// ```
273    /// # #[macro_use] extern crate approx;
274    /// # use std::f32;
275    /// # use nalgebra::{Point3, Similarity3, Vector3};
276    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
277    /// let translation = Vector3::new(1.0, 2.0, 3.0);
278    /// let sim = Similarity3::new(translation, axisangle, 2.0);
279    /// let transformed_point = sim.inverse_transform_point(&Point3::new(4.0, 5.0, 6.0));
280    /// assert_relative_eq!(transformed_point, Point3::new(-1.5, 1.5, 1.5), epsilon = 1.0e-5);
281    /// ```
282    #[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    /// Transform the given vector by the inverse of this similarity,
289    /// ignoring the translational component. This may be cheaper than
290    /// inverting the similarity and then transforming the given vector.
291    ///
292    /// # Example
293    /// ```
294    /// # #[macro_use] extern crate approx;
295    /// # use std::f32;
296    /// # use nalgebra::{Similarity3, Vector3};
297    /// let axisangle = Vector3::y() * f32::consts::FRAC_PI_2;
298    /// let translation = Vector3::new(1.0, 2.0, 3.0);
299    /// let sim = Similarity3::new(translation, axisangle, 2.0);
300    /// let transformed_vector = sim.inverse_transform_vector(&Vector3::new(4.0, 5.0, 6.0));
301    /// assert_relative_eq!(transformed_vector, Vector3::new(-3.0, 2.5, 2.0), epsilon = 1.0e-5);
302    /// ```
303    #[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
310// NOTE: we don't require `R: Rotation<...>` here because this is not useful for the implementation
311// and makes it harder to use it, e.g., for Transform × Isometry implementation.
312// This is OK since all constructors of the isometry enforce the Rotation bound already (and
313// explicit struct construction is prevented by the private scaling factor).
314impl<T: SimdRealField, R, const D: usize> Similarity<T, R, D> {
315    /// Converts this similarity into its equivalent homogeneous transformation matrix.
316    #[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
411/*
412 *
413 * Display
414 *
415 */
416impl<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}