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
8/// Best-first traversal visitor for computing the point closest to a composite shape.
9pub 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    /// Initializes a visitor that allows the computation of the point closest to `point` on `shape`.
17    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}