Skip to main content

math/
collision.rs

1use maths_rs::dot;
2
3use crate::{cube::Cube, magnitude_squared, normalize, plane::Plane, ray::Ray, sphere::Sphere, Vector3};
4
5/// Returns the first intersection time between a ray and an axis-aligned bounding box.
6pub 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
29/// Returns whether a sphere is inside, or touches, a frustum.
30///
31/// The six plane normals must point toward the inside of the frustum.
32///
33/// # Arguments
34/// * `sphere` - The sphere to test.
35/// * `frustum_planes` - The six planes that define the frustum.
36///
37/// # Returns
38/// Returns `true` when any part of the sphere is inside the frustum. Returns
39/// `false` when the sphere is completely outside at least one plane.
40pub fn sphere_in_frustum(sphere: &Sphere, frustum_planes: &[Plane; 6]) -> bool {
41	// For a sphere to be visible, it must be on the "inside" or "positive" side
42	// of all frustum planes (or intersecting them).
43	// The "inside" is the half-space in the direction of the plane's normal.
44
45	for plane in frustum_planes {
46		if !plane.is_sphere_in_half_space(sphere) {
47			// Sphere is entirely outside this plane, so it's not visible.
48			return false;
49		}
50	}
51
52	// Sphere is not completely outside any of the frustum planes, so it is considered visible.
53	true
54}
55
56#[derive(Debug, Clone, PartialEq)]
57pub struct Intersection {
58	pub normal: Vector3,
59	pub depth: f32,
60	/// The contact point on shape A, in world coordinates.
61	pub point_on_a: Vector3,
62	/// The contact point on shape B, in world coordinates.
63	pub point_on_b: Vector3,
64}
65
66impl Intersection {
67	/// Returns this intersection with the contact points swapped.
68	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	// Coincident centers need a stable fallback normal so penetration correction does
91	// not produce NaNs while the solver separates the pair.
92	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	/// The contact point on shape A, in world coordinates.
121	pub point_on_a: Vector3,
122	/// The contact point on shape B, in world coordinates.
123	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		// When the sphere center is inside the AABB, the closest clamped point is the
264		// center itself. Use the nearest face so correction pushes the sphere outside.
265		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
328/// Returns the intersection times of a ray and a sphere, if one exists.
329pub 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	// With no relative motion, this is just a static overlap at the start of the step.
362	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), // Left plane x >= -1 (normal points +X)
442			Plane::new(normalize(Vector3::new(-1.0, 0.0, 0.0)), 1.0), // Right plane x <= 1 (normal points -X)
443			Plane::new(normalize(Vector3::new(0.0, 1.0, 0.0)), 1.0), // Bottom plane y >= -1 (normal points +Y)
444			Plane::new(normalize(Vector3::new(0.0, -1.0, 0.0)), 1.0), // Top plane y <= 1 (normal points -Y)
445			Plane::new(normalize(Vector3::new(0.0, 0.0, 1.0)), 1.0), // Near plane z >= -1 (normal points +Z)
446			Plane::new(normalize(Vector3::new(0.0, 0.0, -1.0)), 1.0), // Far plane z <= 1 (normal points -Z)
447		];
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}