parry3d/query/point/
point_heightfield.rs

1use crate::bounding_volume::Aabb;
2#[cfg(not(feature = "std"))]
3use crate::math::ComplexField;
4use crate::math::{Real, Vector};
5use crate::query::{PointProjection, PointQuery, PointQueryWithLocation};
6use crate::shape::{FeatureId, HeightField, TrianglePointLocation}; // For sqrt.
7
8impl PointQuery for HeightField {
9    fn project_local_point_with_max_dist(
10        &self,
11        pt: Vector,
12        solid: bool,
13        max_dist: Real,
14    ) -> Option<PointProjection> {
15        let aabb = Aabb::new(pt - Vector::splat(max_dist), pt + Vector::splat(max_dist));
16        let mut sq_smallest_dist = Real::MAX;
17        let mut best_proj = None;
18
19        self.map_elements_in_local_aabb(&aabb, &mut |_, triangle| {
20            let proj = triangle.project_local_point(pt, solid);
21            let sq_dist = (pt - proj.point).length_squared();
22
23            if sq_dist < sq_smallest_dist {
24                sq_smallest_dist = sq_dist;
25
26                if sq_dist.sqrt() <= max_dist {
27                    best_proj = Some(proj);
28                }
29            }
30        });
31
32        best_proj
33    }
34
35    #[inline]
36    fn project_local_point(&self, point: Vector, solid: bool) -> PointProjection {
37        // Grow a search neighborhood around `point` geometrically instead of iterating on
38        // every element. A projection found at `dist <= max_dist` is the global closest
39        // one: any closer element would intersect the ball of radius `dist` around
40        // `point`, which the searched AABB contains.
41        let root_aabb = self.root_aabb();
42        let extents = root_aabb.extents();
43
44        // Distance beyond which the search AABB covers every cell, making the search
45        // below exhaustive.
46        let max_search_dist = (point - root_aabb.center()).length() + extents.length();
47
48        #[cfg(feature = "dim2")]
49        let cell_size = self.cell_width();
50        #[cfg(feature = "dim3")]
51        let cell_size = self.cell_width().max(self.cell_height());
52
53        // Initial guess: distance to the root AABB plus one cell, so the first search
54        // usually visits only a few cells.
55        let dist_to_aabb = (point - point.clamp(root_aabb.mins, root_aabb.maxs)).length();
56        let mut search_dist = (dist_to_aabb + cell_size).max(max_search_dist * 1.0e-4);
57
58        // TODO: the search AABB only grows, so each iteration re-tests the elements the
59        //       previous ones already tested. Visit only the ring added by each step.
60        while search_dist < max_search_dist {
61            if let Some(proj) = self.project_local_point_with_max_dist(point, solid, search_dist) {
62                return proj;
63            }
64
65            search_dist *= 4.0;
66        }
67
68        self.project_local_point_with_max_dist(point, solid, max_search_dist)
69            .unwrap_or_else(|| PointProjection::new(false, point))
70    }
71
72    #[inline]
73    fn project_local_point_and_get_feature(&self, point: Vector) -> (PointProjection, FeatureId) {
74        // TODO: compute the feature properly.
75        (self.project_local_point(point, false), FeatureId::Unknown)
76    }
77
78    // TODO: implement distance_to_point too?
79
80    #[inline]
81    fn contains_local_point(&self, _point: Vector) -> bool {
82        false
83    }
84}
85
86impl PointQueryWithLocation for HeightField {
87    type Location = (usize, TrianglePointLocation);
88
89    #[inline]
90    fn project_local_point_and_get_location(
91        &self,
92        _point: Vector,
93        _: bool,
94    ) -> (PointProjection, Self::Location) {
95        unimplemented!()
96    }
97}
parry3d/query/point/point_heightfield.rs

parry3d/query/point/
point_heightfield.rs
