diff --git a/crates/bevy_math/src/primitives/dim2.rs b/crates/bevy_math/src/primitives/dim2.rs index 9cb379706c18b..dfe0e2b95ed0b 100644 --- a/crates/bevy_math/src/primitives/dim2.rs +++ b/crates/bevy_math/src/primitives/dim2.rs @@ -1480,6 +1480,38 @@ impl Segment2d { self.reverse(); self } + + /// Returns the point on the [`Segment2d`] that is closest to the specified `point`. + #[inline(always)] + pub fn closest_point(&self, point: Vec2) -> Vec2 { + // `point` + // x + // ^| + // / | + //`offset`/ | + // / | `segment_vector` + // x----.-------------->x + // 0 t 1 + let segment_vector = self.vertices[1] - self.vertices[0]; + let offset = point - self.vertices[0]; + // The signed projection of `offset` onto `segment_vector`, scaled by the length of the segment. + let projection_scaled = segment_vector.dot(offset); + + // `point` is too far "left" in the picture + if projection_scaled <= 0.0 { + return self.vertices[0]; + } + + let length_squared = segment_vector.length_squared(); + // `point` is too far "right" in the picture + if projection_scaled >= length_squared { + return self.vertices[1]; + } + + // Point lies somewhere in the middle, we compute the closest point by finding the parameter along the line. + let t = projection_scaled / length_squared; + self.vertices[0] + t * segment_vector + } } impl From<[Vec2; 2]> for Segment2d { @@ -2288,6 +2320,52 @@ mod tests { assert_eq!(rhombus.closest_point(Vec2::new(-0.55, 0.35)), Vec2::ZERO); } + #[test] + fn segment_closest_point() { + assert_eq!( + Segment2d::new(Vec2::new(0.0, 0.0), Vec2::new(3.0, 0.0)) + .closest_point(Vec2::new(1.0, 6.0)), + Vec2::new(1.0, 0.0) + ); + + let segments = [ + Segment2d::new(Vec2::new(0.0, 0.0), Vec2::new(0.0, 0.0)), + Segment2d::new(Vec2::new(0.0, 0.0), Vec2::new(1.0, 0.0)), + Segment2d::new(Vec2::new(1.0, 0.0), Vec2::new(0.0, 1.0)), + Segment2d::new(Vec2::new(1.0, 0.0), Vec2::new(1.0, 5.0 * f32::EPSILON)), + ]; + let points = [ + Vec2::new(0.0, 0.0), + Vec2::new(1.0, 0.0), + Vec2::new(-1.0, 1.0), + Vec2::new(1.0, 1.0), + Vec2::new(-1.0, 0.0), + Vec2::new(5.0, -1.0), + Vec2::new(1.0, f32::EPSILON), + ]; + + for point in points.iter() { + for segment in segments.iter() { + let closest = segment.closest_point(*point); + assert!( + point.distance_squared(closest) <= point.distance_squared(segment.point1()), + "Closest point must always be at least as close as either vertex." + ); + assert!( + point.distance_squared(closest) <= point.distance_squared(segment.point2()), + "Closest point must always be at least as close as either vertex." + ); + assert!( + point.distance_squared(closest) <= point.distance_squared(segment.center()), + "Closest point must always be at least as close as the center." + ); + let closest_to_closest = segment.closest_point(closest); + // Closest point must already be on the segment + assert_relative_eq!(closest_to_closest, closest); + } + } + } + #[test] fn circle_math() { let circle = Circle { radius: 3.0 }; diff --git a/crates/bevy_math/src/primitives/dim3.rs b/crates/bevy_math/src/primitives/dim3.rs index 86aa6c5bdf068..a084954a3f132 100644 --- a/crates/bevy_math/src/primitives/dim3.rs +++ b/crates/bevy_math/src/primitives/dim3.rs @@ -548,6 +548,38 @@ impl Segment3d { self.reverse(); self } + + /// Returns the point on the [`Segment3d`] that is closest to the specified `point`. + #[inline(always)] + pub fn closest_point(&self, point: Vec3) -> Vec3 { + // `point` + // x + // ^| + // / | + //`offset`/ | + // / | `segment_vector` + // x----.-------------->x + // 0 t 1 + let segment_vector = self.vertices[1] - self.vertices[0]; + let offset = point - self.vertices[0]; + // The signed projection of `offset` onto `segment_vector`, scaled by the length of the segment. + let projection_scaled = segment_vector.dot(offset); + + // `point` is too far "left" in the picture + if projection_scaled <= 0.0 { + return self.vertices[0]; + } + + let length_squared = segment_vector.length_squared(); + // `point` is too far "right" in the picture + if projection_scaled >= length_squared { + return self.vertices[1]; + } + + // Point lies somewhere in the middle, we compute the closest point by finding the parameter along the line. + let t = projection_scaled / length_squared; + self.vertices[0] + t * segment_vector + } } impl From<[Vec3; 2]> for Segment3d { @@ -1532,6 +1564,55 @@ mod tests { ); } + #[test] + fn segment_closest_point() { + assert_eq!( + Segment3d::new(Vec3::new(0.0, 0.0, 0.0), Vec3::new(3.0, 0.0, 0.0)) + .closest_point(Vec3::new(1.0, 6.0, -2.0)), + Vec3::new(1.0, 0.0, 0.0) + ); + + let segments = [ + Segment3d::new(Vec3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, 0.0)), + Segment3d::new(Vec3::new(0.0, 0.0, 0.0), Vec3::new(1.0, 0.0, 0.0)), + Segment3d::new(Vec3::new(1.0, 0.0, 2.0), Vec3::new(0.0, 1.0, -2.0)), + Segment3d::new( + Vec3::new(1.0, 0.0, 0.0), + Vec3::new(1.0, 5.0 * f32::EPSILON, 0.0), + ), + ]; + let points = [ + Vec3::new(0.0, 0.0, 0.0), + Vec3::new(1.0, 0.0, 0.0), + Vec3::new(-1.0, 1.0, 2.0), + Vec3::new(1.0, 1.0, 1.0), + Vec3::new(-1.0, 0.0, 0.0), + Vec3::new(5.0, -1.0, 0.5), + Vec3::new(1.0, f32::EPSILON, 0.0), + ]; + + for point in points.iter() { + for segment in segments.iter() { + let closest = segment.closest_point(*point); + assert!( + point.distance_squared(closest) <= point.distance_squared(segment.point1()), + "Closest point must always be at least as close as either vertex." + ); + assert!( + point.distance_squared(closest) <= point.distance_squared(segment.point2()), + "Closest point must always be at least as close as either vertex." + ); + assert!( + point.distance_squared(closest) <= point.distance_squared(segment.center()), + "Closest point must always be at least as close as the center." + ); + let closest_to_closest = segment.closest_point(closest); + // Closest point must already be on the segment + assert_relative_eq!(closest_to_closest, closest); + } + } + } + #[test] fn sphere_math() { let sphere = Sphere { radius: 4.0 };