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 #[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
79struct 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}