1use maths_rs::dot;
2
3use crate::{cube::Cube, magnitude_squared, normalize, plane::Plane, ray::Ray, sphere::Sphere, Vector3};
4
5pub fn ray_aabb_intersection(start: Vector3, direction: Vector3, min: Vector3, max: Vector3) -> Option<f32> {
7 let r = 1.0 / direction;
8
9 let mis = min - start;
10 let mas = max - start;
11
12 let t1 = mis.x * r.x;
13 let t2 = mas.x * r.x;
14 let t3 = mis.y * r.y;
15 let t4 = mas.y * r.y;
16 let t5 = mis.z * r.z;
17 let t6 = mas.z * r.z;
18
19 let tmin = t1.min(t2).max(t3.min(t4)).max(t5.min(t6));
20 let tmax = t1.max(t2).min(t3.max(t4)).min(t5.max(t6));
21
22 if tmax >= 0.0 && tmin <= tmax {
23 Some(tmin)
24 } else {
25 None
26 }
27}
28
29pub fn sphere_in_frustum(sphere: &Sphere, frustum_planes: &[Plane; 6]) -> bool {
41 for plane in frustum_planes {
46 if !plane.is_sphere_in_half_space(sphere) {
47 return false;
49 }
50 }
51
52 true
54}
55
56#[derive(Debug, Clone, PartialEq)]
57pub struct Intersection {
58 pub normal: Vector3,
59 pub depth: f32,
60 pub point_on_a: Vector3,
62 pub point_on_b: Vector3,
64}
65
66impl Intersection {
67 pub fn swap(self) -> Intersection {
69 Intersection {
70 normal: self.normal,
71 depth: self.depth,
72 point_on_a: self.point_on_b,
73 point_on_b: self.point_on_a,
74 }
75 }
76}
77
78fn sphere_sphere_intersection_at_time(
79 sphere_a: &Sphere,
80 sphere_b: &Sphere,
81 a_velocity: Vector3,
82 b_velocity: Vector3,
83 toi: f32,
84) -> DynamicIntersection {
85 let new_pos_a = sphere_a.center + a_velocity * toi;
86 let new_pos_b = sphere_b.center + b_velocity * toi;
87 let ab = new_pos_b - new_pos_a;
88 let distance_squared = magnitude_squared(ab);
89
90 let (normal, distance) = if distance_squared > 1e-12 {
93 let distance = distance_squared.sqrt();
94 (ab / distance, distance)
95 } else {
96 let relative_velocity = a_velocity - b_velocity;
97 if magnitude_squared(relative_velocity) > 1e-12 {
98 (normalize(relative_velocity), 0.0)
99 } else {
100 (Vector3::new(1.0, 0.0, 0.0), 0.0)
101 }
102 };
103
104 let depth = (sphere_a.radius + sphere_b.radius - distance).max(0.0);
105
106 DynamicIntersection {
107 toi,
108 normal,
109 depth,
110 point_on_a: new_pos_a + normal * sphere_a.radius,
111 point_on_b: new_pos_b - normal * sphere_b.radius,
112 }
113}
114
115#[derive(Debug, Clone, PartialEq)]
116pub struct DynamicIntersection {
117 pub toi: f32,
118 pub normal: Vector3,
119 pub depth: f32,
120 pub point_on_a: Vector3,
122 pub point_on_b: Vector3,
124}
125
126impl From<DynamicIntersection> for Intersection {
127 fn from(intersection: DynamicIntersection) -> Self {
128 Intersection {
129 normal: intersection.normal,
130 depth: intersection.depth,
131 point_on_a: intersection.point_on_a,
132 point_on_b: intersection.point_on_b,
133 }
134 }
135}
136
137impl PartialOrd for DynamicIntersection {
138 fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
139 self.toi.partial_cmp(&other.toi)
140 }
141}
142
143pub fn sphere_vs_sphere(sphere_a: &Sphere, sphere_b: &Sphere) -> Option<Intersection> {
144 let ab = sphere_b.center - sphere_a.center;
145 let m2 = magnitude_squared(ab);
146
147 if m2 < (sphere_a.radius + sphere_b.radius).powf(2f32) {
148 let ab_mag = m2.sqrt();
149 let normal = ab / ab_mag;
150
151 let depth = sphere_a.radius + sphere_b.radius - ab_mag;
152
153 let point_on_a = sphere_a.center + normal * sphere_a.radius;
154 let point_on_b = sphere_b.center - normal * sphere_b.radius;
155
156 Some(Intersection {
157 normal,
158 depth,
159 point_on_a,
160 point_on_b,
161 })
162 } else {
163 None
164 }
165}
166
167pub fn cube_vs_cube(a: &Cube, b: &Cube) -> Option<Intersection> {
168 let sa = a.half_size;
169 let sb = b.half_size;
170
171 let ab = a.center - b.center;
172 let abs_ab = Vector3::new(ab.x.abs(), ab.y.abs(), ab.z.abs());
173 let overlap = sa + sb - abs_ab;
174
175 if overlap.x <= 0.0 || overlap.y <= 0.0 || overlap.z <= 0.0 {
176 return None;
177 }
178
179 let mut min_depth = overlap.x;
180
181 let axis = if overlap.y < min_depth {
182 min_depth = overlap.y;
183 1
184 } else if overlap.z < min_depth {
185 min_depth = overlap.z;
186 2
187 } else {
188 0
189 };
190
191 let depth = min_depth;
192
193 let sign = match axis {
194 0 => ab.x.signum(),
195 1 => ab.y.signum(),
196 2 => ab.z.signum(),
197 _ => unreachable!(),
198 };
199
200 let a_min = a.center - sa;
201 let a_max = a.center + sa;
202 let b_min = b.center - sb;
203 let b_max = b.center + sb;
204
205 let overlap_min = Vector3::new(a_min.x.max(b_min.x), a_min.y.max(b_min.y), a_min.z.max(b_min.z));
206 let overlap_max = Vector3::new(a_max.x.min(b_max.x), a_max.y.min(b_max.y), a_max.z.min(b_max.z));
207
208 let ox = (overlap_min.x + overlap_max.x) / 2f32;
209 let oy = (overlap_min.y + overlap_max.y) / 2f32;
210 let oz = (overlap_min.z + overlap_max.z) / 2f32;
211
212 let (contact_a, contact_b) = match axis {
213 0 => {
214 let (ax, bx) = if sign > 0f32 { (a_max.x, b_min.x) } else { (a_min.x, b_max.x) };
215
216 (Vector3::new(ax, oy, oz), Vector3::new(bx, oy, oz))
217 }
218 1 => {
219 let (ay, by) = if sign > 0f32 { (a_max.y, b_min.y) } else { (a_min.y, b_max.y) };
220
221 (Vector3::new(ox, ay, oz), Vector3::new(ox, by, oz))
222 }
223 2 => {
224 let (az, bz) = if sign > 0f32 { (a_max.z, b_min.z) } else { (a_min.z, b_max.z) };
225
226 (Vector3::new(ox, oy, az), Vector3::new(ox, oy, bz))
227 }
228 _ => unreachable!(),
229 };
230
231 Some(Intersection {
232 normal: normalize(ab),
233 depth,
234 point_on_a: contact_a,
235 point_on_b: contact_b,
236 })
237}
238
239pub fn sphere_vs_cube(sphere_a: &Sphere, cube_b: &Cube) -> Option<Intersection> {
240 let delta = sphere_a.center - cube_b.center;
241
242 let clamped_delta = Vector3::new(
243 delta.x.clamp(-cube_b.half_size.x, cube_b.half_size.x),
244 delta.y.clamp(-cube_b.half_size.y, cube_b.half_size.y),
245 delta.z.clamp(-cube_b.half_size.z, cube_b.half_size.z),
246 );
247
248 let closest_point_on_cube = cube_b.center + clamped_delta;
249
250 let to_center = sphere_a.center - closest_point_on_cube;
251 let distance_squared = dot(to_center, to_center);
252
253 if distance_squared > sphere_a.radius * sphere_a.radius {
254 return None;
255 }
256
257 let (normal, depth, point_on_b) = if distance_squared > 1e-12 {
258 let distance = distance_squared.sqrt();
259 (normalize(to_center), sphere_a.radius - distance, closest_point_on_cube)
260 } else {
261 let distances_to_faces = cube_b.half_size - Vector3::new(delta.x.abs(), delta.y.abs(), delta.z.abs());
262
263 let axis = if distances_to_faces.y < distances_to_faces.x && distances_to_faces.y <= distances_to_faces.z {
266 1
267 } else if distances_to_faces.z < distances_to_faces.x && distances_to_faces.z < distances_to_faces.y {
268 2
269 } else {
270 0
271 };
272
273 let sign = match axis {
274 0 => delta.x.signum(),
275 1 => delta.y.signum(),
276 2 => delta.z.signum(),
277 _ => unreachable!(),
278 };
279 let sign = if sign == 0.0 { 1.0 } else { sign };
280
281 let normal = match axis {
282 0 => Vector3::new(sign, 0.0, 0.0),
283 1 => Vector3::new(0.0, sign, 0.0),
284 2 => Vector3::new(0.0, 0.0, sign),
285 _ => unreachable!(),
286 };
287
288 let point_on_b = match axis {
289 0 => Vector3::new(
290 cube_b.center.x + sign * cube_b.half_size.x,
291 sphere_a.center.y,
292 sphere_a.center.z,
293 ),
294 1 => Vector3::new(
295 sphere_a.center.x,
296 cube_b.center.y + sign * cube_b.half_size.y,
297 sphere_a.center.z,
298 ),
299 2 => Vector3::new(
300 sphere_a.center.x,
301 sphere_a.center.y,
302 cube_b.center.z + sign * cube_b.half_size.z,
303 ),
304 _ => unreachable!(),
305 };
306
307 let depth = sphere_a.radius
308 + match axis {
309 0 => distances_to_faces.x,
310 1 => distances_to_faces.y,
311 2 => distances_to_faces.z,
312 _ => unreachable!(),
313 };
314
315 (normal, depth, point_on_b)
316 };
317
318 let point_on_a = sphere_a.center - normal * sphere_a.radius;
319
320 Some(Intersection {
321 normal,
322 depth,
323 point_on_a,
324 point_on_b,
325 })
326}
327
328pub fn ray_vs_sphere(ray_a: &Ray, sphere_b: &Sphere) -> Option<(f32, f32)> {
330 let m = sphere_b.center - ray_a.origin;
331 let a = dot(ray_a.direction, ray_a.direction);
332 let b = dot(m, ray_a.direction);
333 let c = dot(m, m) - sphere_b.radius * sphere_b.radius;
334
335 let delta = b * b - a * c;
336
337 if delta < 0.0 {
338 return None;
339 }
340
341 let inv_a = 1.0 / a;
342
343 let delta_root = delta.sqrt();
344 let t1 = inv_a * (b - delta_root);
345 let t2 = inv_a * (b + delta_root);
346
347 Some((t1, t2))
348}
349
350pub fn sphere_vs_sphere_dynamic(
351 sphere_a: &Sphere,
352 sphere_b: &Sphere,
353 a_velocity: Vector3,
354 b_velocity: Vector3,
355 dt: f32,
356) -> Option<DynamicIntersection> {
357 let relative_velocity = a_velocity - b_velocity;
358 let ray_dir = relative_velocity * dt;
359 let expanded_radius = sphere_a.radius + sphere_b.radius;
360
361 if magnitude_squared(ray_dir) < 0.00001 {
363 let ab = sphere_b.center - sphere_a.center;
364 let radius = expanded_radius + 0.00001;
365
366 return (magnitude_squared(ab) <= radius * radius)
367 .then(|| sphere_sphere_intersection_at_time(sphere_a, sphere_b, a_velocity, b_velocity, 0.0));
368 }
369
370 let (t0, t1) = ray_vs_sphere(
371 &Ray::new(sphere_a.center, ray_dir),
372 &Sphere::new(sphere_b.center, expanded_radius),
373 )?;
374
375 if t1 < 0.0 {
376 return None;
377 }
378
379 let toi_fraction = 0f32.max(t0);
380
381 if toi_fraction > 1.0 {
382 return None;
383 }
384
385 Some(sphere_sphere_intersection_at_time(
386 sphere_a,
387 sphere_b,
388 a_velocity,
389 b_velocity,
390 toi_fraction * dt,
391 ))
392}
393
394#[cfg(test)]
395mod tests {
396 use std::assert_matches;
397
398 use maths_rs::num::Base;
399
400 use super::*;
401 use crate::{normalize, Vector3};
402
403 #[test]
404 fn test_ray_aabb_intersection() {
405 {
406 let start = Vector3::new(0.0, 2.0, 0.0);
407 let direction = Vector3::new(0.0, -1.0, 0.0);
408 let min = Vector3::new(-0.5, -0.5, -0.5);
409 let max = Vector3::new(0.5, 0.5, 0.5);
410
411 assert_eq!(ray_aabb_intersection(start, direction, min, max), Some(1.5));
412 }
413
414 {
415 let start = Vector3::new(0.0, 0.0, -2.0);
416 let direction = Vector3::new(0.0, 0.0, 1.0);
417 let min = Vector3::new(-0.5, -0.5, -0.5);
418 let max = Vector3::new(0.5, 0.5, 0.5);
419
420 assert_eq!(ray_aabb_intersection(start, direction, min, max), Some(1.5));
421 }
422
423 {
424 let start = Vector3::new(0.0, 1.0, -1.0);
425 let direction = normalize(Vector3::new(0.0, -1.0, 1.0));
426 let min = Vector3::new(-0.5, -0.5, -0.5);
427 let max = Vector3::new(0.5, 0.5, 0.5);
428
429 assert_eq!(ray_aabb_intersection(start, direction, min, max), Some(0.70710677));
430 }
431 }
432
433 #[test]
434 fn test_sphere_in_frustum() {
435 let sphere = Sphere {
436 center: Vector3::new(0.0, 0.0, 0.0),
437 radius: 1.0,
438 };
439
440 let frustum_planes = [
441 Plane::new(normalize(Vector3::new(1.0, 0.0, 0.0)), 1.0), Plane::new(normalize(Vector3::new(-1.0, 0.0, 0.0)), 1.0), Plane::new(normalize(Vector3::new(0.0, 1.0, 0.0)), 1.0), Plane::new(normalize(Vector3::new(0.0, -1.0, 0.0)), 1.0), Plane::new(normalize(Vector3::new(0.0, 0.0, 1.0)), 1.0), Plane::new(normalize(Vector3::new(0.0, 0.0, -1.0)), 1.0), ];
448
449 assert!(sphere_in_frustum(&sphere, &frustum_planes));
450
451 let outside_sphere = Sphere {
452 center: Vector3::new(5.0, 5.0, 5.0),
453 radius: 1.5,
454 };
455
456 assert!(!sphere_in_frustum(&outside_sphere, &frustum_planes));
457
458 let edge_sphere = Sphere {
459 center: Vector3::new(1.0, 0.0, 0.0),
460 radius: 1.0,
461 };
462
463 assert!(sphere_in_frustum(&edge_sphere, &frustum_planes));
464
465 let edge_sphere_outside = Sphere {
466 center: Vector3::new(2.0, 0.0, 0.0),
467 radius: 1.0,
468 };
469
470 assert!(sphere_in_frustum(&edge_sphere_outside, &frustum_planes));
471 }
472
473 #[test]
474 fn test_sphere_vs_sphere() {
475 let sphere_a = Sphere {
476 center: Vector3::new(0.0, 0.0, 0.0),
477 radius: 1.0,
478 };
479
480 let sphere_b = Sphere {
481 center: Vector3::new(1.98, 0.0, 0.0),
482 radius: 1.0,
483 };
484
485 assert_matches!(sphere_vs_sphere(&sphere_a, &sphere_b), Some(_));
486 }
487
488 #[test]
489 fn test_cube_vs_cube() {
490 let cube_a = Cube {
491 center: Vector3::new(0.0, 0.0, 0.0),
492 half_size: Vector3::new(1.0, 1.0, 1.0),
493 };
494
495 let cube_b = Cube {
496 center: Vector3::new(1.0, 0.0, 0.0),
497 half_size: Vector3::new(1.0, 1.0, 1.0),
498 };
499
500 assert_matches!(cube_vs_cube(&cube_a, &cube_b), Some(_));
501 }
502
503 #[test]
504 fn test_sphere_vs_cube() {
505 let sphere = Sphere {
506 center: Vector3::new(0.0, 2.0, 0.0),
507 radius: 1.0,
508 };
509
510 let cube = Cube {
511 center: Vector3::new(0.0, 0.0, 0.0),
512 half_size: Vector3::new(1.0, 1.0, 1.0),
513 };
514
515 assert_matches!(sphere_vs_cube(&sphere, &cube), Some(_));
516
517 let sphere = Sphere {
518 center: Vector3::new(0.0, 3.0, 0.0),
519 radius: 1.0,
520 };
521
522 let cube = Cube {
523 center: Vector3::new(0.0, 0.0, 0.0),
524 half_size: Vector3::new(1.0, 1.0, 1.0),
525 };
526
527 assert_matches!(sphere_vs_cube(&sphere, &cube), None);
528 }
529
530 #[test]
531 fn sphere_vs_cube_uses_closest_cube_surface_point() {
532 let sphere = Sphere::new(Vector3::new(0.0, 0.9, 0.0), 0.5);
533 let cube = Cube::new(Vector3::zero(), Vector3::new(0.5, 0.5, 0.5));
534
535 let intersection = sphere_vs_cube(&sphere, &cube).unwrap();
536
537 assert_eq!(intersection.point_on_b, Vector3::new(0.0, 0.5, 0.0));
538 assert!((intersection.depth - 0.1).abs() < 1e-6);
539 }
540
541 #[test]
542 fn sphere_vs_cube_pushes_out_from_nearest_face_when_center_is_inside() {
543 let sphere = Sphere::new(Vector3::new(0.0, 0.49, 0.0), 0.5);
544 let cube = Cube::new(Vector3::zero(), Vector3::new(0.5, 0.5, 0.5));
545
546 let intersection = sphere_vs_cube(&sphere, &cube).unwrap();
547
548 assert_eq!(intersection.normal, Vector3::new(0.0, 1.0, 0.0));
549 assert_eq!(intersection.point_on_b, Vector3::new(0.0, 0.5, 0.0));
550 assert!((intersection.depth - 0.51).abs() < 1e-6);
551 }
552
553 #[test]
554 fn sphere_vs_sphere_dynamic_detects_approaching_spheres_before_overlap() {
555 let sphere_a = Sphere::new(Vector3::new(0.0, 0.0, 0.0), 1.0);
556 let sphere_b = Sphere::new(Vector3::new(4.0, 0.0, 0.0), 1.0);
557
558 let intersection = sphere_vs_sphere_dynamic(
559 &sphere_a,
560 &sphere_b,
561 Vector3::new(1.0, 0.0, 0.0),
562 Vector3::new(-1.0, 0.0, 0.0),
563 2.0,
564 )
565 .unwrap();
566
567 assert!((intersection.toi - 1.0).abs() < 1e-6);
568 assert!(intersection.depth <= 1e-6);
569 assert_eq!(intersection.normal, Vector3::new(1.0, 0.0, 0.0));
570 }
571
572 #[test]
573 fn sphere_vs_sphere_dynamic_reports_positive_depth_for_initial_overlap() {
574 let sphere_a = Sphere::new(Vector3::new(0.0, 0.0, 0.0), 1.0);
575 let sphere_b = Sphere::new(Vector3::new(1.5, 0.0, 0.0), 1.0);
576
577 let intersection = sphere_vs_sphere_dynamic(&sphere_a, &sphere_b, Vector3::zero(), Vector3::zero(), 1.0).unwrap();
578
579 assert_eq!(intersection.toi, 0.0);
580 assert!((intersection.depth - 0.5).abs() < 1e-6);
581 assert_eq!(intersection.normal, Vector3::new(1.0, 0.0, 0.0));
582 }
583
584 #[test]
585 fn test_ray_vs_sphere() {
586 let ray = Ray::new(Vector3::zero(), Vector3::new(0.0, 0.0, 1.0));
587 let sphere = Sphere::new(Vector3::new(0.0, 0.0, 10.0), 1.0);
588
589 let result = ray_vs_sphere(&ray, &sphere);
590
591 assert_matches!(result, Some(_));
592
593 let ray = Ray::new(Vector3::zero(), Vector3::new(0.0, 0.0, 1.0));
594 let sphere = Sphere::new(Vector3::new(0.0, 4.0, 10.0), 1.0);
595
596 let result = ray_vs_sphere(&ray, &sphere);
597
598 assert_matches!(result, None);
599 }
600}