ncollide3d/query/ray/
ray_aabb.rs

1use std::mem;
2
3#[cfg(feature = "dim3")]
4use na::Point2;
5use na::{self, RealField};
6
7use crate::bounding_volume::AABB;
8use crate::math::{Isometry, Point, Vector, DIM};
9use crate::query::{Ray, RayCast, RayIntersection};
10use crate::shape::{FeatureId, Segment};
11
12impl<N: RealField + Copy> RayCast<N> for AABB<N> {
13    fn toi_with_ray(&self, m: &Isometry<N>, ray: &Ray<N>, max_toi: N, solid: bool) -> Option<N> {
14        let ls_ray = ray.inverse_transform_by(m);
15
16        let mut tmin: N = na::zero();
17        let mut tmax: N = max_toi;
18
19        for i in 0usize..DIM {
20            if ls_ray.dir[i].is_zero() {
21                if ls_ray.origin[i] < self.mins[i] || ls_ray.origin[i] > self.maxs[i] {
22                    return None;
23                }
24            } else {
25                let _1: N = na::one();
26                let denom = _1 / ls_ray.dir[i];
27                let mut inter_with_near_plane = (self.mins[i] - ls_ray.origin[i]) * denom;
28                let mut inter_with_far_plane = (self.maxs[i] - ls_ray.origin[i]) * denom;
29
30                if inter_with_near_plane > inter_with_far_plane {
31                    mem::swap(&mut inter_with_near_plane, &mut inter_with_far_plane)
32                }
33
34                tmin = tmin.max(inter_with_near_plane);
35                tmax = tmax.min(inter_with_far_plane);
36
37                if tmin > tmax {
38                    // This covers the case where tmax is negative because tmin is
39                    // initialized at zero.
40                    return None;
41                }
42            }
43        }
44
45        if tmin.is_zero() && !solid {
46            Some(tmax)
47        } else {
48            Some(tmin)
49        }
50    }
51
52    #[inline]
53    fn toi_and_normal_with_ray(
54        &self,
55        m: &Isometry<N>,
56        ray: &Ray<N>,
57        max_toi: N,
58        solid: bool,
59    ) -> Option<RayIntersection<N>> {
60        let ls_ray = ray.inverse_transform_by(m);
61
62        ray_aabb(self, &ls_ray, max_toi, solid).map(|(t, n, i)| {
63            let feature = if i < 0 {
64                FeatureId::Face(-i as usize - 1 + 3)
65            } else {
66                FeatureId::Face(i as usize - 1)
67            };
68
69            RayIntersection::new(t, m * n, feature)
70        })
71    }
72
73    #[cfg(feature = "dim3")]
74    fn toi_and_normal_and_uv_with_ray(
75        &self,
76        m: &Isometry<N>,
77        ray: &Ray<N>,
78        max_toi: N,
79        solid: bool,
80    ) -> Option<RayIntersection<N>> {
81        do_toi_and_normal_and_uv_with_ray(m, self, ray, max_toi, solid)
82    }
83}
84
85impl<N: RealField + Copy> AABB<N> {
86    /// Computes the parameters of the two intersection points between a line and this AABB.
87    ///
88    /// The parameters are such that the point are given by `orig + dir * parameter`.
89    /// Returns `None` if there is no intersection.
90    #[inline]
91    pub fn clip_line_parameters(&self, orig: &Point<N>, dir: &Vector<N>) -> Option<(N, N)> {
92        clip_line(self, orig, dir).map(|clip| ((clip.0).0, (clip.1).0))
93    }
94
95    /// Computes the intersection segment between a line and this AABB.
96    ///
97    /// Returns `None` if there is no intersection.
98    #[inline]
99    pub fn clip_line(&self, orig: &Point<N>, dir: &Vector<N>) -> Option<Segment<N>> {
100        clip_line(self, orig, dir)
101            .map(|clip| Segment::new(orig + dir * (clip.0).0, orig + dir * (clip.1).0))
102    }
103
104    /// Computes the parameters of the two intersection points between a ray and this AABB.
105    ///
106    /// The parameters are such that the point are given by `ray.orig + ray.dir * parameter`.
107    /// Returns `None` if there is no intersection.
108    #[inline]
109    pub fn clip_ray_parameters(&self, ray: &Ray<N>) -> Option<(N, N)> {
110        self.clip_line_parameters(&ray.origin, &ray.dir)
111            .and_then(|clip| {
112                let t0 = clip.0;
113                let t1 = clip.1;
114
115                if t1 < N::zero() {
116                    None
117                } else {
118                    Some((t0.max(N::zero()), t1))
119                }
120            })
121    }
122
123    /// Computes the intersection segment between a ray and this AABB.
124    ///
125    /// Returns `None` if there is no intersection.
126    #[inline]
127    pub fn clip_ray(&self, ray: &Ray<N>) -> Option<Segment<N>> {
128        self.clip_ray_parameters(ray)
129            .map(|clip| Segment::new(ray.point_at(clip.0), ray.point_at(clip.1)))
130    }
131}
132
133#[cfg(feature = "dim3")]
134fn do_toi_and_normal_and_uv_with_ray<N: RealField + Copy>(
135    m: &Isometry<N>,
136    aabb: &AABB<N>,
137    ray: &Ray<N>,
138    max_toi: N,
139    solid: bool,
140) -> Option<RayIntersection<N>> {
141    if DIM != 3 {
142        aabb.toi_and_normal_with_ray(m, ray, max_toi, solid)
143    } else {
144        let ls_ray = ray.inverse_transform_by(m);
145
146        ray_aabb(aabb, &ls_ray, max_toi, solid).map(|(t, n, s)| {
147            let pt = ls_ray.origin + ls_ray.dir * t;
148            let dpt = pt - aabb.mins;
149            let scale = aabb.maxs - aabb.mins;
150            let id = s.abs();
151            let gs_n = m * n;
152            let feature = if s < 0 {
153                FeatureId::Face(id as usize - 1 + 3)
154            } else {
155                FeatureId::Face(id as usize - 1)
156            };
157
158            if id == 1 {
159                RayIntersection::new_with_uvs(
160                    t,
161                    gs_n,
162                    feature,
163                    Some(Point2::new(dpt[1] / scale[1], dpt[2] / scale[2])),
164                )
165            } else if id == 2 {
166                RayIntersection::new_with_uvs(
167                    t,
168                    gs_n,
169                    feature,
170                    Some(Point2::new(dpt[2] / scale[2], dpt[0] / scale[0])),
171                )
172            } else {
173                RayIntersection::new_with_uvs(
174                    t,
175                    gs_n,
176                    feature,
177                    Some(Point2::new(dpt[0] / scale[0], dpt[1] / scale[1])),
178                )
179            }
180        })
181    }
182}
183
184fn clip_line<N: RealField + Copy>(
185    aabb: &AABB<N>,
186    origin: &Point<N>,
187    dir: &Vector<N>,
188) -> Option<((N, Vector<N>, isize), (N, Vector<N>, isize))> {
189    // NOTE: we don't start with tmin = 0 so we can return the correct normal
190    // when the ray starts exactly on the object contour.
191
192    let mut tmax: N = N::max_value().unwrap();
193    let mut tmin: N = -tmax;
194    let mut near_side = 0;
195    let mut far_side = 0;
196    let mut near_diag = false;
197    let mut far_diag = false;
198
199    for i in 0usize..DIM {
200        if dir[i].is_zero() {
201            if origin[i] < aabb.mins[i] || origin[i] > aabb.maxs[i] {
202                return None;
203            }
204        } else {
205            let _1: N = na::one();
206            let denom = _1 / dir[i];
207            let flip_sides;
208            let mut inter_with_near_plane = (aabb.mins[i] - origin[i]) * denom;
209            let mut inter_with_far_plane = (aabb.maxs[i] - origin[i]) * denom;
210
211            if inter_with_near_plane > inter_with_far_plane {
212                flip_sides = true;
213                mem::swap(&mut inter_with_near_plane, &mut inter_with_far_plane)
214            } else {
215                flip_sides = false;
216            }
217
218            if inter_with_near_plane > tmin {
219                tmin = inter_with_near_plane;
220                near_side = if flip_sides {
221                    -(i as isize + 1)
222                } else {
223                    i as isize + 1
224                };
225                near_diag = false;
226            } else if inter_with_near_plane == tmin {
227                near_diag = true;
228            }
229
230            if inter_with_far_plane < tmax {
231                tmax = inter_with_far_plane;
232                far_side = if !flip_sides {
233                    -(i as isize + 1)
234                } else {
235                    i as isize + 1
236                };
237                far_diag = false;
238            } else if inter_with_far_plane == tmax {
239                far_diag = true;
240            }
241
242            if tmax < N::zero() || tmin > tmax {
243                return None;
244            }
245        }
246    }
247
248    let near = if near_diag {
249        (tmin, -dir.normalize(), near_side)
250    } else {
251        let mut normal = Vector::zeros();
252
253        if near_side < 0 {
254            normal[(-near_side - 1) as usize] = N::one();
255        } else {
256            normal[(near_side - 1) as usize] = -N::one();
257        }
258
259        (tmin, normal, near_side)
260    };
261
262    let far = if far_diag {
263        (tmax, -dir.normalize(), far_side)
264    } else {
265        let mut normal = Vector::zeros();
266
267        if far_side < 0 {
268            normal[(-far_side - 1) as usize] = -N::one();
269        } else {
270            normal[(far_side - 1) as usize] = N::one();
271        }
272
273        (tmax, normal, far_side)
274    };
275
276    Some((near, far))
277}
278
279fn ray_aabb<N: RealField + Copy>(
280    aabb: &AABB<N>,
281    ray: &Ray<N>,
282    max_toi: N,
283    solid: bool,
284) -> Option<(N, Vector<N>, isize)> {
285    clip_line(aabb, &ray.origin, &ray.dir).and_then(|(near, far)| {
286        if near.0 < N::zero() {
287            if solid {
288                Some((na::zero(), na::zero(), far.2))
289            } else if far.0 <= max_toi {
290                Some(far)
291            } else {
292                None
293            }
294        } else if near.0 <= max_toi {
295            Some(near)
296        } else {
297            None
298        }
299    })
300}