ncollide3d/query/point/
point_support_map.rs

Help
1use na::{RealField, Unit};
2
3use crate::math::{Isometry, Point, Translation, Vector};
4use crate::query::algorithms::{
5    gjk, special_support_maps::ConstantOrigin, CSOPoint, VoronoiSimplex, EPA,
6};
7use crate::query::{PointProjection, PointQuery};
8#[cfg(feature = "dim2")]
9use crate::shape::ConvexPolygon;
10#[cfg(feature = "dim3")]
11use crate::shape::{Cone, ConvexHull, Cylinder};
12use crate::shape::{ConvexPolyhedron, FeatureId, SupportMap};
13
14/// Projects a point on a shape using the GJK algorithm.
15pub fn point_projection_on_support_map<N, G>(
16    m: &Isometry<N>,
17    shape: &G,
18    simplex: &mut VoronoiSimplex<N>,
19    point: &Point<N>,
20    solid: bool,
21) -> PointProjection<N>
22where
23    N: RealField + Copy,
24    G: SupportMap<N>,
25{
26    let id = Isometry::identity();
27    let m = Translation::from(-point.coords) * m;
28
29    let dir =
30        Unit::try_new(-m.translation.vector, N::default_epsilon()).unwrap_or(Vector::x_axis());
31    let support_point = CSOPoint::from_shapes(&m, shape, &id, &ConstantOrigin, &dir);
32
33    simplex.reset(support_point);
34
35    if let Some(proj) = gjk::project_origin(&m, shape, simplex) {
36        PointProjection::new(false, proj + point.coords)
37    } else if solid {
38        PointProjection::new(true, *point)
39    } else {
40        let mut epa = EPA::new();
41        if let Some(pt) = epa.project_origin(&m, shape, simplex) {
42            return PointProjection::new(true, pt + point.coords);
43        } else {
44            // return match minkowski_sampling::project_origin(&m, shape, simplex) {
45            //     Some(p) => PointProjection::new(true, p + point.coords),
46            //     None => PointProjection::new(true, *point),
47            // };
48
49            //// All failed.
50            PointProjection::new(true, *point)
51        }
52    }
53}
54
55#[cfg(feature = "dim3")]
56impl<N: RealField + Copy> PointQuery<N> for Cylinder<N> {
57    #[inline]
58    fn project_point(&self, m: &Isometry<N>, point: &Point<N>, solid: bool) -> PointProjection<N> {
59        point_projection_on_support_map(m, self, &mut VoronoiSimplex::new(), point, solid)
60    }
61
62    #[inline]
63    fn project_point_with_feature(
64        &self,
65        m: &Isometry<N>,
66        point: &Point<N>,
67    ) -> (PointProjection<N>, FeatureId) {
68        (self.project_point(m, point, false), FeatureId::Unknown)
69    }
70}
71
72#[cfg(feature = "dim3")]
73impl<N: RealField + Copy> PointQuery<N> for Cone<N> {
74    #[inline]
75    fn project_point(&self, m: &Isometry<N>, point: &Point<N>, solid: bool) -> PointProjection<N> {
76        point_projection_on_support_map(m, self, &mut VoronoiSimplex::new(), point, solid)
77    }
78
79    #[inline]
80    fn project_point_with_feature(
81        &self,
82        m: &Isometry<N>,
83        point: &Point<N>,
84    ) -> (PointProjection<N>, FeatureId) {
85        (self.project_point(m, point, false), FeatureId::Unknown)
86    }
87}
88
89#[cfg(feature = "dim3")]
90impl<N: RealField + Copy> PointQuery<N> for ConvexHull<N> {
91    #[inline]
92    fn project_point(&self, m: &Isometry<N>, point: &Point<N>, solid: bool) -> PointProjection<N> {
93        point_projection_on_support_map(m, self, &mut VoronoiSimplex::new(), point, solid)
94    }
95
96    #[inline]
97    fn project_point_with_feature(
98        &self,
99        m: &Isometry<N>,
100        point: &Point<N>,
101    ) -> (PointProjection<N>, FeatureId) {
102        let proj = self.project_point(m, point, false);
103        let dpt = *point - proj.point;
104        let local_dir = if proj.is_inside {
105            m.inverse_transform_vector(&-dpt)
106        } else {
107            m.inverse_transform_vector(&dpt)
108        };
109
110        if let Some(local_dir) = Unit::try_new(local_dir, N::default_epsilon()) {
111            let feature = ConvexPolyhedron::<N>::support_feature_id_toward(self, &local_dir);
112            (proj, feature)
113        } else {
114            (proj, FeatureId::Unknown)
115        }
116    }
117}
118
119#[cfg(feature = "dim2")]
120impl<N: RealField + Copy> PointQuery<N> for ConvexPolygon<N> {
121    #[inline]
122    fn project_point(&self, m: &Isometry<N>, point: &Point<N>, solid: bool) -> PointProjection<N> {
123        point_projection_on_support_map(m, self, &mut VoronoiSimplex::new(), point, solid)
124    }
125
126    #[inline]
127    fn project_point_with_feature(
128        &self,
129        m: &Isometry<N>,
130        point: &Point<N>,
131    ) -> (PointProjection<N>, FeatureId) {
132        let proj = self.project_point(m, point, false);
133        let dpt = *point - proj.point;
134        let local_dir = if proj.is_inside {
135            m.inverse_transform_vector(&-dpt)
136        } else {
137            m.inverse_transform_vector(&dpt)
138        };
139
140        if let Some(local_dir) = Unit::try_new(local_dir, N::default_epsilon()) {
141            let feature = self.support_feature_id_toward(&local_dir);
142            (proj, feature)
143        } else {
144            (proj, FeatureId::Unknown)
145        }
146    }
147}
ncollide3d/query/point/point_support_map.rs

ncollide3d/query/point/
point_support_map.rs
