parry2d/query/point/
point_composite_shape.rs

1#![allow(unused_parens)] // Needed by the macro.
2
3use crate::math::{Point, Real};
4use crate::partitioning::BvhNode;
5use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
6use crate::shape::{
7    CompositeShapeRef, FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation,
8    TypedCompositeShape,
9};
10use na;
11
12use crate::shape::{Compound, Polyline};
13
14impl<S: TypedCompositeShape> CompositeShapeRef<'_, S> {
15    /// Project a point on this composite shape.
16    ///
17    /// Returns the projected point as well as the index of the sub-shape of `self` that was hit.
18    /// The third tuple element contains some shape-specific information about the projected point.
19    #[inline]
20    pub fn project_local_point_and_get_location(
21        &self,
22        point: &Point<Real>,
23        max_dist: Real,
24        solid: bool,
25    ) -> Option<(
26        u32,
27        (
28            PointProjection,
29            <S::PartShape as PointQueryWithLocation>::Location,
30        ),
31    )>
32    where
33        S::PartShape: PointQueryWithLocation,
34    {
35        self.0
36            .bvh()
37            .find_best(
38                max_dist,
39                |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true),
40                |primitive, _best_so_far| {
41                    let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| {
42                        if let Some(pose) = pose {
43                            shape.project_point_and_get_location(pose, point, solid)
44                        } else {
45                            shape.project_local_point_and_get_location(point, solid)
46                        }
47                    })?;
48                    let cost = na::distance(&proj.0.point, point);
49                    Some((cost, proj))
50                },
51            )
52            .map(|(best_id, (_, (proj, location)))| (best_id, (proj, location)))
53    }
54
55    /// Project a point on this composite shape.
56    ///
57    /// Returns the projected point as well as the index of the sub-shape of `self` that was hit.
58    /// If `solid` is `false` then the point will be projected to the closest boundary of `self` even
59    /// if it is contained by one of its sub-shapes.
60    pub fn project_local_point(&self, point: &Point<Real>, solid: bool) -> (u32, PointProjection) {
61        let (best_id, (_, proj)) = self
62            .0
63            .bvh()
64            .find_best(
65                Real::MAX,
66                |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true),
67                |primitive, _best_so_far| {
68                    let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| {
69                        if let Some(pose) = pose {
70                            shape.project_point(pose, point, solid)
71                        } else {
72                            shape.project_local_point(point, solid)
73                        }
74                    })?;
75                    let dist = na::distance(&proj.point, point);
76                    Some((dist, proj))
77                },
78            )
79            .unwrap();
80        (best_id, proj)
81    }
82
83    /// Project a point on this composite shape.
84    ///
85    /// Returns the projected point as well as the index of the sub-shape of `self` that was hit.
86    /// The third tuple element contains some shape-specific information about the shape feature
87    /// hit by the projection.
88    #[inline]
89    pub fn project_local_point_and_get_feature(
90        &self,
91        point: &Point<Real>,
92    ) -> (u32, (PointProjection, FeatureId)) {
93        let (best_id, (_, (proj, feature_id))) = self
94            .0
95            .bvh()
96            .find_best(
97                Real::MAX,
98                |node: &BvhNode, _best_so_far| node.aabb().distance_to_local_point(point, true),
99                |primitive, _best_so_far| {
100                    let proj = self.0.map_typed_part_at(primitive, |pose, shape, _| {
101                        if let Some(pose) = pose {
102                            shape.project_point_and_get_feature(pose, point)
103                        } else {
104                            shape.project_local_point_and_get_feature(point)
105                        }
106                    })?;
107                    let cost = na::distance(&proj.0.point, point);
108                    Some((cost, proj))
109                },
110            )
111            .unwrap();
112        (best_id, (proj, feature_id))
113    }
114
115    // TODO: implement distance_to_point too?
116
117    /// Returns the index of any sub-shape of `self` that contains the given point.
118    #[inline]
119    pub fn contains_local_point(&self, point: &Point<Real>) -> Option<u32> {
120        self.0
121            .bvh()
122            .leaves(|node: &BvhNode| node.aabb().contains_local_point(point))
123            .find(|leaf_id| {
124                self.0
125                    .map_typed_part_at(*leaf_id, |pose, shape, _| {
126                        if let Some(pose) = pose {
127                            shape.contains_point(pose, point)
128                        } else {
129                            shape.contains_local_point(point)
130                        }
131                    })
132                    .unwrap_or(false)
133            })
134    }
135}
136
137impl PointQuery for Polyline {
138    #[inline]
139    fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
140        CompositeShapeRef(self).project_local_point(point, solid).1
141    }
142
143    #[inline]
144    fn project_local_point_and_get_feature(
145        &self,
146        point: &Point<Real>,
147    ) -> (PointProjection, FeatureId) {
148        let (seg_id, (proj, feature)) =
149            CompositeShapeRef(self).project_local_point_and_get_feature(point);
150        let polyline_feature = self.segment_feature_to_polyline_feature(seg_id, feature);
151        (proj, polyline_feature)
152    }
153
154    // TODO: implement distance_to_point too?
155
156    #[inline]
157    fn contains_local_point(&self, point: &Point<Real>) -> bool {
158        CompositeShapeRef(self)
159            .contains_local_point(point)
160            .is_some()
161    }
162}
163
164impl PointQuery for TriMesh {
165    #[inline]
166    fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
167        CompositeShapeRef(self).project_local_point(point, solid).1
168    }
169
170    #[inline]
171    fn project_local_point_and_get_feature(
172        &self,
173        point: &Point<Real>,
174    ) -> (PointProjection, FeatureId) {
175        #[cfg(feature = "dim3")]
176        if self.pseudo_normals().is_some() {
177            // If we can, in 3D, take the pseudo-normals into account.
178            let (proj, (id, _feature)) = self.project_local_point_and_get_location(point, false);
179            let feature_id = FeatureId::Face(id);
180            return (proj, feature_id);
181        }
182
183        let solid = cfg!(feature = "dim2");
184        let (tri_id, proj) = CompositeShapeRef(self).project_local_point(point, solid);
185        (proj, FeatureId::Face(tri_id))
186    }
187
188    // TODO: implement distance_to_point too?
189
190    #[inline]
191    fn contains_local_point(&self, point: &Point<Real>) -> bool {
192        #[cfg(feature = "dim3")]
193        if self.pseudo_normals.is_some() {
194            // If we can, in 3D, take the pseudo-normals into account.
195            return self
196                .project_local_point_and_get_location(point, true)
197                .0
198                .is_inside;
199        }
200
201        CompositeShapeRef(self)
202            .contains_local_point(point)
203            .is_some()
204    }
205
206    /// Projects a point on `self` transformed by `m`, unless the projection lies further than the given max distance.
207    fn project_local_point_with_max_dist(
208        &self,
209        pt: &Point<Real>,
210        solid: bool,
211        max_dist: Real,
212    ) -> Option<PointProjection> {
213        self.project_local_point_and_get_location_with_max_dist(pt, solid, max_dist)
214            .map(|proj| proj.0)
215    }
216}
217
218impl PointQuery for Compound {
219    #[inline]
220    fn project_local_point(&self, point: &Point<Real>, solid: bool) -> PointProjection {
221        CompositeShapeRef(self).project_local_point(point, solid).1
222    }
223
224    #[inline]
225    fn project_local_point_and_get_feature(
226        &self,
227        point: &Point<Real>,
228    ) -> (PointProjection, FeatureId) {
229        (
230            CompositeShapeRef(self)
231                .project_local_point_and_get_feature(point)
232                .1
233                 .0,
234            FeatureId::Unknown,
235        )
236    }
237
238    #[inline]
239    fn contains_local_point(&self, point: &Point<Real>) -> bool {
240        CompositeShapeRef(self)
241            .contains_local_point(point)
242            .is_some()
243    }
244}
245
246impl PointQueryWithLocation for Polyline {
247    type Location = (u32, SegmentPointLocation);
248
249    #[inline]
250    fn project_local_point_and_get_location(
251        &self,
252        point: &Point<Real>,
253        solid: bool,
254    ) -> (PointProjection, Self::Location) {
255        let (seg_id, (proj, loc)) = CompositeShapeRef(self)
256            .project_local_point_and_get_location(point, Real::MAX, solid)
257            .unwrap();
258        (proj, (seg_id, loc))
259    }
260}
261
262impl PointQueryWithLocation for TriMesh {
263    type Location = (u32, TrianglePointLocation);
264
265    #[inline]
266    #[allow(unused_mut)] // Because we need mut in 3D but not in 2D.
267    fn project_local_point_and_get_location(
268        &self,
269        point: &Point<Real>,
270        solid: bool,
271    ) -> (PointProjection, Self::Location) {
272        self.project_local_point_and_get_location_with_max_dist(point, solid, Real::MAX)
273            .unwrap()
274    }
275
276    /// Projects a point on `self`, with a maximum projection distance.
277    fn project_local_point_and_get_location_with_max_dist(
278        &self,
279        point: &Point<Real>,
280        solid: bool,
281        max_dist: Real,
282    ) -> Option<(PointProjection, Self::Location)> {
283        #[allow(unused_mut)] // mut is needed in 3D.
284        if let Some((part_id, (mut proj, location))) =
285            CompositeShapeRef(self).project_local_point_and_get_location(point, max_dist, solid)
286        {
287            #[cfg(feature = "dim3")]
288            if let Some(pseudo_normals) = self.pseudo_normals_if_oriented() {
289                let pseudo_normal = match location {
290                    TrianglePointLocation::OnFace(..) | TrianglePointLocation::OnSolid => {
291                        Some(self.triangle(part_id).scaled_normal())
292                    }
293                    TrianglePointLocation::OnEdge(i, _) => pseudo_normals
294                        .edges_pseudo_normal
295                        .get(part_id as usize)
296                        .map(|pn| pn[i as usize]),
297                    TrianglePointLocation::OnVertex(i) => {
298                        let idx = self.indices()[part_id as usize];
299                        pseudo_normals
300                            .vertices_pseudo_normal
301                            .get(idx[i as usize] as usize)
302                            .copied()
303                    }
304                };
305
306                if let Some(pseudo_normal) = pseudo_normal {
307                    let dpt = point - proj.point;
308                    proj.is_inside = dpt.dot(&pseudo_normal) <= 0.0;
309                }
310            }
311
312            Some((proj, (part_id, location)))
313        } else {
314            None
315        }
316    }
317}