parry2d/query/ray/
ray_voxels.rs

1use crate::math::{IVectorExt, Real, Vector, VectorExt};
2use crate::partitioning::BvhNode;
3use crate::query::{Ray, RayCast, RayIntersection};
4use crate::shape::{Voxels, VoxelsChunkRef};
5
6impl RayCast for Voxels {
7    #[inline]
8    fn cast_local_ray_and_get_normal(
9        &self,
10        ray: &Ray,
11        max_time_of_impact: Real,
12        solid: bool,
13    ) -> Option<RayIntersection> {
14        self.chunk_bvh()
15            .find_best(
16                max_time_of_impact,
17                |node: &BvhNode, best_so_far| node.cast_ray(ray, best_so_far),
18                |primitive, best_so_far| {
19                    let chunk = self.chunk_ref(primitive);
20                    chunk.cast_local_ray_and_get_normal(ray, best_so_far, solid)
21                },
22            )
23            .map(|(_chunk_id, hit)| hit)
24    }
25}
26
27impl<'a> RayCast for VoxelsChunkRef<'a> {
28    #[inline]
29    fn cast_local_ray_and_get_normal(
30        &self,
31        ray: &Ray,
32        max_time_of_impact: Real,
33        solid: bool,
34    ) -> Option<RayIntersection> {
35        use num_traits::Bounded;
36
37        let aabb = self.local_aabb();
38        let (min_t, mut max_t) = aabb.clip_ray_parameters(ray)?;
39
40        #[cfg(feature = "dim2")]
41        let ii = [0, 1];
42        #[cfg(feature = "dim3")]
43        let ii = [0, 1, 2];
44
45        if min_t > max_time_of_impact {
46            return None;
47        }
48
49        max_t = max_t.min(max_time_of_impact);
50        let clip_ray_a = ray.point_at(min_t);
51        let voxel_key_signed = self.voxel_at_point_unchecked(clip_ray_a);
52        let mut voxel_key = self.clamp_voxel(voxel_key_signed);
53        let [domain_mins, domain_maxs] = self.domain();
54
55        loop {
56            let aabb = self.voxel_aabb_unchecked(voxel_key);
57
58            if let Some(voxel) = self.voxel_state(voxel_key) {
59                if !voxel.is_empty() {
60                    // We hit a voxel!
61                    // TODO: if `solid` is false, and we started hitting from the first iteration,
62                    //       then we should continue the ray propagation until we reach empty space again.
63                    let hit = aabb.cast_local_ray_and_get_normal(ray, max_t, solid);
64
65                    if let Some(mut hit) = hit {
66                        // The feature stays the one the voxel's Aabb reported; `subshape` says
67                        // which voxel it belongs to.
68                        hit.subshape = self.flat_id(voxel_key).unwrap_or_else(|| unreachable!());
69                        return Some(hit);
70                    }
71                }
72            }
73
74            /*
75             * Find the next voxel to cast the ray on.
76             */
77            let toi = ii.map(|i| {
78                if ray.dir.vget(i) > 0.0 {
79                    let t = (aabb.maxs.vget(i) - ray.origin.vget(i)) / ray.dir.vget(i);
80                    if t < 0.0 {
81                        (Real::max_value(), true)
82                    } else {
83                        (t, true)
84                    }
85                } else if ray.dir.vget(i) < 0.0 {
86                    let t = (aabb.mins.vget(i) - ray.origin.vget(i)) / ray.dir.vget(i);
87                    if t < 0.0 {
88                        (Real::max_value(), false)
89                    } else {
90                        (t, false)
91                    }
92                } else {
93                    (Real::max_value(), false)
94                }
95            });
96
97            #[cfg(feature = "dim2")]
98            if toi[0].0 > max_t && toi[1].0 > max_t {
99                break;
100            }
101
102            #[cfg(feature = "dim3")]
103            if toi[0].0 > max_t && toi[1].0 > max_t && toi[2].0 > max_t {
104                break;
105            }
106
107            let imin = Vector::from(toi.map(|t| t.0)).min_position();
108
109            if toi[imin].1 {
110                if voxel_key.ivget(imin) < domain_maxs.ivget(imin) - 1 {
111                    voxel_key.ivset(imin, voxel_key.ivget(imin) + 1);
112                } else {
113                    // Leaving the shape's bounds.
114                    break;
115                }
116            } else if voxel_key.ivget(imin) > domain_mins.ivget(imin) {
117                voxel_key.ivset(imin, voxel_key.ivget(imin) - 1);
118            } else {
119                // Leaving the shape’s bounds.
120                break;
121            }
122        }
123
124        None
125    }
126}
parry2d/query/ray/ray_voxels.rs

parry2d/query/ray/
ray_voxels.rs
