ncollide3d/query/visitors/
composite_closest_point_visitor.rs
1use crate::bounding_volume::AABB;
2use crate::math::{Isometry, Point};
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
4use crate::query::{PointProjection, PointQuery};
5use crate::shape::CompositeShape;
6use na::{self, RealField};
7
8pub struct CompositeClosestPointVisitor<'a, N: 'a + RealField + Copy, S: 'a + CompositeShape<N>> {
10 shape: &'a S,
11 point: &'a Point<N>,
12 solid: bool,
13}
14
15impl<'a, N: RealField + Copy, S: CompositeShape<N>> CompositeClosestPointVisitor<'a, N, S> {
16 pub fn new(shape: &'a S, point: &'a Point<N>, solid: bool) -> Self {
18 CompositeClosestPointVisitor {
19 shape,
20 point,
21 solid,
22 }
23 }
24}
25
26impl<'a, N: RealField + Copy, S: CompositeShape<N> + PointQuery<N>>
27 BestFirstVisitor<N, usize, AABB<N>> for CompositeClosestPointVisitor<'a, N, S>
28{
29 type Result = PointProjection<N>;
30
31 #[inline]
32 fn visit(
33 &mut self,
34 best: N,
35 aabb: &AABB<N>,
36 data: Option<&usize>,
37 ) -> BestFirstVisitStatus<N, Self::Result> {
38 let dist = aabb.distance_to_point(&Isometry::identity(), self.point, true);
39
40 let mut res = BestFirstVisitStatus::Continue {
41 cost: dist,
42 result: None,
43 };
44
45 if let Some(b) = data {
46 if dist < best {
47 self.shape
48 .map_part_at(*b, &Isometry::identity(), &mut |objm, obj| {
49 let proj = obj.project_point(objm, self.point, self.solid);
50
51 res = BestFirstVisitStatus::Continue {
52 cost: na::distance(self.point, &proj.point),
53 result: Some(proj),
54 };
55 });
56 }
57 }
58
59 res
60 }
61}