Implement bounding volume intersections

crates/bevy_math/src/bounding/bounded2d/mod.rs

@@ -2,7 +2,7 @@ mod primitive_impls;
 
 use glam::Mat2;
 
-use super::BoundingVolume;
+use super::{BoundingVolume, IntersectsVolume};
 use crate::prelude::Vec2;
 
 /// Computes the geometric center of the given set of points.
@@ -70,6 +70,16 @@ impl Aabb2d {
         let radius = self.min.distance(self.max) / 2.0;
         BoundingCircle::new(self.center(), radius)
     }
+
+    /// Finds the point on the AABB that is closest to the given `point`.
+    ///
+    /// If the point is outside the AABB, the returned point will be on the surface of the AABB.
+    /// Otherwise, it will be inside the AABB and returned as is.
+    #[inline(always)]
+    pub fn closest_point(&self, point: Vec2) -> Vec2 {
+        // Clamp point coordinates to the AABB
+        point.clamp(self.min, self.max)
+    }
 }
 
 impl BoundingVolume for Aabb2d {
@@ -129,10 +139,32 @@ impl BoundingVolume for Aabb2d {
     }
 }
 
+impl IntersectsVolume<Self> for Aabb2d {
+    #[inline(always)]
+    fn intersects(&self, other: &Self) -> bool {
+        let x_overlaps = self.min.x <= other.max.x && self.max.x >= other.min.x;
+        let y_overlaps = self.min.y <= other.max.y && self.max.y >= other.min.y;
+        x_overlaps && y_overlaps
+    }
+}
+
+impl IntersectsVolume<BoundingCircle> for Aabb2d {
+    #[inline(always)]
+    fn intersects(&self, circle: &BoundingCircle) -> bool {
+        let closest_point = self.closest_point(circle.center);
+        let distance_squared = circle.center.distance_squared(closest_point);
+        let radius_squared = circle.radius().powi(2);
+        distance_squared < radius_squared
+    }
+}
+
 #[cfg(test)]
 mod aabb2d_tests {
     use super::Aabb2d;
-    use crate::{bounding::BoundingVolume, Vec2};
+    use crate::{
+        bounding::{BoundingCircle, BoundingVolume, IntersectsVolume},
+        Vec2,
+    };
 
     #[test]
     fn center() {
@@ -234,6 +266,53 @@ mod aabb2d_tests {
         assert!(a.contains(&shrunk));
         assert!(!shrunk.contains(&a));
     }
+
+    #[test]
+    fn closest_point() {
+        let aabb = Aabb2d {
+            min: Vec2::NEG_ONE,
+            max: Vec2::ONE,
+        };
+        assert_eq!(aabb.closest_point(Vec2::X * 10.0), Vec2::X);
+        assert_eq!(aabb.closest_point(Vec2::NEG_ONE * 10.0), Vec2::NEG_ONE);
+        assert_eq!(
+            aabb.closest_point(Vec2::new(0.25, 0.1)),
+            Vec2::new(0.25, 0.1)
+        );
+    }
+
+    #[test]
+    fn intersect_aabb() {
+        let aabb = Aabb2d {
+            min: Vec2::NEG_ONE,
+            max: Vec2::ONE,
+        };
+        assert!(aabb.intersects(&aabb));
+        assert!(aabb.intersects(&Aabb2d {
+            min: Vec2::new(0.5, 0.5),
+            max: Vec2::new(2.0, 2.0),
+        }));
+        assert!(aabb.intersects(&Aabb2d {
+            min: Vec2::new(-2.0, -2.0),
+            max: Vec2::new(-0.5, -0.5),
+        }));
+        assert!(!aabb.intersects(&Aabb2d {
+            min: Vec2::new(1.1, 0.0),
+            max: Vec2::new(2.0, 0.5),
+        }));
+    }
+
+    #[test]
+    fn intersect_bounding_circle() {
+        let aabb = Aabb2d {
+            min: Vec2::NEG_ONE,
+            max: Vec2::ONE,
+        };
+        assert!(aabb.intersects(&BoundingCircle::new(Vec2::ZERO, 1.0)));
+        assert!(aabb.intersects(&BoundingCircle::new(Vec2::ONE * 1.5, 1.0)));
+        assert!(aabb.intersects(&BoundingCircle::new(Vec2::NEG_ONE * 1.5, 1.0)));
+        assert!(!aabb.intersects(&BoundingCircle::new(Vec2::ONE * 1.75, 1.0)));
+    }
 }
 
 use crate::primitives::Circle;
@@ -295,6 +374,26 @@ impl BoundingCircle {
             max: self.center + Vec2::splat(self.radius()),
         }
     }
+
+    /// Finds the point on the bounding circle that is closest to the given `point`.
+    ///
+    /// If the point is outside the circle, the returned point will be on the surface of the circle.
+    /// Otherwise, it will be inside the circle and returned as is.
+    #[inline(always)]
+    pub fn closest_point(&self, point: Vec2) -> Vec2 {
+        let offset_from_center = point - self.center;
+        let distance_to_center_squared = offset_from_center.length_squared();
+
+        if distance_to_center_squared <= self.radius().powi(2) {
+            // The point is inside the circle
+            point
+        } else {
+            // The point is outside the circle.
+            // Find the closest point on the surface of the circle.
+            let dir_to_point = offset_from_center / distance_to_center_squared.sqrt();
+            self.center() + self.radius() * dir_to_point
+        }
+    }
 }
 
 impl BoundingVolume for BoundingCircle {
@@ -353,10 +452,29 @@ impl BoundingVolume for BoundingCircle {
     }
 }
 
+impl IntersectsVolume<Self> for BoundingCircle {
+    #[inline(always)]
+    fn intersects(&self, other: &Self) -> bool {
+        let center_distance_squared = self.center.distance_squared(other.center);
+        let radius_sum_squared = (self.radius() + other.radius()).powi(2);
+        center_distance_squared <= radius_sum_squared
+    }
+}
+
+impl IntersectsVolume<Aabb2d> for BoundingCircle {
+    #[inline(always)]
+    fn intersects(&self, aabb: &Aabb2d) -> bool {
+        aabb.intersects(self)
+    }
+}
+
 #[cfg(test)]
 mod bounding_circle_tests {
     use super::BoundingCircle;
-    use crate::{bounding::BoundingVolume, Vec2};
+    use crate::{
+        bounding::{BoundingVolume, IntersectsVolume},
+        Vec2,
+    };
 
     #[test]
     fn area() {
@@ -433,4 +551,27 @@ mod bounding_circle_tests {
         assert!(a.contains(&shrunk));
         assert!(!shrunk.contains(&a));
     }
+
+    #[test]
+    fn closest_point() {
+        let circle = BoundingCircle::new(Vec2::ZERO, 1.0);
+        assert_eq!(circle.closest_point(Vec2::X * 10.0), Vec2::X);
+        assert_eq!(
+            circle.closest_point(Vec2::NEG_ONE * 10.0),
+            Vec2::NEG_ONE.normalize()
+        );
+        assert_eq!(
+            circle.closest_point(Vec2::new(0.25, 0.1)),
+            Vec2::new(0.25, 0.1)
+        );
+    }
+
+    #[test]
+    fn intersect_bounding_circle() {
+        let circle = BoundingCircle::new(Vec2::ZERO, 1.0);
+        assert!(circle.intersects(&BoundingCircle::new(Vec2::ZERO, 1.0)));
+        assert!(circle.intersects(&BoundingCircle::new(Vec2::ONE * 1.25, 1.0)));
+        assert!(circle.intersects(&BoundingCircle::new(Vec2::NEG_ONE * 1.25, 1.0)));
+        assert!(!circle.intersects(&BoundingCircle::new(Vec2::ONE * 1.5, 1.0)));
+    }
 }

crates/bevy_math/src/bounding/bounded3d/mod.rs

@@ -1,6 +1,6 @@
 mod primitive_impls;
 
-use super::BoundingVolume;
+use super::{BoundingVolume, IntersectsVolume};
 use crate::prelude::{Quat, Vec3};
 
 /// Computes the geometric center of the given set of points.
@@ -64,6 +64,16 @@ impl Aabb3d {
         let radius = self.min.distance(self.max) / 2.0;
         BoundingSphere::new(self.center(), radius)
     }
+
+    /// Finds the point on the AABB that is closest to the given `point`.
+    ///
+    /// If the point is outside the AABB, the returned point will be on the surface of the AABB.
+    /// Otherwise, it will be inside the AABB and returned as is.
+    #[inline(always)]
+    pub fn closest_point(&self, point: Vec3) -> Vec3 {
+        // Clamp point coordinates to the AABB
+        point.clamp(self.min, self.max)
+    }
 }
 
 impl BoundingVolume for Aabb3d {
@@ -125,10 +135,33 @@ impl BoundingVolume for Aabb3d {
     }
 }
 
+impl IntersectsVolume<Self> for Aabb3d {
+    #[inline(always)]
+    fn intersects(&self, other: &Self) -> bool {
+        let x_overlaps = self.min.x <= other.max.x && self.max.x >= other.min.x;
+        let y_overlaps = self.min.y <= other.max.y && self.max.y >= other.min.y;
+        let z_overlaps = self.min.z <= other.max.z && self.max.z >= other.min.z;
+        x_overlaps && y_overlaps && z_overlaps
+    }
+}
+
+impl IntersectsVolume<BoundingSphere> for Aabb3d {
+    #[inline(always)]
+    fn intersects(&self, sphere: &BoundingSphere) -> bool {
+        let closest_point = self.closest_point(sphere.center);
+        let distance_squared = sphere.center.distance_squared(closest_point);
+        let radius_squared = sphere.radius().powi(2);
+        distance_squared < radius_squared
+    }
+}
+
 #[cfg(test)]
 mod aabb3d_tests {
     use super::Aabb3d;
-    use crate::{bounding::BoundingVolume, Vec3};
+    use crate::{
+        bounding::{BoundingSphere, BoundingVolume, IntersectsVolume},
+        Vec3,
+    };
 
     #[test]
     fn center() {
@@ -229,6 +262,53 @@ mod aabb3d_tests {
         assert!(a.contains(&shrunk));
         assert!(!shrunk.contains(&a));
     }
+
+    #[test]
+    fn closest_point() {
+        let aabb = Aabb3d {
+            min: Vec3::NEG_ONE,
+            max: Vec3::ONE,
+        };
+        assert_eq!(aabb.closest_point(Vec3::X * 10.0), Vec3::X);
+        assert_eq!(aabb.closest_point(Vec3::NEG_ONE * 10.0), Vec3::NEG_ONE);
+        assert_eq!(
+            aabb.closest_point(Vec3::new(0.25, 0.1, 0.3)),
+            Vec3::new(0.25, 0.1, 0.3)
+        );
+    }
+
+    #[test]
+    fn intersect_aabb() {
+        let aabb = Aabb3d {
+            min: Vec3::NEG_ONE,
+            max: Vec3::ONE,
+        };
+        assert!(aabb.intersects(&aabb));
+        assert!(aabb.intersects(&Aabb3d {
+            min: Vec3::splat(0.5),
+            max: Vec3::splat(2.0),
+        }));
+        assert!(aabb.intersects(&Aabb3d {
+            min: Vec3::splat(-2.0),
+            max: Vec3::splat(-0.5),
+        }));
+        assert!(!aabb.intersects(&Aabb3d {
+            min: Vec3::new(1.1, 0.0, 0.0),
+            max: Vec3::new(2.0, 0.5, 0.25),
+        }));
+    }
+
+    #[test]
+    fn intersect_bounding_sphere() {
+        let aabb = Aabb3d {
+            min: Vec3::NEG_ONE,
+            max: Vec3::ONE,
+        };
+        assert!(aabb.intersects(&BoundingSphere::new(Vec3::ZERO, 1.0)));
+        assert!(aabb.intersects(&BoundingSphere::new(Vec3::ONE * 1.5, 1.0)));
+        assert!(aabb.intersects(&BoundingSphere::new(Vec3::NEG_ONE * 1.5, 1.0)));
+        assert!(!aabb.intersects(&BoundingSphere::new(Vec3::ONE * 1.75, 1.0)));
+    }
 }
 
 use crate::primitives::Sphere;
@@ -286,6 +366,26 @@ impl BoundingSphere {
             max: self.center + Vec3::splat(self.radius()),
         }
     }
+
+    /// Finds the point on the bounding sphere that is closest to the given `point`.
+    ///
+    /// If the point is outside the sphere, the returned point will be on the surface of the sphere.
+    /// Otherwise, it will be inside the sphere and returned as is.
+    #[inline(always)]
+    pub fn closest_point(&self, point: Vec3) -> Vec3 {
+        let offset_from_center = point - self.center;
+        let distance_to_center_squared = offset_from_center.length_squared();
+
+        if distance_to_center_squared <= self.radius().powi(2) {
+            // The point is inside the sphere
+            point
+        } else {
+            // The point is outside the sphere.
+            // Find the closest point on the surface of the sphere.
+            let dir_to_point = offset_from_center / distance_to_center_squared.sqrt();
+            self.center() + self.radius() * dir_to_point
+        }
+    }
 }
 
 impl BoundingVolume for BoundingSphere {
@@ -354,10 +454,29 @@ impl BoundingVolume for BoundingSphere {
     }
 }
 
+impl IntersectsVolume<Self> for BoundingSphere {
+    #[inline(always)]
+    fn intersects(&self, other: &Self) -> bool {
+        let center_distance_squared = self.center.distance_squared(other.center);
+        let radius_sum_squared = (self.radius() + other.radius()).powi(2);
+        center_distance_squared <= radius_sum_squared
+    }
+}
+
+impl IntersectsVolume<Aabb3d> for BoundingSphere {
+    #[inline(always)]
+    fn intersects(&self, aabb: &Aabb3d) -> bool {
+        aabb.intersects(self)
+    }
+}
+
 #[cfg(test)]
 mod bounding_sphere_tests {
     use super::BoundingSphere;
-    use crate::{bounding::BoundingVolume, Vec3};
+    use crate::{
+        bounding::{BoundingVolume, IntersectsVolume},
+        Vec3,
+    };
 
     #[test]
     fn area() {
@@ -434,4 +553,27 @@ mod bounding_sphere_tests {
         assert!(a.contains(&shrunk));
         assert!(!shrunk.contains(&a));
     }
+
+    #[test]
+    fn closest_point() {
+        let sphere = BoundingSphere::new(Vec3::ZERO, 1.0);
+        assert_eq!(sphere.closest_point(Vec3::X * 10.0), Vec3::X);
+        assert_eq!(
+            sphere.closest_point(Vec3::NEG_ONE * 10.0),
+            Vec3::NEG_ONE.normalize()
+        );
+        assert_eq!(
+            sphere.closest_point(Vec3::new(0.25, 0.1, 0.3)),
+            Vec3::new(0.25, 0.1, 0.3)
+        );
+    }
+
+    #[test]
+    fn intersect_bounding_sphere() {
+        let sphere = BoundingSphere::new(Vec3::ZERO, 1.0);
+        assert!(sphere.intersects(&BoundingSphere::new(Vec3::ZERO, 1.0)));
+        assert!(sphere.intersects(&BoundingSphere::new(Vec3::ONE * 1.1, 1.0)));
+        assert!(sphere.intersects(&BoundingSphere::new(Vec3::NEG_ONE * 1.1, 1.0)));
+        assert!(!sphere.intersects(&BoundingSphere::new(Vec3::ONE * 1.2, 1.0)));
+    }
 }