Fix bug wher ethe broad-phase could miss some contacts pairs.

src/geometry/broad_phase_multi_sap/broad_phase.rs

@@ -119,6 +119,10 @@ impl BroadPhase {
     }
 
     /// Maintain the broad-phase internal state by taking collider removal into account.
+    ///
+    /// For each colliders marked as removed, we make their containing layer mark
+    /// its proxy as pre-deleted. The actual proxy removal will happen at the end
+    /// of the `BroadPhase::update`.
     fn handle_removed_colliders(&mut self, colliders: &mut ColliderSet) {
         // Ensure we already subscribed the collider-removed events.
         if self.removed_colliders.is_none() {
@@ -141,13 +145,13 @@ impl BroadPhase {
         self.removed_colliders = Some(cursor);
     }
 
-    /// Removes a proxy from this broad-phase.
+    /// Pre-deletes a proxy from this broad-phase.
     ///
     /// The removal of a proxy is a semi-lazy process. It will mark
     /// the proxy as predeleted, and will set its AABB as +infinity.
     /// After this method has been called with all the proxies to
     /// remove, the `complete_removal` method MUST be called to
-    /// complete the removal of these proxies, by removing them
+    /// complete the removal of these proxies, by actually removing them
     /// from all the relevant layers/regions/axes.
     fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) {
         if proxy_index == crate::INVALID_U32 {
@@ -220,20 +224,27 @@ impl BroadPhase {
         colliders.removed_colliders.ack(&cursor);
     }
 
+    /// Finalize the insertion of the layer identified by `layer_id`.
+    ///
+    /// This will:
+    /// - Remove all the subregion proxies from the larger layer.
+    /// - Pre-insert all the smaller layer's region proxies into this layer.
     fn finalize_layer_insertion(&mut self, layer_id: u8) {
+        // Remove all the region endpoints from the larger layer.
+        // They will be automatically replaced by the new layer's regions.
         if let Some(larger_layer) = self.layers[layer_id as usize].larger_layer {
-            // Remove all the region endpoints from the larger layer.
-            // They will be automatically replaced by the new layer's regions.
             self.layers[larger_layer as usize].unregister_all_subregions(&mut self.proxies);
         }
 
+        // Add all the regions from the smaller layer to the new layer.
+        // This will result in new regions to be created in the new layer.
+        // These new regions will automatically propagate to the larger layers in
+        // the Phase 3 of `Self::update`.
         if let Some(smaller_layer) = self.layers[layer_id as usize].smaller_layer {
             let (smaller_layer, new_layer) = self
                 .layers
                 .index_mut2(smaller_layer as usize, layer_id as usize);
 
-            // Add all the regions from the smaller layer to the new layer.
-            // This will propagate to the bigger layers automatically.
             smaller_layer.propagate_existing_regions(
                 new_layer,
                 &mut self.proxies,
@@ -242,6 +253,17 @@ impl BroadPhase {
         }
     }
 
+    /// Ensures that a given layer exists.
+    ///
+    /// If the layer does not exist then:
+    /// 1. It is created and added to `self.layers`.
+    /// 2. The smaller/larger layer indices are updated to order them
+    ///    properly depending on their depth.
+    /// 3. All the subregion proxies from the larger layer are deleted:
+    ///    they will be replaced by this new layer's regions later in
+    ///    the `update` function.
+    /// 4. All the regions from the smaller layer are added to that new
+    ///    layer.
     fn ensure_layer_exists(&mut self, new_depth: i8) -> u8 {
         // Special case: we don't have any layers yet.
         if self.layers.is_empty() {
@@ -264,6 +286,9 @@ impl BroadPhase {
 
         match larger_layer_id {
             None => {
+                // The layer we are currently creating is the new largest layer. So
+                // we need to update `self.largest_layer` accordingly then call
+                // `self.finalize_layer_insertion.
                 assert_ne!(self.layers.len() as u8, u8::MAX, "Not yet implemented.");
                 let new_layer_id = self.layers.len() as u8;
                 self.layers[self.largest_layer as usize].larger_layer = Some(new_layer_id);
@@ -283,6 +308,10 @@ impl BroadPhase {
                     larger_layer_id
                 } else {
                     // The layer does not exist yet. Create it.
+                    // And we found another layer that is larger than this one.
+                    // So we need to adjust the smaller/larger layer indices too
+                    // keep the list sorted, and then call `self.finalize_layer_insertion`
+                    // to deal with region propagation.
                     let new_layer_id = self.layers.len() as u8;
                     let smaller_layer_id = self.layers[larger_layer_id as usize].smaller_layer;
                     self.layers[larger_layer_id as usize].smaller_layer = Some(new_layer_id);
@@ -315,10 +344,12 @@ impl BroadPhase {
         colliders: &mut ColliderSet,
         events: &mut Vec<BroadPhasePairEvent>,
     ) {
+        // Phase 1: pre-delete the collisions that have been deleted.
         self.handle_removed_colliders(colliders);
 
         let mut need_region_propagation = false;
 
+        // Phase 2: pre-delete the collisions that have been deleted.
         for body_handle in bodies
             .modified_inactive_set
             .iter()
@@ -353,23 +384,23 @@ impl BroadPhase {
             }
         }
 
-        // Bottom-up pass to propagate regions from smaller layers to larger layers.
+        // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
         if need_region_propagation {
             self.propagate_created_regions();
         }
 
-        // Top-down pass to propagate proxies from larger layers to smaller layers.
+        // Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
         self.update_layers_and_find_pairs(events);
 
-        // Bottom-up pass to remove proxies, and propagate region removed from smaller layers to
+        // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
-        // possible remove regions from larger layers that would become empty that way.
+        // layers to possible remove regions from larger layers that would become empty that way.
         self.complete_removals(colliders);
     }
 
-    /// Propagate regions from the smaller layers up to the larger layers.
+    /// Propagate regions from the smallest layers up to the larger layers.
     ///
     /// Whenever a region is created on a layer `n`, then its AABB must be
-    /// added to all the larger layers so we can detect whaen a object
+    /// added to its larger layer so we can detect when an object
     /// in a larger layer may start interacting with objects in a smaller
     /// layer.
     fn propagate_created_regions(&mut self) {
@@ -389,8 +420,9 @@ impl BroadPhase {
                         &mut self.proxies,
                         &mut self.region_pool,
                     );
+                    layer.created_regions.clear();
                 } else {
-                    // Always clear the set of created regions, even else if
+                    // Always clear the set of created regions, even if
                     // there is no larger layer.
                     layer.created_regions.clear();
                 }

src/geometry/broad_phase_multi_sap/sap_layer.rs

@@ -38,23 +38,39 @@ impl SAPLayer {
         }
     }
 
+    /// Deletes from all the regions of this layer, all the endpoints corresponding
+    /// to subregions. Clears the arrays of subregions indices from all the regions of
+    /// this layer.
     pub fn unregister_all_subregions(&mut self, proxies: &mut SAPProxies) {
         for region_id in self.regions.values() {
-            if let Some(mut region) = proxies[*region_id].data.take_region() {
+            // Extract the region to make the borrow-checker happy.
-                region.delete_all_region_endpoints(proxies);
+            let mut region = proxies[*region_id]
+                .data
+                .take_region()
+                .expect("Should be a region proxy.");
 
-                for subregion in region.subregions.drain(..) {
+            // Delete the endpoints.
-                    proxies[subregion]
+            region.delete_all_region_endpoints(proxies);
-                        .data
-                        .as_region_mut()
-                        .id_in_parent_subregion = crate::INVALID_U32;
-                }
 
-                proxies[*region_id].data.set_region(region);
+            // Clear the subregions vec and reset the subregions parent ids.
+            for subregion in region.subregions.drain(..) {
+                proxies[subregion]
+                    .data
+                    .as_region_mut()
+                    .id_in_parent_subregion = crate::INVALID_U32;
             }
+
+            // Re set the region to make the borrow-checker happy.
+            proxies[*region_id].data.set_region(region);
         }
     }
 
+    /// Register into `larger_layer` all the region proxies of the recently-created regions
+    /// contained by `self`.
+    ///
+    /// This method must be called in a bottom-up loop, propagating new regions from the
+    /// smallest layer, up to the largest layer. That loop is done by the Phase 3 of the
+    /// BroadPhase::update.
     pub fn propagate_created_regions(
         &mut self,
         larger_layer: &mut Self,
@@ -66,6 +82,7 @@ impl SAPLayer {
         }
     }
 
+    /// Register into `larger_layer` all the region proxies of the region contained in `self`.
     pub fn propagate_existing_regions(
         &mut self,
         larger_layer: &mut Self,
@@ -77,7 +94,12 @@ impl SAPLayer {
         }
     }
 
-    // Preupdates the proxy of a subregion.
+    /// Registers a subregion of this layer.
+    ///
+    /// The subregion proxy will be added to the region of `self` that contains
+    /// that subregion center. Because the hierarchical grid cells have aligned boundaries
+    /// at each depth, we have the guarantee that a given subregion will only be part of
+    /// one region on its parent "larger" layer.
     fn register_subregion(
         &mut self,
         proxy_id: SAPProxyIndex,
@@ -85,7 +107,9 @@ impl SAPLayer {
         pool: &mut SAPRegionPool,
     ) {
         if let Some(proxy) = proxies.get(proxy_id) {
-            if proxy.data.as_region().id_in_parent_subregion == crate::INVALID_U32 {
+            let curr_id_in_parent_subregion = proxy.data.as_region().id_in_parent_subregion;
+
+            if curr_id_in_parent_subregion == crate::INVALID_U32 {
                 let region_key = super::point_key(proxy.aabb.center(), self.region_width);
                 let region_id = self.ensure_region_exists(region_key, proxies, pool);
                 let region = proxies[region_id].data.as_region_mut();
@@ -95,6 +119,20 @@ impl SAPLayer {
                     .data
                     .as_region_mut()
                     .id_in_parent_subregion = id_in_parent_subregion as u32;
+            } else {
+                // NOTE: all the following are just assertions to make sure the
+                // region ids are correctly wired. If this piece of code causes
+                // any performance problem, it can be deleted completely without
+                // hesitation.
+                if curr_id_in_parent_subregion != crate::INVALID_U32 {
+                    let region_key = super::point_key(proxy.aabb.center(), self.region_width);
+                    let region_id = self.regions.get(&region_key).unwrap();
+                    let region = proxies[*region_id].data.as_region_mut();
+                    assert_eq!(
+                        region.subregions[curr_id_in_parent_subregion as usize],
+                        proxy_id
+                    );
+                }
             }
         }
     }
@@ -138,6 +176,15 @@ impl SAPLayer {
         }
     }
 
+    /// Ensures a given region exists in this layer.
+    ///
+    /// If the region with the given region key does not exist yet, it is created.
+    /// When a region is created, it creates a new proxy for that region, and its
+    /// proxy ID is added to `self.created_region` so it can be propagated during
+    /// the Phase 3 of `BroadPhase::update`.
+    ///
+    /// This returns the proxy ID of the already existing region if it existed, or
+    /// of the new region if it did not exist and has been created by this method.
     pub fn ensure_region_exists(
         &mut self,
         region_key: Point<i32>,
@@ -145,14 +192,19 @@ impl SAPLayer {
         pool: &mut SAPRegionPool,
     ) -> SAPProxyIndex {
         match self.regions.entry(region_key) {
+            // Yay, the region already exists!
             Entry::Occupied(occupied) => *occupied.get(),
+            // The region does not exist, create it.
             Entry::Vacant(vacant) => {
                 let region_bounds = super::region_aabb(region_key, self.region_width);
                 let region = SAPRegion::recycle_or_new(region_bounds, pool);
+                // Create a new proxy for that region.
                 let region_proxy =
                     SAPProxy::subregion(region, region_bounds, self.layer_id, self.depth);
                 let region_proxy_id = proxies.insert(region_proxy);
+                // Push this region's proxy ID to the set of created regions.
                 self.created_regions.push(region_proxy_id as u32);
+                // Insert the new region to this layer's region hashmap.
                 let _ = vacant.insert(region_proxy_id);
                 region_proxy_id
             }

src/geometry/broad_phase_multi_sap/sap_region.rs

@@ -73,18 +73,43 @@ impl SAPRegion {
         }
     }
 
+    /// Deletes from the axes of this region all the endpoints that point
+    /// to a region.
     pub fn delete_all_region_endpoints(&mut self, proxies: &SAPProxies) {
-        for axis in &mut self.axes {
+        let mut num_deleted_subregion_endpoints = [0; DIM];
+
+        for (i, axis) in self.axes.iter_mut().enumerate() {
             let existing_proxies = &mut self.existing_proxies;
             axis.endpoints.retain(|e| {
+                // NOTE: we use `if let` instead of `unwrap` because no
+                // proxy will be found for the sentinels.
                 if let Some(proxy) = proxies.get(e.proxy()) {
-                    existing_proxies.set(e.proxy() as usize, false);
+                    if proxy.data.is_region() {
-                    !proxy.data.is_region()
+                        existing_proxies.set(e.proxy() as usize, false);
-                } else {
+                        num_deleted_subregion_endpoints[i] += 1;
-                    true
+                        return false;
+                    }
                 }
+
+                true
             });
         }
+
+        // All axes should have deleted the same number of region endpoints.
+        for k in 1..DIM {
+            assert_eq!(
+                num_deleted_subregion_endpoints[0],
+                num_deleted_subregion_endpoints[k]
+            );
+        }
+
+        // The number of deleted endpoints should be even because there
+        // are two endpoints per proxy on each axes.
+        assert_eq!(num_deleted_subregion_endpoints[0] % 2, 0);
+
+        // All the region endpoints are subproper proxies.
+        // So update the subproper proxy count accordingly.
+        self.subproper_proxy_count -= num_deleted_subregion_endpoints[0] / 2;
     }
 
     pub fn predelete_proxy(&mut self, _proxy_id: SAPProxyIndex) {

src/geometry/broad_phase_multi_sap/sap_utils.rs

@@ -44,7 +44,7 @@ pub(crate) fn region_width(depth: i8) -> Real {
 ///
 /// The idea here is that an AABB should be part of a layer which has
 /// regions large enough so that one AABB doesn't crosses too many
-/// regions. But the regions must also not bee too large, otherwise
+/// regions. But the regions must also not be too large, otherwise
 /// we are loosing the benefits of Multi-SAP.
 ///
 /// If the code bellow, we select a layer such that each region can