Allow collider modification after its insersion to the ColliderSet.

4 files changed, 84 insertions, 44 deletions

diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase.rs
index c97c737..e036ec0 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase.rs
@@ -2,8 +2,8 @@ use super::{
     BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
 };
 use crate::data::pubsub::Subscription;
-use crate::dynamics::RigidBodySet;
 use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
+use crate::geometry::collider::ColliderChanges;
 use crate::geometry::{ColliderSet, RemovedCollider};
 use crate::math::Real;
 use crate::utils::IndexMut2;
@@ -340,7 +340,6 @@ impl BroadPhase {
     pub fn update(
         &mut self,
         prediction_distance: Real,
-        bodies: &RigidBodySet,
         colliders: &mut ColliderSet,
         events: &mut Vec<BroadPhasePairEvent>,
     ) {
@@ -350,39 +349,54 @@ impl BroadPhase {
         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()
-            .chain(bodies.active_dynamic_set.iter())
-            .chain(bodies.active_kinematic_set.iter())
-        {
-            for handle in &bodies[*body_handle].colliders {
-                let collider = &mut colliders[*handle];
-                let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
-                aabb.mins = super::clamp_point(aabb.mins);
-                aabb.maxs = super::clamp_point(aabb.maxs);
-
-                let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
-                    proxy.aabb = aabb;
-                    proxy.layer_id
-                } else {
-                    let layer_depth = super::layer_containing_aabb(&aabb);
-                    let layer_id = self.ensure_layer_exists(layer_depth);
+        colliders.foreach_modified_colliders_mut_internal(|handle, collider| {
+            if !collider.changes.needs_broad_phase_update() {
+                return;
+            }
 
-                    // Create the proxy.
-                    let proxy = SAPProxy::collider(*handle, aabb, layer_id, layer_depth);
-                    collider.proxy_index = self.proxies.insert(proxy);
-                    layer_id
-                };
+            let mut aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
+            aabb.mins = super::clamp_point(aabb.mins);
+            aabb.maxs = super::clamp_point(aabb.maxs);
+
+            let layer_id = if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
+                let mut layer_id = proxy.layer_id;
+                proxy.aabb = aabb;
+
+                if collider.changes.contains(ColliderChanges::SHAPE) {
+                    // If the shape was changed, then we need to see if this proxy should be
+                    // migrated to a larger layer. Indeed, if the shape was replaced by
+                    // a much larger shape, we need to promote the proxy to a bigger layer
+                    // to avoid the O(n²) discretization problem.
+                    let new_layer_depth = super::layer_containing_aabb(&aabb);
+                    if new_layer_depth > proxy.layer_depth {
+                        self.layers[proxy.layer_id as usize].proper_proxy_moved_to_bigger_layer(
+                            &mut self.proxies,
+                            collider.proxy_index,
+                        );
+
+                        // We need to promote the proxy to the bigger layer.
+                        layer_id = self.ensure_layer_exists(new_layer_depth);
+                        self.proxies[collider.proxy_index].layer_id = layer_id;
+                    }
+                }
 
-                let layer = &mut self.layers[layer_id as usize];
+                layer_id
+            } else {
+                let layer_depth = super::layer_containing_aabb(&aabb);
+                let layer_id = self.ensure_layer_exists(layer_depth);
 
-                // Preupdate the collider in the layer.
-                layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool);
-                need_region_propagation =
-                    need_region_propagation || !layer.created_regions.is_empty();
-            }
-        }
+                // Create the proxy.
+                let proxy = SAPProxy::collider(handle, aabb, layer_id, layer_depth);
+                collider.proxy_index = self.proxies.insert(proxy);
+                layer_id
+            };
+
+            let layer = &mut self.layers[layer_id as usize];
+
+            // Preupdate the collider in the layer.
+            layer.preupdate_collider(collider, &aabb, &mut self.proxies, &mut self.region_pool);
+            need_region_propagation = need_region_propagation || !layer.created_regions.is_empty();
+        });
 
         // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
         if need_region_propagation {
@@ -527,7 +541,7 @@ mod test {
         broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
 
         bodies.remove(hrb, &mut colliders, &mut joints);
-        broad_phase.maintain(&mut colliders);
+        broad_phase.handle_user_changes(&mut colliders);
         broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
 
         // Create another body.
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs b/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs
index c27917b..fdc9bfd 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase_pair_event.rs
@@ -15,14 +15,6 @@ impl ColliderPair {
         }
     }
 
-    pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
-        if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
-            Self::new(collider1, collider2)
-        } else {
-            Self::new(collider2, collider1)
-        }
-    }
-
     pub fn swap(self) -> Self {
         Self::new(self.collider2, self.collider1)
     }
diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs
index 1ee4468..9a9c296 100644
--- a/src/geometry/broad_phase_multi_sap/sap_layer.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs
@@ -294,7 +294,7 @@ impl SAPLayer {
                     proxies[*subregion_id].data.as_region_mut().mark_as_dirty();
                 }
 
-                if region.subproper_proxy_count == 0 {
+                if !region.contains_subproper_proxies() {
                     self.regions_to_potentially_remove.push(*point);
                 }
 
@@ -325,7 +325,7 @@ impl SAPLayer {
                     region.update_after_subregion_removal(proxies, self.depth);
 
                     // Check if we can actually delete this region.
-                    if region.subproper_proxy_count == 0 {
+                    if !region.contains_subproper_proxies() {
                         let region_id = region_id.remove();
 
                         // We can delete this region. So we need to tell the larger
@@ -352,4 +352,21 @@ impl SAPLayer {
             }
         }
     }
+
+    pub fn proper_proxy_moved_to_bigger_layer(
+        &mut self,
+        proxies: &mut SAPProxies,
+        proxy_id: SAPProxyIndex,
+    ) {
+        for (point, region_id) in &self.regions {
+            let region = &mut proxies[*region_id].data.as_region_mut();
+            let region_contains_proxy = region.proper_proxy_moved_to_a_bigger_layer(proxy_id);
+
+            // If that proper proxy was the last one keeping that region
+            // alive, mark the region as potentially removable.
+            if region_contains_proxy && !region.contains_subproper_proxies() {
+                self.regions_to_potentially_remove.push(*point);
+            }
+        }
+    }
 }
diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs
index 1c3036d..4cebdda 100644
--- a/src/geometry/broad_phase_multi_sap/sap_region.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_region.rs
@@ -21,7 +21,7 @@ pub struct SAPRegion {
     // Number of proxies (added to this region) that originates
     // from the layer at depth <= the depth of the layer containing
     // this region.
-    pub subproper_proxy_count: usize,
+    subproper_proxy_count: usize,
 }
 
 impl SAPRegion {
@@ -73,6 +73,23 @@ impl SAPRegion {
         }
     }
 
+    /// Does this region still contain endpoints of subproper proxies?
+    pub fn contains_subproper_proxies(&self) -> bool {
+        self.subproper_proxy_count > 0
+    }
+
+    /// If this region contains the given proxy, this will decrement this region's proxy count.
+    ///
+    /// Returns `true` if this region contained the proxy. Returns `false` otherwise.
+    pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: SAPProxyIndex) -> bool {
+        if self.existing_proxies[proxy_id as usize] {
+            self.subproper_proxy_count -= 1;
+            true
+        } else {
+            false
+        }
+    }
+
     /// 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) {