From e3a3b5b9c2583f67bcb926e23f3f9c6a9636f220 Mon Sep 17 00:00:00 2001 From: josh65536 Date: Tue, 8 Mar 2022 00:30:41 +0000 Subject: [PATCH] Fixed the frustum-sphere collision and added tests (#4035) # Objective Fixes #3744 ## Solution The old code used the formula `normal . center + d + radius <= 0` to determine if the sphere with center `center` and radius `radius` is outside the plane with normal `normal` and distance from origin `d`. This only works if `normal` is normalized, which is not necessarily the case. Instead, `normal` and `d` are both multiplied by some factor that `radius` isn't multiplied by. So the additional code multiplied `radius` by that factor. --- crates/bevy_render/src/primitives/mod.rs | 217 +++++++++++++++++++++-- 1 file changed, 203 insertions(+), 14 deletions(-) diff --git a/crates/bevy_render/src/primitives/mod.rs b/crates/bevy_render/src/primitives/mod.rs index 623abd0d1801c..161e8c24fd75a 100644 --- a/crates/bevy_render/src/primitives/mod.rs +++ b/crates/bevy_render/src/primitives/mod.rs @@ -1,5 +1,5 @@ use bevy_ecs::{component::Component, reflect::ReflectComponent}; -use bevy_math::{Mat4, Vec3, Vec3A, Vec4}; +use bevy_math::{Mat4, Vec3, Vec3A, Vec4, Vec4Swizzles}; use bevy_reflect::Reflect; /// An Axis-Aligned Bounding Box @@ -72,12 +72,47 @@ impl Sphere { } } -/// A plane defined by a normal and distance value along the normal -/// Any point p is in the plane if n.p = d -/// For planes defining half-spaces such as for frusta, if n.p > d then p is on the positive side of the plane. +/// A plane defined by a unit normal and distance from the origin along the normal +/// Any point p is in the plane if n.p + d = 0 +/// For planes defining half-spaces such as for frusta, if n.p + d > 0 then p is on +/// the positive side (inside) of the plane. #[derive(Clone, Copy, Debug, Default)] pub struct Plane { - pub normal_d: Vec4, + normal_d: Vec4, +} + +impl Plane { + /// Constructs a `Plane` from a 4D vector whose first 3 components + /// are the normal and whose last component is the distance along the normal + /// from the origin. + /// This constructor ensures that the normal is normalized and the distance is + /// scaled accordingly so it represents the signed distance from the origin. + #[inline] + pub fn new(normal_d: Vec4) -> Self { + Self { + normal_d: normal_d * normal_d.xyz().length_recip(), + } + } + + /// `Plane` unit normal + #[inline] + pub fn normal(&self) -> Vec3 { + self.normal_d.xyz() + } + + /// Signed distance from the origin along the unit normal such that n.p + d = 0 for point p in + /// the `Plane` + #[inline] + pub fn d(&self) -> f32 { + self.normal_d.w + } + + /// `Plane` unit normal and signed distance from the origin such that n.p + d = 0 for point p + /// in the `Plane` + #[inline] + pub fn normal_d(&self) -> Vec4 { + self.normal_d + } } #[derive(Component, Clone, Copy, Debug, Default, Reflect)] @@ -102,23 +137,20 @@ impl Frustum { let mut planes = [Plane::default(); 6]; for (i, plane) in planes.iter_mut().enumerate().take(5) { let row = view_projection.row(i / 2); - plane.normal_d = if (i & 1) == 0 && i != 4 { + *plane = Plane::new(if (i & 1) == 0 && i != 4 { row3 + row } else { row3 - row - } - .normalize(); + }); } let far_center = *view_translation - far * *view_backward; - planes[5].normal_d = view_backward - .extend(-view_backward.dot(far_center)) - .normalize(); + planes[5] = Plane::new(view_backward.extend(-view_backward.dot(far_center))); Self { planes } } pub fn intersects_sphere(&self, sphere: &Sphere) -> bool { for plane in &self.planes { - if plane.normal_d.dot(sphere.center.extend(1.0)) + sphere.radius <= 0.0 { + if plane.normal_d().dot(sphere.center.extend(1.0)) + sphere.radius <= 0.0 { return false; } } @@ -134,9 +166,9 @@ impl Frustum { ]; for plane in &self.planes { - let p_normal = Vec3A::from(plane.normal_d); + let p_normal = Vec3A::from(plane.normal_d()); let relative_radius = aabb.relative_radius(&p_normal, &axes); - if plane.normal_d.dot(aabb_center_world) + relative_radius <= 0.0 { + if plane.normal_d().dot(aabb_center_world) + relative_radius <= 0.0 { return false; } } @@ -159,3 +191,160 @@ impl CubemapFrusta { self.frusta.iter_mut() } } + +#[cfg(test)] +mod tests { + use super::*; + + // A big, offset frustum + fn big_frustum() -> Frustum { + Frustum { + planes: [ + Plane::new(Vec4::new(-0.9701, -0.2425, -0.0000, 7.7611)), + Plane::new(Vec4::new(-0.0000, 1.0000, -0.0000, 4.0000)), + Plane::new(Vec4::new(-0.0000, -0.2425, -0.9701, 2.9104)), + Plane::new(Vec4::new(-0.0000, -1.0000, -0.0000, 4.0000)), + Plane::new(Vec4::new(-0.0000, -0.2425, 0.9701, 2.9104)), + Plane::new(Vec4::new(0.9701, -0.2425, -0.0000, -1.9403)), + ], + } + } + + #[test] + fn intersects_sphere_big_frustum_outside() { + // Sphere outside frustum + let frustum = big_frustum(); + let sphere = Sphere { + center: Vec3::new(0.9167, 0.0000, 0.0000), + radius: 0.7500, + }; + assert!(!frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_big_frustum_intersect() { + // Sphere intersects frustum boundary + let frustum = big_frustum(); + let sphere = Sphere { + center: Vec3::new(7.9288, 0.0000, 2.9728), + radius: 2.0000, + }; + assert!(frustum.intersects_sphere(&sphere)); + } + + // A frustum + fn frustum() -> Frustum { + Frustum { + planes: [ + Plane::new(Vec4::new(-0.9701, -0.2425, -0.0000, 0.7276)), + Plane::new(Vec4::new(-0.0000, 1.0000, -0.0000, 1.0000)), + Plane::new(Vec4::new(-0.0000, -0.2425, -0.9701, 0.7276)), + Plane::new(Vec4::new(-0.0000, -1.0000, -0.0000, 1.0000)), + Plane::new(Vec4::new(-0.0000, -0.2425, 0.9701, 0.7276)), + Plane::new(Vec4::new(0.9701, -0.2425, -0.0000, 0.7276)), + ], + } + } + + #[test] + fn intersects_sphere_frustum_surrounding() { + // Sphere surrounds frustum + let frustum = frustum(); + let sphere = Sphere { + center: Vec3::new(0.0000, 0.0000, 0.0000), + radius: 3.0000, + }; + assert!(frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_frustum_contained() { + // Sphere is contained in frustum + let frustum = frustum(); + let sphere = Sphere { + center: Vec3::new(0.0000, 0.0000, 0.0000), + radius: 0.7000, + }; + assert!(frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_frustum_intersects_plane() { + // Sphere intersects a plane + let frustum = frustum(); + let sphere = Sphere { + center: Vec3::new(0.0000, 0.0000, 0.9695), + radius: 0.7000, + }; + assert!(frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_frustum_intersects_2_planes() { + // Sphere intersects 2 planes + let frustum = frustum(); + let sphere = Sphere { + center: Vec3::new(1.2037, 0.0000, 0.9695), + radius: 0.7000, + }; + assert!(frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_frustum_intersects_3_planes() { + // Sphere intersects 3 planes + let frustum = frustum(); + let sphere = Sphere { + center: Vec3::new(1.2037, -1.0988, 0.9695), + radius: 0.7000, + }; + assert!(frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_frustum_dodges_1_plane() { + // Sphere avoids intersecting the frustum by 1 plane + let frustum = frustum(); + let sphere = Sphere { + center: Vec3::new(-1.7020, 0.0000, 0.0000), + radius: 0.7000, + }; + assert!(!frustum.intersects_sphere(&sphere)); + } + + // A long frustum. + fn long_frustum() -> Frustum { + Frustum { + planes: [ + Plane::new(Vec4::new(-0.9998, -0.0222, -0.0000, -1.9543)), + Plane::new(Vec4::new(-0.0000, 1.0000, -0.0000, 45.1249)), + Plane::new(Vec4::new(-0.0000, -0.0168, -0.9999, 2.2718)), + Plane::new(Vec4::new(-0.0000, -1.0000, -0.0000, 45.1249)), + Plane::new(Vec4::new(-0.0000, -0.0168, 0.9999, 2.2718)), + Plane::new(Vec4::new(0.9998, -0.0222, -0.0000, 7.9528)), + ], + } + } + + #[test] + fn intersects_sphere_long_frustum_outside() { + // Sphere outside frustum + let frustum = long_frustum(); + let sphere = Sphere { + center: Vec3::new(-4.4889, 46.9021, 0.0000), + radius: 0.7500, + }; + assert!(!frustum.intersects_sphere(&sphere)); + } + + #[test] + fn intersects_sphere_long_frustum_intersect() { + // Sphere intersects frustum boundary + let frustum = long_frustum(); + let sphere = Sphere { + center: Vec3::new(-4.9957, 0.0000, -0.7396), + radius: 4.4094, + }; + assert!(frustum.intersects_sphere(&sphere)); + } +}