parry3d/query/ray/
ray_aabb.rs

1use core::mem;
2
3use crate::bounding_volume::Aabb;
4use crate::math::{Real, Vector, VectorExt, DIM};
5use crate::query::{Ray, RayCast, RayIntersection};
6use crate::shape::FeatureId;
7use num::Zero;
8
9impl RayCast for Aabb {
10    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
11        let mut tmin: Real = 0.0;
12        let mut tmax: Real = max_time_of_impact;
13
14        for i in 0usize..DIM {
15            if ray.dir.vget(i).is_zero() {
16                if ray.origin.vget(i) < self.mins.vget(i) || ray.origin.vget(i) > self.maxs.vget(i)
17                {
18                    return None;
19                }
20            } else {
21                let denom = 1.0 / ray.dir.vget(i);
22                let mut inter_with_near_halfspace =
23                    (self.mins.vget(i) - ray.origin.vget(i)) * denom;
24                let mut inter_with_far_halfspace = (self.maxs.vget(i) - ray.origin.vget(i)) * denom;
25
26                if inter_with_near_halfspace > inter_with_far_halfspace {
27                    mem::swap(
28                        &mut inter_with_near_halfspace,
29                        &mut inter_with_far_halfspace,
30                    )
31                }
32
33                tmin = tmin.max(inter_with_near_halfspace);
34                tmax = tmax.min(inter_with_far_halfspace);
35
36                if tmin > tmax {
37                    // This covers the case where tmax is negative because tmin is
38                    // initialized at zero.
39                    return None;
40                }
41            }
42        }
43
44        if tmin.is_zero() && !solid {
45            Some(tmax)
46        } else {
47            Some(tmin)
48        }
49    }
50
51    #[inline]
52    fn cast_local_ray_and_get_normal(
53        &self,
54        ray: &Ray,
55        max_time_of_impact: Real,
56        solid: bool,
57    ) -> Option<RayIntersection> {
58        ray_aabb(self, ray, max_time_of_impact, solid).map(|(t, n, i)| {
59            // NOTE: `i` is the side of the AABB that was hit, as an integer in `[-DIM, DIM]`.
60            //       The special value `0` indicates that the ray direction is zero/NaN and its
61            //       origin lies inside the AABB; in that case there is no specific face hit.
62            let feature = if i == 0 {
63                FeatureId::Unknown
64            } else if i < 0 {
65                FeatureId::Face((-i) as u32 - 1 + 3)
66            } else {
67                FeatureId::Face(i as u32 - 1)
68            };
69
70            RayIntersection::new(t, n, feature)
71        })
72    }
73}
74
75fn ray_aabb(
76    aabb: &Aabb,
77    ray: &Ray,
78    max_time_of_impact: Real,
79    solid: bool,
80) -> Option<(Real, Vector, isize)> {
81    use crate::query::clip;
82    clip::clip_aabb_line(aabb, ray.origin, ray.dir).and_then(|(near, far)| {
83        if near.0 < 0.0 {
84            if solid {
85                Some((0.0, Vector::ZERO, far.2))
86            } else if far.0 <= max_time_of_impact {
87                Some(far)
88            } else {
89                None
90            }
91        } else if near.0 <= max_time_of_impact {
92            Some(near)
93        } else {
94            None
95        }
96    })
97}
98
99#[cfg(test)]
100mod test {
101    use super::*;
102
103    /// Regression test for <https://github.com/dimforge/parry/issues/383>:
104    /// casting a ray with a zero direction starting inside an AABB used to panic with
105    /// "attempt to subtract with overflow" while computing the hit feature id.
106    #[test]
107    fn cast_zero_dir_ray_from_inside_aabb_does_not_overflow() {
108        let aabb = Aabb::new(-Vector::splat(1.0), Vector::splat(1.0));
109        let ray = Ray::new(Vector::ZERO, Vector::ZERO);
110
111        // Both `solid` and non-`solid` variants must not panic.
112        let solid = aabb.cast_local_ray_and_get_normal(&ray, 100.0, true);
113        let hollow = aabb.cast_local_ray_and_get_normal(&ray, 100.0, false);
114
115        assert!(solid.is_some());
116        assert!(hollow.is_some());
117        assert_eq!(solid.unwrap().feature, FeatureId::Unknown);
118    }
119}
parry3d/query/ray/ray_aabb.rs

parry3d/query/ray/
ray_aabb.rs
