Add RayTest2d and RayTest3d (#11310)

# Objective

Implement a raycast intersection test for bounding volumes

## Solution

- Implement RayTest2d and RayTest3d types

---------

Co-authored-by: Alice Cecile <[email protected]>
Co-authored-by: IQuick 143 <[email protected]>

Author: 3 people

crates/bevy_math/src/bounding/mod.rs

@@ -59,3 +59,8 @@ mod bounded2d;
 pub use bounded2d::*;
 mod bounded3d;
 pub use bounded3d::*;
+
+mod raytest2d;
+pub use raytest2d::*;
+mod raytest3d;
+pub use raytest3d::*;

crates/bevy_math/src/bounding/raytest2d.rs

@@ -0,0 +1,330 @@
+use super::{Aabb2d, BoundingCircle, IntersectsVolume};
+use crate::{primitives::Direction2d, Ray2d, Vec2};
+
+/// A raycast intersection test for 2D bounding volumes
+#[derive(Debug)]
+pub struct RayTest2d {
+    /// The ray for the test
+    pub ray: Ray2d,
+    /// The maximum distance for the ray
+    pub max: f32,
+    /// The multiplicative inverse direction of the ray
+    direction_recip: Vec2,
+}
+
+impl RayTest2d {
+    /// Construct a [`RayTest2d`] from an origin, [`Direction2d`] and max distance.
+    pub fn new(origin: Vec2, direction: Direction2d, max: f32) -> Self {
+        Self::from_ray(Ray2d { origin, direction }, max)
+    }
+
+    /// Construct a [`RayTest2d`] from a [`Ray2d`] and max distance.
+    pub fn from_ray(ray: Ray2d, max: f32) -> Self {
+        Self {
+            ray,
+            direction_recip: ray.direction.recip(),
+            max,
+        }
+    }
+
+    /// Get the cached multiplicative inverse of the direction of the ray.
+    pub fn direction_recip(&self) -> Vec2 {
+        self.direction_recip
+    }
+
+    /// Get the distance of an intersection with an [`Aabb2d`], if any.
+    pub fn aabb_intersection_at(&self, aabb: &Aabb2d) -> Option<f32> {
+        let (min_x, max_x) = if self.ray.direction.x.is_sign_positive() {
+            (aabb.min.x, aabb.max.x)
+        } else {
+            (aabb.max.x, aabb.min.x)
+        };
+        let (min_y, max_y) = if self.ray.direction.y.is_sign_positive() {
+            (aabb.min.y, aabb.max.y)
+        } else {
+            (aabb.max.y, aabb.min.y)
+        };
+
+        // Calculate the minimum/maximum time for each axis based on how much the direction goes that
+        // way. These values can get arbitrarily large, or even become NaN, which is handled by the
+        // min/max operations below
+        let tmin_x = (min_x - self.ray.origin.x) * self.direction_recip.x;
+        let tmin_y = (min_y - self.ray.origin.y) * self.direction_recip.y;
+        let tmax_x = (max_x - self.ray.origin.x) * self.direction_recip.x;
+        let tmax_y = (max_y - self.ray.origin.y) * self.direction_recip.y;
+
+        // An axis that is not relevant to the ray direction will be NaN. When one of the arguments
+        // to min/max is NaN, the other argument is used.
+        // An axis for which the direction is the wrong way will return an arbitrarily large
+        // negative value.
+        let tmin = tmin_x.max(tmin_y).max(0.);
+        let tmax = tmax_y.min(tmax_x).min(self.max);
+
+        if tmin <= tmax {
+            Some(tmin)
+        } else {
+            None
+        }
+    }
+
+    /// Get the distance of an intersection with a [`BoundingCircle`], if any.
+    pub fn circle_intersection_at(&self, circle: &BoundingCircle) -> Option<f32> {
+        let offset = self.ray.origin - circle.center;
+        let projected = offset.dot(*self.ray.direction);
+        let closest_point = offset - projected * *self.ray.direction;
+        let distance_squared = circle.radius().powi(2) - closest_point.length_squared();
+        if distance_squared < 0. || projected.powi(2).copysign(-projected) < -distance_squared {
+            None
+        } else {
+            let toi = -projected - distance_squared.sqrt();
+            if toi > self.max {
+                None
+            } else {
+                Some(toi.max(0.))
+            }
+        }
+    }
+}
+
+impl IntersectsVolume<Aabb2d> for RayTest2d {
+    fn intersects(&self, volume: &Aabb2d) -> bool {
+        self.aabb_intersection_at(volume).is_some()
+    }
+}
+
+impl IntersectsVolume<BoundingCircle> for RayTest2d {
+    fn intersects(&self, volume: &BoundingCircle) -> bool {
+        self.circle_intersection_at(volume).is_some()
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    const EPSILON: f32 = 0.001;
+
+    #[test]
+    fn test_ray_intersection_circle_hits() {
+        for (test, volume, expected_distance) in &[
+            (
+                // Hit the center of a centered bounding circle
+                RayTest2d::new(Vec2::Y * -5., Direction2d::Y, 90.),
+                BoundingCircle::new(Vec2::ZERO, 1.),
+                4.,
+            ),
+            (
+                // Hit the center of a centered bounding circle, but from the other side
+                RayTest2d::new(Vec2::Y * 5., -Direction2d::Y, 90.),
+                BoundingCircle::new(Vec2::ZERO, 1.),
+                4.,
+            ),
+            (
+                // Hit the center of an offset circle
+                RayTest2d::new(Vec2::ZERO, Direction2d::Y, 90.),
+                BoundingCircle::new(Vec2::Y * 3., 2.),
+                1.,
+            ),
+            (
+                // Just barely hit the circle before the max distance
+                RayTest2d::new(Vec2::X, Direction2d::Y, 1.),
+                BoundingCircle::new(Vec2::ONE, 0.01),
+                0.99,
+            ),
+            (
+                // Hit a circle off-center
+                RayTest2d::new(Vec2::X, Direction2d::Y, 90.),
+                BoundingCircle::new(Vec2::Y * 5., 2.),
+                3.268,
+            ),
+            (
+                // Barely hit a circle on the side
+                RayTest2d::new(Vec2::X * 0.99999, Direction2d::Y, 90.),
+                BoundingCircle::new(Vec2::Y * 5., 1.),
+                4.996,
+            ),
+        ] {
+            let case = format!(
+                "Case:\n  Test: {:?}\n  Volume: {:?}\n  Expected distance: {:?}",
+                test, volume, expected_distance
+            );
+            assert!(test.intersects(volume), "{}", case);
+            let actual_distance = test.circle_intersection_at(volume).unwrap();
+            assert!(
+                (actual_distance - expected_distance).abs() < EPSILON,
+                "{}\n  Actual distance: {}",
+                case,
+                actual_distance
+            );
+
+            let inverted_ray = RayTest2d::new(test.ray.origin, -test.ray.direction, test.max);
+            assert!(!inverted_ray.intersects(volume), "{}", case);
+        }
+    }
+
+    #[test]
+    fn test_ray_intersection_circle_misses() {
+        for (test, volume) in &[
+            (
+                // The ray doesn't go in the right direction
+                RayTest2d::new(Vec2::ZERO, Direction2d::X, 90.),
+                BoundingCircle::new(Vec2::Y * 2., 1.),
+            ),
+            (
+                // Ray's alignment isn't enough to hit the circle
+                RayTest2d::new(Vec2::ZERO, Direction2d::from_xy(1., 1.).unwrap(), 90.),
+                BoundingCircle::new(Vec2::Y * 2., 1.),
+            ),
+            (
+                // The ray's maximum distance isn't high enough
+                RayTest2d::new(Vec2::ZERO, Direction2d::Y, 0.5),
+                BoundingCircle::new(Vec2::Y * 2., 1.),
+            ),
+        ] {
+            assert!(
+                !test.intersects(volume),
+                "Case:\n  Test: {:?}\n  Volume: {:?}",
+                test,
+                volume,
+            );
+        }
+    }
+
+    #[test]
+    fn test_ray_intersection_circle_inside() {
+        let volume = BoundingCircle::new(Vec2::splat(0.5), 1.);
+        for origin in &[Vec2::X, Vec2::Y, Vec2::ONE, Vec2::ZERO] {
+            for direction in &[
+                Direction2d::X,
+                Direction2d::Y,
+                -Direction2d::X,
+                -Direction2d::Y,
+            ] {
+                for max in &[0., 1., 900.] {
+                    let test = RayTest2d::new(*origin, *direction, *max);
+
+                    let case = format!(
+                        "Case:\n  origin: {:?}\n  Direction: {:?}\n  Max: {}",
+                        origin, direction, max,
+                    );
+                    assert!(test.intersects(&volume), "{}", case);
+
+                    let actual_distance = test.circle_intersection_at(&volume);
+                    assert_eq!(actual_distance, Some(0.), "{}", case);
+                }
+            }
+        }
+    }
+
+    #[test]
+    fn test_ray_intersection_aabb_hits() {
+        for (test, volume, expected_distance) in &[
+            (
+                // Hit the center of a centered aabb
+                RayTest2d::new(Vec2::Y * -5., Direction2d::Y, 90.),
+                Aabb2d::new(Vec2::ZERO, Vec2::ONE),
+                4.,
+            ),
+            (
+                // Hit the center of a centered aabb, but from the other side
+                RayTest2d::new(Vec2::Y * 5., -Direction2d::Y, 90.),
+                Aabb2d::new(Vec2::ZERO, Vec2::ONE),
+                4.,
+            ),
+            (
+                // Hit the center of an offset aabb
+                RayTest2d::new(Vec2::ZERO, Direction2d::Y, 90.),
+                Aabb2d::new(Vec2::Y * 3., Vec2::splat(2.)),
+                1.,
+            ),
+            (
+                // Just barely hit the aabb before the max distance
+                RayTest2d::new(Vec2::X, Direction2d::Y, 1.),
+                Aabb2d::new(Vec2::ONE, Vec2::splat(0.01)),
+                0.99,
+            ),
+            (
+                // Hit an aabb off-center
+                RayTest2d::new(Vec2::X, Direction2d::Y, 90.),
+                Aabb2d::new(Vec2::Y * 5., Vec2::splat(2.)),
+                3.,
+            ),
+            (
+                // Barely hit an aabb on corner
+                RayTest2d::new(Vec2::X * -0.001, Direction2d::from_xy(1., 1.).unwrap(), 90.),
+                Aabb2d::new(Vec2::Y * 2., Vec2::ONE),
+                1.414,
+            ),
+        ] {
+            let case = format!(
+                "Case:\n  Test: {:?}\n  Volume: {:?}\n  Expected distance: {:?}",
+                test, volume, expected_distance
+            );
+            assert!(test.intersects(volume), "{}", case);
+            let actual_distance = test.aabb_intersection_at(volume).unwrap();
+            assert!(
+                (actual_distance - expected_distance).abs() < EPSILON,
+                "{}\n  Actual distance: {}",
+                case,
+                actual_distance
+            );
+
+            let inverted_ray = RayTest2d::new(test.ray.origin, -test.ray.direction, test.max);
+            assert!(!inverted_ray.intersects(volume), "{}", case);
+        }
+    }
+
+    #[test]
+    fn test_ray_intersection_aabb_misses() {
+        for (test, volume) in &[
+            (
+                // The ray doesn't go in the right direction
+                RayTest2d::new(Vec2::ZERO, Direction2d::X, 90.),
+                Aabb2d::new(Vec2::Y * 2., Vec2::ONE),
+            ),
+            (
+                // Ray's alignment isn't enough to hit the aabb
+                RayTest2d::new(Vec2::ZERO, Direction2d::from_xy(1., 0.99).unwrap(), 90.),
+                Aabb2d::new(Vec2::Y * 2., Vec2::ONE),
+            ),
+            (
+                // The ray's maximum distance isn't high enough
+                RayTest2d::new(Vec2::ZERO, Direction2d::Y, 0.5),
+                Aabb2d::new(Vec2::Y * 2., Vec2::ONE),
+            ),
+        ] {
+            assert!(
+                !test.intersects(volume),
+                "Case:\n  Test: {:?}\n  Volume: {:?}",
+                test,
+                volume,
+            );
+        }
+    }
+
+    #[test]
+    fn test_ray_intersection_aabb_inside() {
+        let volume = Aabb2d::new(Vec2::splat(0.5), Vec2::ONE);
+        for origin in &[Vec2::X, Vec2::Y, Vec2::ONE, Vec2::ZERO] {
+            for direction in &[
+                Direction2d::X,
+                Direction2d::Y,
+                -Direction2d::X,
+                -Direction2d::Y,
+            ] {
+                for max in &[0., 1., 900.] {
+                    let test = RayTest2d::new(*origin, *direction, *max);
+
+                    let case = format!(
+                        "Case:\n  origin: {:?}\n  Direction: {:?}\n  Max: {}",
+                        origin, direction, max,
+                    );
+                    assert!(test.intersects(&volume), "{}", case);
+
+                    let actual_distance = test.aabb_intersection_at(&volume);
+                    assert_eq!(actual_distance, Some(0.), "{}", case,);
+                }
+            }
+        }
+    }
+}