Fix broad-phase bug that could result in missed collision pairs when an object leaves then re-enter a region

2 files changed, 60 insertions, 11 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index 63f1a55..4dc3e3a 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -3,7 +3,7 @@ use super::{
 };
 use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
 use crate::geometry::{
-    ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape,
+    ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderShape, AABB,
 };
 use crate::math::Real;
 use crate::utils::IndexMut2;
@@ -351,9 +351,11 @@ impl BroadPhase {
 
         aabb.mins = super::clamp_point(aabb.mins);
         aabb.maxs = super::clamp_point(aabb.maxs);
+        let prev_aabb;
 
         let layer_id = if let Some(proxy) = self.proxies.get_mut(*proxy_index) {
             let mut layer_id = proxy.layer_id;
+            prev_aabb = proxy.aabb;
             proxy.aabb = aabb;
 
             if co_changes.contains(ColliderChanges::SHAPE) {
@@ -380,6 +382,7 @@ impl BroadPhase {
 
             // Create the proxy.
             let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
+            prev_aabb = aabb;
             *proxy_index = self.proxies.insert(proxy);
             layer_id
         };
@@ -387,12 +390,41 @@ impl BroadPhase {
         let layer = &mut self.layers[layer_id as usize];
 
         // Preupdate the collider in the layer.
-        layer.preupdate_collider(
-            *proxy_index,
-            &aabb,
-            &mut self.proxies,
-            &mut self.region_pool,
-        );
+        // We need to use both the prev AABB and the new AABB for this update, to
+        // handle special cases where one AABB has left a region that doesn’t contain
+        // any other modified AABBs.
+        // If the combination of both previous and new aabbs isn’t more than 25% bigger
+        // than the new AABB, we just merge them to save some computation times (to avoid
+        // discretizing twice the area at their intersection. If it’s bigger than 25% then
+        // we discretize both aabbs individually.
+        let merged_aabbs = prev_aabb.merged(&aabb);
+
+        if merged_aabbs.volume() > aabb.volume() * 1.25 {
+            layer.preupdate_collider(
+                *proxy_index,
+                &aabb,
+                None,
+                &mut self.proxies,
+                &mut self.region_pool,
+            );
+
+            layer.preupdate_collider(
+                *proxy_index,
+                &prev_aabb,
+                Some(&aabb),
+                &mut self.proxies,
+                &mut self.region_pool,
+            );
+        } else {
+            layer.preupdate_collider(
+                *proxy_index,
+                &merged_aabbs,
+                Some(&aabb),
+                &mut self.proxies,
+                &mut self.region_pool,
+            );
+        }
+
         let need_region_propagation = !layer.created_regions.is_empty();
 
         need_region_propagation
diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs
index e4a9f42..216ea05 100644
--- a/src/geometry/broad_phase_multi_sap/sap_layer.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs
@@ -2,6 +2,7 @@ use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool};
 use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
 use crate::geometry::{SAPProxyIndex, AABB};
 use crate::math::{Point, Real};
+use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::{Entry, HashMap};
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -214,12 +215,13 @@ impl SAPLayer {
     pub fn preupdate_collider(
         &mut self,
         proxy_id: u32,
-        aabb: &AABB,
+        aabb_to_discretize: &AABB,
+        actual_aabb: Option<&AABB>,
         proxies: &mut SAPProxies,
         pool: &mut SAPRegionPool,
     ) {
-        let start = super::point_key(aabb.mins, self.region_width);
-        let end = super::point_key(aabb.maxs, self.region_width);
+        let start = super::point_key(aabb_to_discretize.mins, self.region_width);
+        let end = super::point_key(aabb_to_discretize.maxs, self.region_width);
 
         // Discretize the aabb.
         #[cfg(feature = "dim2")]
@@ -235,7 +237,22 @@ impl SAPLayer {
                     #[cfg(feature = "dim3")]
                     let region_key = Point::new(i, j, _k);
                     let region_id = self.ensure_region_exists(region_key, proxies, pool);
-                    let region = proxies[region_id].data.as_region_mut();
+                    let region_proxy = &mut proxies[region_id];
+                    let region = region_proxy.data.as_region_mut();
+
+                    if let Some(actual_aabb) = actual_aabb {
+                        // NOTE: if the actual AABB doesn't intersect the
+                        //       region’s AABB, then we need to delete the
+                        //       proxy from that region because it means that
+                        //       during the last update the proxy intersected
+                        //       that region, but it doesn't intersect it any
+                        //       more during the current update.
+                        if !region_proxy.aabb.intersects(actual_aabb) {
+                            region.predelete_proxy(proxy_id);
+                            continue;
+                        }
+                    }
+
                     region.preupdate_proxy(proxy_id, true);
                 }
             }