parry2d/utils/
isometry_ops.rs

1use crate::math::{Isometry, Point, Real, Vector};
2use na::Unit;
3
4#[cfg(feature = "simd-is-enabled")]
5use crate::math::SimdReal;
6
7/// Extra operations with isometries.
8pub trait IsometryOps<T> {
9    /// Transform a vector by the absolute value of the homogeneous matrix
10    /// equivalent to `self`.
11    fn absolute_transform_vector(&self, v: &Vector<T>) -> Vector<T>;
12}
13
14impl IsometryOps<Real> for Isometry<Real> {
15    #[inline]
16    fn absolute_transform_vector(&self, v: &Vector<Real>) -> Vector<Real> {
17        self.rotation.to_rotation_matrix().into_inner().abs() * *v
18    }
19}
20
21#[cfg(feature = "simd-is-enabled")]
22impl IsometryOps<SimdReal> for Isometry<SimdReal> {
23    #[inline]
24    fn absolute_transform_vector(&self, v: &Vector<SimdReal>) -> Vector<SimdReal> {
25        use na::SimdComplexField;
26        self.rotation
27            .to_rotation_matrix()
28            .into_inner()
29            .map(|e| e.simd_abs())
30            * *v
31    }
32}
33
34/// Various operations usable with `Option<Isometry>` and `Option<&Isometry>`
35/// where `None` is assumed to be equivalent to the identity.
36pub trait IsometryOpt {
37    /// Computes `self.inverse() * rhs`.
38    fn inv_mul(self, rhs: &Isometry<Real>) -> Isometry<Real>;
39    /// Computes `rhs * self`.
40    fn prepend_to(self, rhs: &Isometry<Real>) -> Isometry<Real>;
41    /// Computes `self * p`.
42    fn transform_point(self, p: &Point<Real>) -> Point<Real>;
43    /// Computes `self * v`.
44    fn transform_vector(self, v: &Vector<Real>) -> Vector<Real>;
45    /// Computes `self * v`.
46    fn transform_unit_vector(self, v: &Unit<Vector<Real>>) -> Unit<Vector<Real>>;
47    /// Computes `self.inverse() * p`.
48    fn inverse_transform_point(self, p: &Point<Real>) -> Point<Real>;
49    /// Computes `self.inverse() * v`.
50    fn inverse_transform_vector(self, v: &Vector<Real>) -> Vector<Real>;
51    /// Computes `self.inverse() * v`.
52    fn inverse_transform_unit_vector(self, v: &Unit<Vector<Real>>) -> Unit<Vector<Real>>;
53}
54
55impl IsometryOpt for Option<&Isometry<Real>> {
56    #[inline]
57    fn inv_mul(self, rhs: &Isometry<Real>) -> Isometry<Real> {
58        if let Some(iso) = self {
59            iso.inv_mul(rhs)
60        } else {
61            *rhs
62        }
63    }
64
65    #[inline]
66    fn prepend_to(self, rhs: &Isometry<Real>) -> Isometry<Real> {
67        if let Some(iso) = self {
68            rhs * iso
69        } else {
70            *rhs
71        }
72    }
73
74    #[inline]
75    fn transform_point(self, p: &Point<Real>) -> Point<Real> {
76        if let Some(iso) = self {
77            iso * p
78        } else {
79            *p
80        }
81    }
82
83    #[inline]
84    fn transform_vector(self, v: &Vector<Real>) -> Vector<Real> {
85        if let Some(iso) = self {
86            iso * v
87        } else {
88            *v
89        }
90    }
91
92    #[inline]
93    fn transform_unit_vector(self, v: &Unit<Vector<Real>>) -> Unit<Vector<Real>> {
94        if let Some(iso) = self {
95            iso * v
96        } else {
97            *v
98        }
99    }
100
101    #[inline]
102    fn inverse_transform_point(self, p: &Point<Real>) -> Point<Real> {
103        if let Some(iso) = self {
104            iso.inverse_transform_point(p)
105        } else {
106            *p
107        }
108    }
109
110    #[inline]
111    fn inverse_transform_vector(self, v: &Vector<Real>) -> Vector<Real> {
112        if let Some(iso) = self {
113            iso.inverse_transform_vector(v)
114        } else {
115            *v
116        }
117    }
118
119    #[inline]
120    fn inverse_transform_unit_vector(self, v: &Unit<Vector<Real>>) -> Unit<Vector<Real>> {
121        if let Some(iso) = self {
122            iso.inverse_transform_unit_vector(v)
123        } else {
124            *v
125        }
126    }
127}
128
129impl IsometryOpt for Option<Isometry<Real>> {
130    #[inline]
131    fn inv_mul(self, rhs: &Isometry<Real>) -> Isometry<Real> {
132        if let Some(iso) = self {
133            iso.inv_mul(rhs)
134        } else {
135            *rhs
136        }
137    }
138
139    #[inline]
140    fn prepend_to(self, rhs: &Isometry<Real>) -> Isometry<Real> {
141        if let Some(iso) = self {
142            rhs * iso
143        } else {
144            *rhs
145        }
146    }
147
148    #[inline]
149    fn transform_point(self, p: &Point<Real>) -> Point<Real> {
150        if let Some(iso) = self {
151            iso * p
152        } else {
153            *p
154        }
155    }
156
157    #[inline]
158    fn transform_vector(self, v: &Vector<Real>) -> Vector<Real> {
159        if let Some(iso) = self {
160            iso * v
161        } else {
162            *v
163        }
164    }
165
166    #[inline]
167    fn transform_unit_vector(self, v: &Unit<Vector<Real>>) -> Unit<Vector<Real>> {
168        if let Some(iso) = self {
169            iso * v
170        } else {
171            *v
172        }
173    }
174
175    #[inline]
176    fn inverse_transform_point(self, p: &Point<Real>) -> Point<Real> {
177        if let Some(iso) = self {
178            iso.inverse_transform_point(p)
179        } else {
180            *p
181        }
182    }
183
184    #[inline]
185    fn inverse_transform_vector(self, v: &Vector<Real>) -> Vector<Real> {
186        if let Some(iso) = self {
187            iso.inverse_transform_vector(v)
188        } else {
189            *v
190        }
191    }
192
193    #[inline]
194    fn inverse_transform_unit_vector(self, v: &Unit<Vector<Real>>) -> Unit<Vector<Real>> {
195        if let Some(iso) = self {
196            iso.inverse_transform_unit_vector(v)
197        } else {
198            *v
199        }
200    }
201}
parry2d/utils/isometry_ops.rs

parry2d/utils/
isometry_ops.rs
