parry3d/query/clip/
clip_aabb_line.rs

1use crate::bounding_volume::Aabb;
2use crate::math::{Point, Real, Vector, DIM};
3use crate::query::Ray;
4use crate::shape::Segment;
5use num::{Bounded, Zero};
6
7impl Aabb {
8    /// Computes the intersection of a segment with this Aabb.
9    ///
10    /// Returns `None` if there is no intersection or if `pa` is invalid (contains `NaN`).
11    #[inline]
12    pub fn clip_segment(&self, pa: &Point<Real>, pb: &Point<Real>) -> Option<Segment> {
13        let ab = pb - pa;
14        clip_aabb_line(self, pa, &ab)
15            .map(|clip| Segment::new(pa + ab * (clip.0).0.max(0.0), pa + ab * (clip.1).0.min(1.0)))
16    }
17
18    /// Computes the parameters of the two intersection points between a line and this Aabb.
19    ///
20    /// The parameters are such that the point are given by `orig + dir * parameter`.
21    /// Returns `None` if there is no intersection or if `orig` is invalid (contains `NaN`).
22    #[inline]
23    pub fn clip_line_parameters(
24        &self,
25        orig: &Point<Real>,
26        dir: &Vector<Real>,
27    ) -> Option<(Real, Real)> {
28        clip_aabb_line(self, orig, dir).map(|clip| ((clip.0).0, (clip.1).0))
29    }
30
31    /// Computes the intersection segment between a line and this Aabb.
32    ///
33    /// Returns `None` if there is no intersection or if `orig` is invalid (contains `NaN`).
34    #[inline]
35    pub fn clip_line(&self, orig: &Point<Real>, dir: &Vector<Real>) -> Option<Segment> {
36        clip_aabb_line(self, orig, dir)
37            .map(|clip| Segment::new(orig + dir * (clip.0).0, orig + dir * (clip.1).0))
38    }
39
40    /// Computes the parameters of the two intersection points between a ray and this Aabb.
41    ///
42    /// The parameters are such that the point are given by `ray.orig + ray.dir * parameter`.
43    /// Returns `None` if there is no intersection or if `ray.orig` is invalid (contains `NaN`).
44    #[inline]
45    pub fn clip_ray_parameters(&self, ray: &Ray) -> Option<(Real, Real)> {
46        self.clip_line_parameters(&ray.origin, &ray.dir)
47            .and_then(|clip| {
48                let t0 = clip.0;
49                let t1 = clip.1;
50
51                if t1 < 0.0 {
52                    None
53                } else {
54                    Some((t0.max(0.0), t1))
55                }
56            })
57    }
58
59    /// Computes the intersection segment between a ray and this Aabb.
60    ///
61    /// Returns `None` if there is no intersection.
62    #[inline]
63    pub fn clip_ray(&self, ray: &Ray) -> Option<Segment> {
64        self.clip_ray_parameters(ray)
65            .map(|clip| Segment::new(ray.point_at(clip.0), ray.point_at(clip.1)))
66    }
67}
68
69/// Computes the segment given by the intersection of a line and an Aabb.
70///
71/// Returns the two intersections represented as `(t, normal, side)` such that:
72/// - `origin + dir * t` gives the intersection points.
73/// - `normal` is the face normal at the intersection. This is equal to the zero vector if `dir`
74///   is invalid (a zero vector or NaN) and `origin` is inside the AABB.
75/// - `side` is the side of the AABB that was hit. This is an integer in [-3, 3] where `1` represents
76///   the `+X` axis, `2` the `+Y` axis, etc., and negative values represent the corresponding
77///   negative axis. The special value of `0` indicates that the provided `dir` is zero or `NaN`
78///   and the line origin is inside the AABB.
79pub fn clip_aabb_line(
80    aabb: &Aabb,
81    origin: &Point<Real>,
82    dir: &Vector<Real>,
83) -> Option<((Real, Vector<Real>, isize), (Real, Vector<Real>, isize))> {
84    let mut tmax: Real = Bounded::max_value();
85    let mut tmin: Real = -tmax;
86    let mut near_side = 0;
87    let mut far_side = 0;
88    let mut near_diag = false;
89    let mut far_diag = false;
90
91    for i in 0usize..DIM {
92        if dir[i].is_zero() {
93            if origin[i] < aabb.mins[i] || origin[i] > aabb.maxs[i] {
94                return None;
95            }
96        } else {
97            let denom = 1.0 / dir[i];
98            let flip_sides;
99            let mut inter_with_near_halfspace = (aabb.mins[i] - origin[i]) * denom;
100            let mut inter_with_far_halfspace = (aabb.maxs[i] - origin[i]) * denom;
101
102            if inter_with_near_halfspace > inter_with_far_halfspace {
103                flip_sides = true;
104                core::mem::swap(
105                    &mut inter_with_near_halfspace,
106                    &mut inter_with_far_halfspace,
107                )
108            } else {
109                flip_sides = false;
110            }
111
112            if inter_with_near_halfspace > tmin {
113                tmin = inter_with_near_halfspace;
114                near_side = if flip_sides {
115                    -(i as isize + 1)
116                } else {
117                    i as isize + 1
118                };
119                near_diag = false;
120            } else if inter_with_near_halfspace == tmin {
121                near_diag = true;
122            }
123
124            if inter_with_far_halfspace < tmax {
125                tmax = inter_with_far_halfspace;
126                far_side = if !flip_sides {
127                    -(i as isize + 1)
128                } else {
129                    i as isize + 1
130                };
131                far_diag = false;
132            } else if inter_with_far_halfspace == tmax {
133                far_diag = true;
134            }
135
136            if tmax < 0.0 || tmin > tmax {
137                return None;
138            }
139        }
140    }
141
142    let near = if near_diag {
143        (tmin, -dir.normalize(), near_side)
144    } else {
145        // If near_side is 0, we are in a special case where `dir` is
146        // zero or NaN. Return `Some` only if the ray starts inside the
147        // aabb.
148        if near_side == 0 {
149            let zero = (0.0, Vector::zeros(), 0);
150            return aabb.contains_local_point(origin).then_some((zero, zero));
151        }
152
153        let mut normal = Vector::zeros();
154
155        if near_side < 0 {
156            normal[(-near_side - 1) as usize] = 1.0;
157        } else {
158            normal[(near_side - 1) as usize] = -1.0;
159        }
160
161        (tmin, normal, near_side)
162    };
163
164    let far = if far_diag {
165        (tmax, -dir.normalize(), far_side)
166    } else {
167        // If far_side is 0, we are in a special case where `dir` is
168        // zero or NaN. Return `Some` only if the ray starts inside the
169        // aabb.
170        if far_side == 0 {
171            let zero = (0.0, Vector::zeros(), 0);
172            return aabb.contains_local_point(origin).then_some((zero, zero));
173        }
174
175        let mut normal = Vector::zeros();
176
177        if far_side < 0 {
178            normal[(-far_side - 1) as usize] = -1.0;
179        } else {
180            normal[(far_side - 1) as usize] = 1.0;
181        }
182
183        (tmax, normal, far_side)
184    };
185
186    Some((near, far))
187}
188#[cfg(test)]
189mod test {
190    use super::*;
191
192    #[test]
193    pub fn clip_empty_aabb_line() {
194        assert!(clip_aabb_line(
195            &Aabb::new(Point::origin(), Point::origin()),
196            &Point::origin(),
197            &Vector::zeros(),
198        )
199        .is_some());
200        assert!(clip_aabb_line(
201            &Aabb::new(Vector::repeat(1.0).into(), Vector::repeat(2.0).into()),
202            &Point::origin(),
203            &Vector::zeros(),
204        )
205        .is_none());
206    }
207
208    #[test]
209    pub fn clip_empty_aabb_segment() {
210        let aabb_origin = Aabb::new(Point::origin(), Point::origin());
211        let aabb_shifted = Aabb::new(Vector::repeat(1.0).into(), Vector::repeat(2.0).into());
212        assert!(aabb_origin
213            .clip_segment(&Point::origin(), &Point::from(Vector::repeat(Real::NAN)))
214            .is_some());
215        assert!(aabb_shifted
216            .clip_segment(&Point::origin(), &Point::from(Vector::repeat(Real::NAN)))
217            .is_none());
218    }
219}
parry3d/query/clip/clip_aabb_line.rs

parry3d/query/clip/
clip_aabb_line.rs
