parry2d/query/point/
point_composite_shape.rs

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

parry2d/query/point/
point_composite_shape.rs
