1#![allow(unused_parens)] use 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 #[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 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 #[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 #[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 #[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 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 #[inline]
191 fn contains_local_point(&self, point: &Point<Real>) -> bool {
192 #[cfg(feature = "dim3")]
193 if self.pseudo_normals.is_some() {
194 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 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)] 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 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)] 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}