parry3d/query/point/
point_voxels.rs

1use crate::math::{Point, Real, Vector};
2use crate::query::{PointProjection, PointQuery};
3use crate::shape::{Cuboid, FeatureId, Voxels, VoxelsChunkRef};
4
5impl PointQuery for Voxels {
6    #[inline]
7    fn project_local_point(&self, pt: &Point<Real>, solid: bool) -> PointProjection {
8        self.chunk_bvh()
9            .project_point(pt, Real::MAX, |chunk_id, _| {
10                let chunk = self.chunk_ref(chunk_id);
11                chunk
12                    .project_local_point_and_get_vox_id(pt, solid)
13                    .map(|(proj, _)| proj)
14            })
15            .map(|res| res.1 .1)
16            .unwrap_or(PointProjection::new(
17                false,
18                Vector::repeat(Real::MAX).into(),
19            ))
20    }
21
22    #[inline]
23    fn project_local_point_and_get_feature(
24        &self,
25        pt: &Point<Real>,
26    ) -> (PointProjection, FeatureId) {
27        self.chunk_bvh()
28            .project_point_and_get_feature(pt, Real::MAX, |chunk_id, _| {
29                let chunk = self.chunk_ref(chunk_id);
30                // TODO: we need a way to return both the voxel id, and the feature on the voxel.
31                chunk
32                    .project_local_point_and_get_vox_id(pt, false)
33                    .map(|(proj, vox)| (proj, FeatureId::Face(vox)))
34            })
35            .map(|res| res.1 .1)
36            .unwrap_or((
37                PointProjection::new(false, Vector::repeat(Real::MAX).into()),
38                FeatureId::Unknown,
39            ))
40    }
41}
42
43impl<'a> VoxelsChunkRef<'a> {
44    #[inline]
45    fn project_local_point_and_get_vox_id(
46        &self,
47        pt: &Point<Real>,
48        solid: bool,
49    ) -> Option<(PointProjection, u32)> {
50        // TODO: optimize this naive implementation that just iterates on all the voxels
51        //       from this chunk.
52        let base_cuboid = Cuboid::new(self.parent.voxel_size() / 2.0);
53        let mut smallest_dist = Real::MAX;
54        let mut result = PointProjection::new(false, *pt);
55        let mut result_vox_id = 0;
56
57        for vox in self.voxels() {
58            let mut candidate = base_cuboid.project_local_point(&(pt - vox.center.coords), solid);
59            candidate.point += vox.center.coords;
60
61            let candidate_dist = (candidate.point - pt).norm();
62            if candidate_dist < smallest_dist {
63                result = candidate;
64                result_vox_id = vox.linear_id.flat_id();
65                smallest_dist = candidate_dist;
66            }
67        }
68
69        (smallest_dist < Real::MAX).then_some((result, result_vox_id as u32))
70    }
71}
parry3d/query/point/point_voxels.rs

parry3d/query/point/
point_voxels.rs
