parry2d/query/ray/
ray_aabb.rs

1use core::mem;
2
3use crate::bounding_volume::Aabb;
4use crate::math::{Real, Vector, 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[i].is_zero() {
16                if ray.origin[i] < self.mins[i] || ray.origin[i] > self.maxs[i] {
17                    return None;
18                }
19            } else {
20                let denom = 1.0 / ray.dir[i];
21                let mut inter_with_near_halfspace = (self.mins[i] - ray.origin[i]) * denom;
22                let mut inter_with_far_halfspace = (self.maxs[i] - ray.origin[i]) * denom;
23
24                if inter_with_near_halfspace > inter_with_far_halfspace {
25                    mem::swap(
26                        &mut inter_with_near_halfspace,
27                        &mut inter_with_far_halfspace,
28                    )
29                }
30
31                tmin = tmin.max(inter_with_near_halfspace);
32                tmax = tmax.min(inter_with_far_halfspace);
33
34                if tmin > tmax {
35                    // This covers the case where tmax is negative because tmin is
36                    // initialized at zero.
37                    return None;
38                }
39            }
40        }
41
42        if tmin.is_zero() && !solid {
43            Some(tmax)
44        } else {
45            Some(tmin)
46        }
47    }
48
49    #[inline]
50    fn cast_local_ray_and_get_normal(
51        &self,
52        ray: &Ray,
53        max_time_of_impact: Real,
54        solid: bool,
55    ) -> Option<RayIntersection> {
56        ray_aabb(self, ray, max_time_of_impact, solid).map(|(t, n, i)| {
57            let feature = if i < 0 {
58                FeatureId::Face((-i) as u32 - 1 + 3)
59            } else {
60                FeatureId::Face(i as u32 - 1)
61            };
62
63            RayIntersection::new(t, n, feature)
64        })
65    }
66}
67
68fn ray_aabb(
69    aabb: &Aabb,
70    ray: &Ray,
71    max_time_of_impact: Real,
72    solid: bool,
73) -> Option<(Real, Vector, isize)> {
74    use crate::query::clip;
75    clip::clip_aabb_line(aabb, ray.origin, ray.dir).and_then(|(near, far)| {
76        if near.0 < 0.0 {
77            if solid {
78                Some((0.0, Vector::ZERO, far.2))
79            } else if far.0 <= max_time_of_impact {
80                Some(far)
81            } else {
82                None
83            }
84        } else if near.0 <= max_time_of_impact {
85            Some(near)
86        } else {
87            None
88        }
89    })
90}
parry2d/query/ray/ray_aabb.rs

parry2d/query/ray/
ray_aabb.rs
