parry2d/query/point/
point_support_map.rs

1use crate::math::{Pose, Vector};
2#[cfg(feature = "alloc")]
3use crate::query::epa::EPA;
4use crate::query::gjk::{self, ConstantOrigin, CsoPoint, VoronoiSimplex};
5use crate::query::{PointProjection, PointQuery};
6#[cfg(feature = "dim2")]
7#[cfg(feature = "alloc")]
8use crate::shape::ConvexPolygon;
9#[cfg(feature = "dim3")]
10#[cfg(feature = "alloc")]
11use crate::shape::ConvexPolyhedron;
12use crate::shape::{FeatureId, SupportMap};
13
14/// Projects a point on a shape using the GJK algorithm.
15pub fn local_point_projection_on_support_map<G>(
16    shape: &G,
17    simplex: &mut VoronoiSimplex,
18    point: Vector,
19    solid: bool,
20) -> PointProjection
21where
22    G: SupportMap,
23{
24    #[cfg(feature = "dim2")]
25    let m = Pose::new(-point, 0.0);
26    #[cfg(feature = "dim3")]
27    let m = Pose::new(-point, Vector::ZERO);
28    #[cfg(feature = "dim2")]
29    let m_inv = Pose::new(point, 0.0);
30    #[cfg(feature = "dim3")]
31    let m_inv = Pose::new(point, Vector::ZERO);
32    let dir = (-m.translation).try_normalize().unwrap_or(Vector::X);
33    let support_point = CsoPoint::from_shapes(&m_inv, shape, &ConstantOrigin, dir);
34
35    simplex.reset(support_point);
36
37    if let Some(proj) = gjk::project_origin(&m, shape, simplex) {
38        PointProjection::new(false, proj)
39    } else if solid {
40        PointProjection::new(true, point)
41    } else {
42        let mut epa = EPA::new();
43        if let Some(pt) = epa.project_origin(&m, shape, simplex) {
44            PointProjection::new(true, pt)
45        } else {
46            // return match minkowski_sampling::project_origin(&m, shape, simplex) {
47            //     Some(p) => PointProjection::new(true, p + point),
48            //     None => PointProjection::new(true, *point),
49            // };
50
51            //// All failed.
52            PointProjection::new(true, point)
53        }
54    }
55}
56
57#[cfg(feature = "dim3")]
58impl PointQuery for ConvexPolyhedron {
59    #[inline]
60    fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection {
61        local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), point, solid)
62    }
63
64    #[inline]
65    fn project_local_point_and_get_feature(&self, point: Vector) -> (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) = (local_dir).try_normalize() {
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: Vector, 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(&self, point: Vector) -> (PointProjection, FeatureId) {
88        let proj = self.project_local_point(point, false);
89        let dpt = point - proj.point;
90        let local_dir = if proj.is_inside { -dpt } else { dpt };
91
92        if let Some(local_dir) = (local_dir).try_normalize() {
93            let feature = self.support_feature_id_toward(local_dir);
94            (proj, feature)
95        } else {
96            (proj, FeatureId::Unknown)
97        }
98    }
99}
parry2d/query/point/point_support_map.rs

parry2d/query/point/
point_support_map.rs
