Add optimized is_in_half_space_identity, contains_aabb_identity, and intersects_obb_identity (#21249)

# Objective

This adds an optimized version of is_in_half_space,
`Frustum::contains_aabb_identity`, `Aabb::is_in_half_space_identity`,
and `Frustum::intersects_obb_identity` that takes advantage of how
calling with IDENTITY (common) reduces the amount of matrix
multiplications.

## Solution

Add a specialized function without touching the original that assumes
Identity was passed to simplify math.

## Testing

I use this function extensively in my own project. I asserted the old
function called with identity always returns the same as my new
function.
I bench marked and profiled the usage in my real application and noticed
a 16% speed up on Linux and 20% on windows for contains_aabb.
I bench marked intersects_obb_identity and noticed a 39% increase on
linux and 38% on windows.
Both functions have unit tests that assert calling with identity yields
the same result in both versions.

---------

Co-authored-by: François Mockers <[email protected]>
Co-authored-by: IQuick 143 <[email protected]>

Author: 3 people

crates/bevy_camera/src/primitives.rs

@@ -133,6 +133,16 @@ impl Aabb {
         let signed_distance = p_normal.dot(aabb_center_world) + half_space.d();
         signed_distance > r
     }
+
+    /// Optimized version of [`Self::is_in_half_space`] when the AABB is already in world space.
+    /// Use this when `world_from_local` would be the identity transform.
+    #[inline]
+    pub fn is_in_half_space_identity(&self, half_space: &HalfSpace) -> bool {
+        let p_normal = half_space.normal();
+        let r = self.half_extents.abs().dot(p_normal.abs());
+        let signed_distance = p_normal.dot(self.center) + half_space.d();
+        signed_distance > r
+    }
 }
 
 impl From<Aabb3d> for Aabb {
@@ -369,7 +379,22 @@ impl Frustum {
         true
     }
 
-    /// Check if the frustum contains the Axis-Aligned Bounding Box (AABB).
+    /// Optimized version of [`Frustum::intersects_obb`]
+    /// where the transform is [`Affine3A::IDENTITY`] and both `intersect_near` and `intersect_far` are `true`.
+    #[inline]
+    pub fn intersects_obb_identity(&self, aabb: &Aabb) -> bool {
+        let aabb_center_world = aabb.center.extend(1.0);
+        for half_space in self.half_spaces.iter() {
+            let p_normal = half_space.normal();
+            let relative_radius = aabb.half_extents.abs().dot(p_normal.abs());
+            if half_space.normal_d().dot(aabb_center_world) + relative_radius <= 0.0 {
+                return false;
+            }
+        }
+        true
+    }
+
+    /// Check if the frustum contains the entire Axis-Aligned Bounding Box (AABB).
     /// Referenced from: [Frustum Culling](https://learnopengl.com/Guest-Articles/2021/Scene/Frustum-Culling)
     #[inline]
     pub fn contains_aabb(&self, aabb: &Aabb, world_from_local: &Affine3A) -> bool {
@@ -380,6 +405,18 @@ impl Frustum {
         }
         true
     }
+
+    /// Optimized version of [`Self::contains_aabb`] when the AABB is already in world space.
+    /// Use this when `world_from_local` would be [`Affine3A::IDENTITY`].
+    #[inline]
+    pub fn contains_aabb_identity(&self, aabb: &Aabb) -> bool {
+        for half_space in &self.half_spaces {
+            if !aabb.is_in_half_space_identity(half_space) {
+                return false;
+            }
+        }
+        true
+    }
 }
 
 pub struct CubeMapFace {
@@ -772,4 +809,62 @@ mod tests {
         );
         assert!(!frustum.contains_aabb(&aabb, &model));
     }
+
+    #[test]
+    fn test_identity_optimized_equivalence() {
+        let cases = vec![
+            (
+                Aabb {
+                    center: Vec3A::ZERO,
+                    half_extents: Vec3A::splat(1.0),
+                },
+                HalfSpace::new(Vec4::new(1.0, 0.0, 0.0, -0.5)),
+            ),
+            (
+                Aabb {
+                    center: Vec3A::new(2.0, -1.0, 0.5),
+                    half_extents: Vec3A::new(1.0, 2.0, 0.5),
+                },
+                HalfSpace::new(Vec4::new(1.0, 1.0, 1.0, -1.0).normalize()),
+            ),
+            (
+                Aabb {
+                    center: Vec3A::new(1.0, 1.0, 1.0),
+                    half_extents: Vec3A::ZERO,
+                },
+                HalfSpace::new(Vec4::new(0.0, 0.0, 1.0, -2.0)),
+            ),
+        ];
+        for (aabb, half_space) in cases {
+            let general = aabb.is_in_half_space(&half_space, &Affine3A::IDENTITY);
+            let identity = aabb.is_in_half_space_identity(&half_space);
+            assert_eq!(general, identity,);
+        }
+    }
+
+    #[test]
+    fn intersects_obb_identity_matches_standard_true_true() {
+        let frusta = [frustum(), long_frustum(), big_frustum()];
+        let aabbs = [
+            Aabb {
+                center: Vec3A::ZERO,
+                half_extents: Vec3A::new(0.5, 0.5, 0.5),
+            },
+            Aabb {
+                center: Vec3A::new(1.0, 0.0, 0.5),
+                half_extents: Vec3A::new(0.9, 0.9, 0.9),
+            },
+            Aabb {
+                center: Vec3A::new(100.0, 100.0, 100.0),
+                half_extents: Vec3A::new(1.0, 1.0, 1.0),
+            },
+        ];
+        for fr in &frusta {
+            for aabb in &aabbs {
+                let standard = fr.intersects_obb(aabb, &Affine3A::IDENTITY, true, true);
+                let optimized = fr.intersects_obb_identity(aabb);
+                assert_eq!(standard, optimized);
+            }
+        }
+    }
 }