parry3d/utils/
isometry_ops.rs

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

parry3d/utils/
isometry_ops.rs
