parry3d/query/ray/
ray_aabb.rs

1use core::mem;
2
3use na;
4
5use crate::bounding_volume::Aabb;
6use crate::math::{Real, Vector, DIM};
7use crate::query::{Ray, RayCast, RayIntersection};
8use crate::shape::FeatureId;
9use num::Zero;
10
11impl RayCast for Aabb {
12    fn cast_local_ray(&self, ray: &Ray, max_time_of_impact: Real, solid: bool) -> Option<Real> {
13        let mut tmin: Real = 0.0;
14        let mut tmax: Real = max_time_of_impact;
15
16        for i in 0usize..DIM {
17            if ray.dir[i].is_zero() {
18                if ray.origin[i] < self.mins[i] || ray.origin[i] > self.maxs[i] {
19                    return None;
20                }
21            } else {
22                let denom = 1.0 / ray.dir[i];
23                let mut inter_with_near_halfspace = (self.mins[i] - ray.origin[i]) * denom;
24                let mut inter_with_far_halfspace = (self.maxs[i] - ray.origin[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            let feature = if i < 0 {
60                FeatureId::Face((-i) as u32 - 1 + 3)
61            } else {
62                FeatureId::Face(i as u32 - 1)
63            };
64
65            RayIntersection::new(t, n, feature)
66        })
67    }
68}
69
70fn ray_aabb(
71    aabb: &Aabb,
72    ray: &Ray,
73    max_time_of_impact: Real,
74    solid: bool,
75) -> Option<(Real, Vector<Real>, isize)> {
76    use crate::query::clip;
77    clip::clip_aabb_line(aabb, &ray.origin, &ray.dir).and_then(|(near, far)| {
78        if near.0 < 0.0 {
79            if solid {
80                Some((0.0, na::zero(), far.2))
81            } else if far.0 <= max_time_of_impact {
82                Some(far)
83            } else {
84                None
85            }
86        } else if near.0 <= max_time_of_impact {
87            Some(near)
88        } else {
89            None
90        }
91    })
92}
parry3d/query/ray/ray_aabb.rs

parry3d/query/ray/
ray_aabb.rs
