From 735977c5933c671065bd9ddcb1733c74decdf7d9 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 20 Jan 2024 19:22:16 +0200 Subject: [PATCH 1/5] Implement intersections between bounding volumes --- .../bevy_math/src/bounding/bounded2d/mod.rs | 67 +++++++++++++++++- .../bevy_math/src/bounding/bounded3d/mod.rs | 68 ++++++++++++++++++- 2 files changed, 133 insertions(+), 2 deletions(-) diff --git a/crates/bevy_math/src/bounding/bounded2d/mod.rs b/crates/bevy_math/src/bounding/bounded2d/mod.rs index 163a3776bbbaa..9509224ab1189 100644 --- a/crates/bevy_math/src/bounding/bounded2d/mod.rs +++ b/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,6 +139,25 @@ impl BoundingVolume for Aabb2d { } } +impl IntersectsVolume 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 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; @@ -295,6 +324,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,6 +402,22 @@ impl BoundingVolume for BoundingCircle { } } +impl IntersectsVolume 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 for BoundingCircle { + #[inline(always)] + fn intersects(&self, aabb: &Aabb2d) -> bool { + aabb.intersects(self) + } +} + #[cfg(test)] mod bounding_circle_tests { use super::BoundingCircle; diff --git a/crates/bevy_math/src/bounding/bounded3d/mod.rs b/crates/bevy_math/src/bounding/bounded3d/mod.rs index f46a68ffd45a9..ae60e3e67c49a 100644 --- a/crates/bevy_math/src/bounding/bounded3d/mod.rs +++ b/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,6 +135,26 @@ impl BoundingVolume for Aabb3d { } } +impl IntersectsVolume 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 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; @@ -286,6 +316,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,6 +404,22 @@ impl BoundingVolume for BoundingSphere { } } +impl IntersectsVolume 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 for BoundingSphere { + #[inline(always)] + fn intersects(&self, aabb: &Aabb3d) -> bool { + aabb.intersects(self) + } +} + #[cfg(test)] mod bounding_sphere_tests { use super::BoundingSphere; From f0f522b4641dcb8b3c2fd55366c8dfb99a8131cb Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 20 Jan 2024 20:00:05 +0200 Subject: [PATCH 2/5] Add tests for bounding volume intersections --- .../bevy_math/src/bounding/bounded2d/mod.rs | 52 ++++++++++++++++++- .../bevy_math/src/bounding/bounded3d/mod.rs | 52 ++++++++++++++++++- 2 files changed, 100 insertions(+), 4 deletions(-) diff --git a/crates/bevy_math/src/bounding/bounded2d/mod.rs b/crates/bevy_math/src/bounding/bounded2d/mod.rs index 9509224ab1189..08a6332bdaf4a 100644 --- a/crates/bevy_math/src/bounding/bounded2d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded2d/mod.rs @@ -161,7 +161,10 @@ impl IntersectsVolume for Aabb2d { #[cfg(test)] mod aabb2d_tests { use super::Aabb2d; - use crate::{bounding::BoundingVolume, Vec2}; + use crate::{ + bounding::{BoundingCircle, BoundingVolume, IntersectsVolume}, + Vec2, + }; #[test] fn center() { @@ -263,6 +266,39 @@ mod aabb2d_tests { assert!(a.contains(&shrunk)); assert!(!shrunk.contains(&a)); } + + #[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; @@ -421,7 +457,10 @@ impl IntersectsVolume for BoundingCircle { #[cfg(test)] mod bounding_circle_tests { use super::BoundingCircle; - use crate::{bounding::BoundingVolume, Vec2}; + use crate::{ + bounding::{BoundingVolume, IntersectsVolume}, + Vec2, + }; #[test] fn area() { @@ -498,4 +537,13 @@ mod bounding_circle_tests { assert!(a.contains(&shrunk)); assert!(!shrunk.contains(&a)); } + + #[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))); + } } diff --git a/crates/bevy_math/src/bounding/bounded3d/mod.rs b/crates/bevy_math/src/bounding/bounded3d/mod.rs index ae60e3e67c49a..4461495b574d9 100644 --- a/crates/bevy_math/src/bounding/bounded3d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded3d/mod.rs @@ -158,7 +158,10 @@ impl IntersectsVolume for Aabb3d { #[cfg(test)] mod aabb3d_tests { use super::Aabb3d; - use crate::{bounding::BoundingVolume, Vec3}; + use crate::{ + bounding::{BoundingSphere, BoundingVolume, IntersectsVolume}, + Vec3, + }; #[test] fn center() { @@ -259,6 +262,39 @@ mod aabb3d_tests { assert!(a.contains(&shrunk)); assert!(!shrunk.contains(&a)); } + + #[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; @@ -423,7 +459,10 @@ impl IntersectsVolume for BoundingSphere { #[cfg(test)] mod bounding_sphere_tests { use super::BoundingSphere; - use crate::{bounding::BoundingVolume, Vec3}; + use crate::{ + bounding::{BoundingVolume, IntersectsVolume}, + Vec3, + }; #[test] fn area() { @@ -500,4 +539,13 @@ mod bounding_sphere_tests { assert!(a.contains(&shrunk)); assert!(!shrunk.contains(&a)); } + + #[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))); + } } From 24e5b888e237a7220c7d09baccf550e224f8f56f Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 20 Jan 2024 20:06:12 +0200 Subject: [PATCH 3/5] Add tests for `closest_point` methods --- .../bevy_math/src/bounding/bounded2d/mod.rs | 28 +++++++++++++++++++ .../bevy_math/src/bounding/bounded3d/mod.rs | 28 +++++++++++++++++++ 2 files changed, 56 insertions(+) diff --git a/crates/bevy_math/src/bounding/bounded2d/mod.rs b/crates/bevy_math/src/bounding/bounded2d/mod.rs index 08a6332bdaf4a..7c364d25ee937 100644 --- a/crates/bevy_math/src/bounding/bounded2d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded2d/mod.rs @@ -267,6 +267,20 @@ mod aabb2d_tests { 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 { @@ -538,6 +552,20 @@ mod bounding_circle_tests { 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); diff --git a/crates/bevy_math/src/bounding/bounded3d/mod.rs b/crates/bevy_math/src/bounding/bounded3d/mod.rs index 4461495b574d9..41f898519eff2 100644 --- a/crates/bevy_math/src/bounding/bounded3d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded3d/mod.rs @@ -263,6 +263,20 @@ mod aabb3d_tests { 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 { @@ -540,6 +554,20 @@ mod bounding_sphere_tests { 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); From 6b01a1d5c3c59a5cfd7cc18c7df7e9d910d7a8ad Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 20 Jan 2024 20:36:40 +0200 Subject: [PATCH 4/5] Use `<=` instead of `<` in intersection test Co-authored-by: IQuick 143 --- crates/bevy_math/src/bounding/bounded2d/mod.rs | 2 +- crates/bevy_math/src/bounding/bounded3d/mod.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/crates/bevy_math/src/bounding/bounded2d/mod.rs b/crates/bevy_math/src/bounding/bounded2d/mod.rs index 7c364d25ee937..4326611c93344 100644 --- a/crates/bevy_math/src/bounding/bounded2d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded2d/mod.rs @@ -154,7 +154,7 @@ impl IntersectsVolume for Aabb2d { 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 + distance_squared <= radius_squared } } diff --git a/crates/bevy_math/src/bounding/bounded3d/mod.rs b/crates/bevy_math/src/bounding/bounded3d/mod.rs index 41f898519eff2..56ea8f69b8c56 100644 --- a/crates/bevy_math/src/bounding/bounded3d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded3d/mod.rs @@ -151,7 +151,7 @@ impl IntersectsVolume for Aabb3d { 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 + distance_squared <= radius_squared } } From a93da968a809d7440ab5ddbc37e0a39e9732bc8a Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 20 Jan 2024 21:16:27 +0200 Subject: [PATCH 5/5] Add `closest_point` to `Rectangle`, `Circle`, `Cuboid`, and `Sphere` --- .../bevy_math/src/bounding/bounded2d/mod.rs | 17 +----- .../bevy_math/src/bounding/bounded3d/mod.rs | 13 +---- crates/bevy_math/src/primitives/dim2.rs | 56 +++++++++++++++++++ crates/bevy_math/src/primitives/dim3.rs | 56 +++++++++++++++++++ 4 files changed, 116 insertions(+), 26 deletions(-) diff --git a/crates/bevy_math/src/bounding/bounded2d/mod.rs b/crates/bevy_math/src/bounding/bounded2d/mod.rs index 4326611c93344..a6943432fe41b 100644 --- a/crates/bevy_math/src/bounding/bounded2d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded2d/mod.rs @@ -73,7 +73,7 @@ impl Aabb2d { /// 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. + /// If the point is outside the AABB, the returned point will be on the perimeter of the AABB. /// Otherwise, it will be inside the AABB and returned as is. #[inline(always)] pub fn closest_point(&self, point: Vec2) -> Vec2 { @@ -377,22 +377,11 @@ impl BoundingCircle { /// 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. + /// If the point is outside the circle, the returned point will be on the perimeter 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 - } + self.circle.closest_point(point - self.center) + self.center } } diff --git a/crates/bevy_math/src/bounding/bounded3d/mod.rs b/crates/bevy_math/src/bounding/bounded3d/mod.rs index 56ea8f69b8c56..c5eaf136c82c8 100644 --- a/crates/bevy_math/src/bounding/bounded3d/mod.rs +++ b/crates/bevy_math/src/bounding/bounded3d/mod.rs @@ -373,18 +373,7 @@ impl BoundingSphere { /// 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 - } + self.sphere.closest_point(point - self.center) + self.center } } diff --git a/crates/bevy_math/src/primitives/dim2.rs b/crates/bevy_math/src/primitives/dim2.rs index cb832f9f614a3..fb6f35dd34265 100644 --- a/crates/bevy_math/src/primitives/dim2.rs +++ b/crates/bevy_math/src/primitives/dim2.rs @@ -82,6 +82,27 @@ pub struct Circle { } impl Primitive2d for Circle {} +impl Circle { + /// Finds the point on the circle that is closest to the given `point`. + /// + /// If the point is outside the circle, the returned point will be on the perimeter 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 distance_squared = point.length_squared(); + + if distance_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 perimeter of the circle. + let dir_to_point = point / distance_squared.sqrt(); + self.radius * dir_to_point + } + } +} + /// An ellipse primitive #[derive(Clone, Copy, Debug)] pub struct Ellipse { @@ -351,6 +372,16 @@ impl Rectangle { half_size: size / 2., } } + + /// Finds the point on the rectangle that is closest to the given `point`. + /// + /// If the point is outside the rectangle, the returned point will be on the perimeter of the rectangle. + /// Otherwise, it will be inside the rectangle and returned as is. + #[inline(always)] + pub fn closest_point(&self, point: Vec2) -> Vec2 { + // Clamp point coordinates to the rectangle + point.clamp(-self.half_size, self.half_size) + } } /// A polygon with N vertices. @@ -542,4 +573,29 @@ mod tests { < 1e-7, ); } + + #[test] + fn rectangle_closest_point() { + let rectangle = Rectangle::new(2.0, 2.0); + assert_eq!(rectangle.closest_point(Vec2::X * 10.0), Vec2::X); + assert_eq!(rectangle.closest_point(Vec2::NEG_ONE * 10.0), Vec2::NEG_ONE); + assert_eq!( + rectangle.closest_point(Vec2::new(0.25, 0.1)), + Vec2::new(0.25, 0.1) + ); + } + + #[test] + fn circle_closest_point() { + let circle = Circle { radius: 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) + ); + } } diff --git a/crates/bevy_math/src/primitives/dim3.rs b/crates/bevy_math/src/primitives/dim3.rs index 192b9074a933f..60e5c91ebaa78 100644 --- a/crates/bevy_math/src/primitives/dim3.rs +++ b/crates/bevy_math/src/primitives/dim3.rs @@ -86,6 +86,27 @@ pub struct Sphere { } impl Primitive3d for Sphere {} +impl Sphere { + /// Finds the point on the 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 distance_squared = point.length_squared(); + + if distance_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 = point / distance_squared.sqrt(); + self.radius * dir_to_point + } + } +} + /// An unbounded plane in 3D space. It forms a separating surface through the origin, /// stretching infinitely far #[derive(Clone, Copy, Debug)] @@ -238,6 +259,16 @@ impl Cuboid { half_size: size / 2., } } + + /// Finds the point on the cuboid that is closest to the given `point`. + /// + /// If the point is outside the cuboid, the returned point will be on the surface of the cuboid. + /// Otherwise, it will be inside the cuboid and returned as is. + #[inline(always)] + pub fn closest_point(&self, point: Vec3) -> Vec3 { + // Clamp point coordinates to the cuboid + point.clamp(-self.half_size, self.half_size) + } } /// A cylinder primitive @@ -426,4 +457,29 @@ mod test { Ok((Direction3d::from_normalized(Vec3::X), 6.5)) ); } + + #[test] + fn cuboid_closest_point() { + let cuboid = Cuboid::new(2.0, 2.0, 2.0); + assert_eq!(cuboid.closest_point(Vec3::X * 10.0), Vec3::X); + assert_eq!(cuboid.closest_point(Vec3::NEG_ONE * 10.0), Vec3::NEG_ONE); + assert_eq!( + cuboid.closest_point(Vec3::new(0.25, 0.1, 0.3)), + Vec3::new(0.25, 0.1, 0.3) + ); + } + + #[test] + fn sphere_closest_point() { + let sphere = Sphere { radius: 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) + ); + } }