ncollide3d/query/point/
point_trimesh.rs

1use crate::bounding_volume::AABB;
2use crate::math::{Isometry, Point};
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
4use crate::query::{
5    visitors::CompositePointContainmentTest, PointProjection, PointQuery, PointQueryWithLocation,
6};
7use crate::shape::{CompositeShape, FeatureId, TriMesh, TrianglePointLocation};
8use na::{self, RealField};
9
10impl<N: RealField + Copy> PointQuery<N> for TriMesh<N> {
11    #[inline]
12    fn project_point(&self, m: &Isometry<N>, point: &Point<N>, solid: bool) -> PointProjection<N> {
13        let (projection, _) = self.project_point_with_location(m, point, solid);
14        projection
15    }
16
17    #[inline]
18    fn project_point_with_feature(
19        &self,
20        m: &Isometry<N>,
21        point: &Point<N>,
22    ) -> (PointProjection<N>, FeatureId) {
23        let (projection, (triangle_id, location)) =
24            self.project_point_with_location(m, point, false);
25        let face = &self.faces()[triangle_id];
26        let feature_id = match location {
27            TrianglePointLocation::OnVertex(triangle_local_id) => {
28                FeatureId::Vertex(face.indices[triangle_local_id])
29            }
30            TrianglePointLocation::OnEdge(triangle_local_id, _) => {
31                FeatureId::Edge(face.edges[triangle_local_id])
32            }
33            TrianglePointLocation::OnFace(_, _) => FeatureId::Face(triangle_id),
34            TrianglePointLocation::OnSolid => FeatureId::Unknown,
35        };
36        (projection, feature_id)
37    }
38
39    // FIXME: implement distance_to_point too?
40
41    #[inline]
42    fn contains_point(&self, m: &Isometry<N>, point: &Point<N>) -> bool {
43        let ls_pt = m.inverse_transform_point(point);
44        let mut visitor = CompositePointContainmentTest {
45            shape: self,
46            point: &ls_pt,
47            found: false,
48        };
49
50        self.bvh().visit(&mut visitor);
51
52        visitor.found
53    }
54}
55
56impl<N: RealField + Copy> PointQueryWithLocation<N> for TriMesh<N> {
57    type Location = (usize, TrianglePointLocation<N>);
58
59    #[inline]
60    fn project_point_with_location(
61        &self,
62        m: &Isometry<N>,
63        point: &Point<N>,
64        _: bool,
65    ) -> (PointProjection<N>, Self::Location) {
66        let ls_pt = m.inverse_transform_point(point);
67        let mut visitor = TriMeshPointProjVisitor {
68            polyline: self,
69            point: &ls_pt,
70        };
71
72        let (mut proj, extra_info) = self.bvh().best_first_search(&mut visitor).unwrap().1;
73        proj.point = m * proj.point;
74
75        (proj, extra_info)
76    }
77}
78
79/*
80 * Visitors
81 */
82struct TriMeshPointProjVisitor<'a, N: 'a + RealField + Copy> {
83    polyline: &'a TriMesh<N>,
84    point: &'a Point<N>,
85}
86
87impl<'a, N: RealField + Copy> BestFirstVisitor<N, usize, AABB<N>>
88    for TriMeshPointProjVisitor<'a, N>
89{
90    type Result = (PointProjection<N>, (usize, TrianglePointLocation<N>));
91
92    #[inline]
93    fn visit(
94        &mut self,
95        best: N,
96        aabb: &AABB<N>,
97        data: Option<&usize>,
98    ) -> BestFirstVisitStatus<N, Self::Result> {
99        let dist = aabb.distance_to_point(&Isometry::identity(), self.point, true);
100
101        let mut res = BestFirstVisitStatus::Continue {
102            cost: dist,
103            result: None,
104        };
105
106        if let Some(b) = data {
107            if dist < best {
108                let (proj, extra_info) = self.polyline.triangle_at(*b).project_point_with_location(
109                    &Isometry::identity(),
110                    self.point,
111                    true,
112                );
113
114                let extra_info = (*b, extra_info);
115                res = BestFirstVisitStatus::Continue {
116                    cost: na::distance(self.point, &proj.point),
117                    result: Some((proj, extra_info)),
118                };
119            }
120        }
121
122        res
123    }
124}