parry3d/query/point/
point_voxels.rs

1use crate::math::{Point, Real};
2use crate::query::{PointProjection, PointQuery};
3use crate::shape::{Cuboid, FeatureId, VoxelType, Voxels};
4
5impl PointQuery for Voxels {
6    #[inline]
7    fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
8        // TODO: optimize this very naive implementation.
9        let base_cuboid = Cuboid::new(self.voxel_size() / 2.0);
10        let mut smallest_dist = Real::MAX;
11        let mut result = PointProjection::new(false, *pt);
12
13        for vox in self.voxels() {
14            if vox.state.voxel_type() != VoxelType::Empty {
15                let mut candidate =
16                    base_cuboid.project_local_point(&(pt - vox.center.coords), solid);
17                candidate.point += vox.center.coords;
18
19                let candidate_dist = (candidate.point - pt).norm();
20                if candidate_dist < smallest_dist {
21                    result = candidate;
22                    smallest_dist = candidate_dist;
23                }
24            }
25        }
26
27        result
28    }
29
30    #[inline]
31    fn project_local_point_and_get_feature(
32        &self,
33        pt: &Point<Real>,
34    ) -> (PointProjection, FeatureId) {
35        // TODO: get the actual feature.
36        (self.project_local_point(pt, false), FeatureId::Unknown)
37    }
38}
parry3d/query/point/point_voxels.rs

parry3d/query/point/
point_voxels.rs
