parry3d/utils/
isometry_ops.rs

1use crate::math::{Pose, Vector};
2
3/// Extra operations with isometries.
4pub trait PoseOps {
5    /// Transform a vector by the absolute value of the homogeneous matrix
6    /// equivalent to `self`.
7    // TODO: move this to the rotation
8    fn absolute_transform_vector(&self, v: Vector) -> Vector;
9}
10
11impl PoseOps for Pose {
12    #[inline]
13    fn absolute_transform_vector(&self, v: Vector) -> Vector {
14        #[cfg(feature = "dim3")]
15        {
16            let rot_matrix = crate::math::Matrix::from_quat(self.rotation);
17            rot_matrix.abs() * v
18        }
19        #[cfg(feature = "dim2")]
20        {
21            let rot_matrix = self.rotation.to_mat();
22            rot_matrix.abs() * v
23        }
24    }
25}
26
27/// Various operations usable with `Option<Pose>` and `Option<&Pose>`
28/// where `None` is assumed to be equivalent to the identity.
29pub trait PoseOpt {
30    /// Computes `self.inverse() * rhs`.
31    fn inv_mul(self, rhs: &Pose) -> Pose;
32    /// Computes `rhs * self`.
33    fn prepend_to(self, rhs: &Pose) -> Pose;
34    /// Computes `self * p`.
35    fn transform_point(self, p: Vector) -> Vector;
36    /// Computes `self.inverse() * p`.
37    fn inverse_transform_point(self, p: Vector) -> Vector;
38}
39
40impl PoseOpt for Option<&Pose> {
41    #[inline]
42    fn inv_mul(self, rhs: &Pose) -> Pose {
43        if let Some(iso) = self {
44            iso.inv_mul(rhs)
45        } else {
46            *rhs
47        }
48    }
49
50    #[inline]
51    fn prepend_to(self, rhs: &Pose) -> Pose {
52        if let Some(iso) = self {
53            *rhs * *iso
54        } else {
55            *rhs
56        }
57    }
58
59    #[inline]
60    fn transform_point(self, p: Vector) -> Vector {
61        if let Some(iso) = self {
62            iso * p
63        } else {
64            p
65        }
66    }
67
68    #[inline]
69    fn inverse_transform_point(self, p: Vector) -> Vector {
70        if let Some(iso) = self {
71            iso.inverse_transform_point(p)
72        } else {
73            p
74        }
75    }
76}
77
78impl PoseOpt for Option<Pose> {
79    #[inline]
80    fn inv_mul(self, rhs: &Pose) -> Pose {
81        if let Some(iso) = self {
82            iso.inv_mul(rhs)
83        } else {
84            *rhs
85        }
86    }
87
88    #[inline]
89    fn prepend_to(self, rhs: &Pose) -> Pose {
90        if let Some(iso) = self {
91            *rhs * iso
92        } else {
93            *rhs
94        }
95    }
96
97    #[inline]
98    fn transform_point(self, p: Vector) -> Vector {
99        if let Some(iso) = self {
100            iso * p
101        } else {
102            p
103        }
104    }
105
106    #[inline]
107    fn inverse_transform_point(self, p: Vector) -> Vector {
108        if let Some(iso) = self {
109            iso.inverse_transform_point(p)
110        } else {
111            p
112        }
113    }
114}
parry3d/utils/isometry_ops.rs

parry3d/utils/
isometry_ops.rs
