parry3d/query/point/
point_support_map.rs

1use na::Unit;
2
3use crate::math::{Isometry, Point, Real, Vector};
4#[cfg(feature = "alloc")]
5use crate::query::epa::EPA;
6use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex};
7use crate::query::{PointProjection, PointQuery};
8#[cfg(feature = "dim2")]
9#[cfg(feature = "alloc")]
10use crate::shape::ConvexPolygon;
11#[cfg(feature = "dim3")]
12#[cfg(feature = "alloc")]
13use crate::shape::ConvexPolyhedron;
14use crate::shape::{FeatureId, SupportMap};
15
16/// Projects a point on a shape using the GJK algorithm.
17pub fn local_point_projection_on_support_map<G>(
18    shape: &G,
19    simplex: &mut VoronoiSimplex,
20    point: &Point<Real>,
21    solid: bool,
22) -> PointProjection
23where
24    G: SupportMap,
25{
26    let m = Isometry::new(-point.coords, na::zero());
27    let m_inv = Isometry::new(point.coords, na::zero());
28    let dir = Unit::try_new(-m.translation.vector, crate::math::DEFAULT_EPSILON)
29        .unwrap_or(Vector::x_axis());
30    let support_point = CSOPoint::from_shapes(&m_inv, shape, &ConstantOrigin, &dir);
31
32    simplex.reset(support_point);
33
34    if let Some(proj) = gjk::project_origin(&m, shape, simplex) {
35        PointProjection::new(false, proj)
36    } else if solid {
37        PointProjection::new(true, *point)
38    } else {
39        let mut epa = EPA::new();
40        if let Some(pt) = epa.project_origin(&m, shape, simplex) {
41            return PointProjection::new(true, pt);
42        } else {
43            // return match minkowski_sampling::project_origin(&m, shape, simplex) {
44            //     Some(p) => PointProjection::new(true, p + point.coords),
45            //     None => PointProjection::new(true, *point),
46            // };
47
48            //// All failed.
49            PointProjection::new(true, *point)
50        }
51    }
52}
53
54#[cfg(feature = "dim3")]
55impl PointQuery for ConvexPolyhedron {
56    #[inline]
57    fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
58        local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid)
59    }
60
61    #[inline]
62    fn project_local_point_and_get_feature(
63        &self,
64        point: &Point<Real>,
65    ) -> (PointProjection, FeatureId) {
66        let proj = self.project_local_point(point, false);
67        let dpt = *point - proj.point;
68        let local_dir = if proj.is_inside { -dpt } else { dpt };
69
70        if let Some(local_dir) = Unit::try_new(local_dir, crate::math::DEFAULT_EPSILON) {
71            let feature = self.support_feature_id_toward(&local_dir);
72            (proj, feature)
73        } else {
74            (proj, FeatureId::Unknown)
75        }
76    }
77}
78
79#[cfg(feature = "dim2")]
80impl PointQuery for ConvexPolygon {
81    #[inline]
82    fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
83        local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid)
84    }
85
86    #[inline]
87    fn project_local_point_and_get_feature(
88        &self,
89        point: &Point<Real>,
90    ) -> (PointProjection, FeatureId) {
91        let proj = self.project_local_point(point, false);
92        let dpt = *point - proj.point;
93        let local_dir = if proj.is_inside { -dpt } else { dpt };
94
95        if let Some(local_dir) = Unit::try_new(local_dir, crate::math::DEFAULT_EPSILON) {
96            let feature = self.support_feature_id_toward(&local_dir);
97            (proj, feature)
98        } else {
99            (proj, FeatureId::Unknown)
100        }
101    }
102}
parry3d/query/point/point_support_map.rs

parry3d/query/point/
point_support_map.rs
