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 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 #[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 #[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 #[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 #[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 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}