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

@@ -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));
+    }
+}