Allow collider modification after its insersion to the ColliderSet.

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);
-
-                    // 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();
+        colliders.foreach_modified_colliders_mut_internal(|handle, collider| {
+            if !collider.changes.needs_broad_phase_update() {
+                return;
             }
-        }
+
+            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;
+                    }
+                }
+
+                layer_id
+            } else {
+                let layer_depth = super::layer_containing_aabb(&aabb);
+                let layer_id = self.ensure_layer_exists(layer_depth);
+
+                // 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.

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)
     }

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

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) {

src/geometry/collider.rs

@@ -2,6 +2,7 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
 use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags};
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
+use na::Unit;
 use parry::bounding_volume::{BoundingVolume, AABB};
 use parry::shape::Shape;
 
@@ -49,6 +50,34 @@ enum MassInfo {
     MassProperties(Box<MassProperties>),
 }
 
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags describing how the collider has been modified by the user.
+    pub(crate) struct ColliderChanges: u32 {
+        const MODIFIED             = 1 << 0;
+        const POSITION_WRT_PARENT  = 1 << 1; // => BF & NF updates.
+        const POSITION             = 1 << 2; // => BF & NF updates.
+        const COLLISION_GROUPS     = 1 << 3; // => NF update.
+        const SOLVER_GROUPS        = 1 << 4; // => NF update.
+        const SHAPE                = 1 << 5; // => BF & NF update. NF pair workspace invalidation.
+        const SENSOR               = 1 << 6; // => NF update. NF pair invalidation.
+    }
+}
+
+impl ColliderChanges {
+    pub fn needs_broad_phase_update(self) -> bool {
+        self.intersects(
+            ColliderChanges::POSITION_WRT_PARENT
+                | ColliderChanges::POSITION
+                | ColliderChanges::SHAPE,
+        )
+    }
+
+    pub fn needs_narrow_phase_update(self) -> bool {
+        self.bits() > 1
+    }
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
@@ -59,10 +88,10 @@ pub struct Collider {
     mass_info: MassInfo,
     pub(crate) flags: ColliderFlags,
     pub(crate) solver_flags: SolverFlags,
+    pub(crate) changes: ColliderChanges,
     pub(crate) parent: RigidBodyHandle,
     pub(crate) delta: Isometry<Real>,
     pub(crate) position: Isometry<Real>,
-    pub(crate) prev_position: Isometry<Real>,
     /// The friction coefficient of this collider.
     pub friction: Real,
     /// The restitution coefficient of this collider.
@@ -78,6 +107,7 @@ impl Collider {
     pub(crate) fn reset_internal_references(&mut self) {
         self.parent = RigidBodyHandle::invalid();
         self.proxy_index = crate::INVALID_U32;
+        self.changes = ColliderChanges::empty();
     }
 
     /// The rigid body this collider is attached to.
@@ -90,6 +120,42 @@ impl Collider {
         self.flags.is_sensor()
     }
 
+    /// The combine rule used by this collider to combine its friction
+    /// coefficient with the friction coefficient of the other collider it
+    /// is in contact with.
+    pub fn friction_combine_rule(&self) -> CoefficientCombineRule {
+        CoefficientCombineRule::from_value(self.flags.friction_combine_rule_value())
+    }
+
+    /// Sets the combine rule used by this collider to combine its friction
+    /// coefficient with the friction coefficient of the other collider it
+    /// is in contact with.
+    pub fn set_friction_combine_rule(&mut self, rule: CoefficientCombineRule) {
+        self.flags = self.flags.with_friction_combine_rule(rule);
+    }
+
+    /// The combine rule used by this collider to combine its restitution
+    /// coefficient with the restitution coefficient of the other collider it
+    /// is in contact with.
+    pub fn restitution_combine_rule(&self) -> CoefficientCombineRule {
+        CoefficientCombineRule::from_value(self.flags.restitution_combine_rule_value())
+    }
+
+    /// Sets the combine rule used by this collider to combine its restitution
+    /// coefficient with the restitution coefficient of the other collider it
+    /// is in contact with.
+    pub fn set_restitution_combine_rule(&mut self, rule: CoefficientCombineRule) {
+        self.flags = self.flags.with_restitution_combine_rule(rule)
+    }
+
+    /// Sets whether or not this is a sensor collider.
+    pub fn set_sensor(&mut self, is_sensor: bool) {
+        if is_sensor != self.is_sensor() {
+            self.changes.insert(ColliderChanges::SENSOR);
+            self.flags.set(ColliderFlags::SENSOR, is_sensor);
+        }
+    }
+
     #[doc(hidden)]
     pub fn set_position_debug(&mut self, position: Isometry<Real>) {
         self.position = position;
@@ -106,21 +172,49 @@ impl Collider {
         &self.position
     }
 
+    /// Sets the position of this collider wrt. its parent rigid-body.
+    pub(crate) fn set_position(&mut self, position: Isometry<Real>) {
+        self.changes.insert(ColliderChanges::POSITION);
+        self.position = position;
+    }
+
     /// The position of this collider wrt the body it is attached to.
     pub fn position_wrt_parent(&self) -> &Isometry<Real> {
         &self.delta
     }
 
+    /// Sets the position of this collider wrt. its parent rigid-body.
+    pub fn set_position_wrt_parent(&mut self, position: Isometry<Real>) {
+        self.changes.insert(ColliderChanges::POSITION_WRT_PARENT);
+        self.delta = position;
+    }
+
     /// The collision groups used by this collider.
     pub fn collision_groups(&self) -> InteractionGroups {
         self.collision_groups
     }
 
+    /// Sets the collision groups of this collider.
+    pub fn set_collision_groups(&mut self, groups: InteractionGroups) {
+        if self.collision_groups != groups {
+            self.changes.insert(ColliderChanges::COLLISION_GROUPS);
+            self.collision_groups = groups;
+        }
+    }
+
     /// The solver groups used by this collider.
     pub fn solver_groups(&self) -> InteractionGroups {
         self.solver_groups
     }
 
+    /// Sets the solver groups of this collider.
+    pub fn set_solver_groups(&mut self, groups: InteractionGroups) {
+        if self.solver_groups != groups {
+            self.changes.insert(ColliderChanges::SOLVER_GROUPS);
+            self.solver_groups = groups;
+        }
+    }
+
     /// The density of this collider, if set.
     pub fn density(&self) -> Option<Real> {
         match &self.mass_info {
@@ -134,6 +228,12 @@ impl Collider {
         &*self.shape.0
     }
 
+    /// Sets the shape of this collider.
+    pub fn set_shape(&mut self, shape: SharedShape) {
+        self.changes.insert(ColliderChanges::SHAPE);
+        self.shape = shape;
+    }
+
     /// Compute the axis-aligned bounding box of this collider.
     pub fn compute_aabb(&self) -> AABB {
         self.shape.compute_aabb(&self.position)
@@ -219,6 +319,12 @@ impl ColliderBuilder {
         Self::new(SharedShape::ball(radius))
     }
 
+    /// Initialize a new collider build with a half-space shape defined by the outward normal
+    /// of its planar boundary.
+    pub fn halfspace(outward_normal: Unit<Vector<Real>>) -> Self {
+        Self::new(SharedShape::halfspace(outward_normal))
+    }
+
     /// Initialize a new collider builder with a cylindrical shape defined by its half-height
     /// (along along the y axis) and its radius.
     #[cfg(feature = "dim3")]
@@ -595,8 +701,8 @@ impl ColliderBuilder {
             delta: self.delta,
             flags,
             solver_flags,
+            changes: ColliderChanges::all(),
             parent: RigidBodyHandle::invalid(),
-            prev_position: Isometry::identity(),
             position: Isometry::identity(),
             proxy_index: crate::INVALID_U32,
             collision_groups: self.collision_groups,

src/geometry/collider_set.rs

@@ -1,6 +1,7 @@
 use crate::data::arena::Arena;
 use crate::data::pubsub::PubSub;
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
+use crate::geometry::collider::ColliderChanges;
 use crate::geometry::{Collider, SAPProxyIndex};
 use parry::partitioning::IndexedData;
 use std::ops::{Index, IndexMut};
@@ -54,6 +55,8 @@ pub(crate) struct RemovedCollider {
 pub struct ColliderSet {
     pub(crate) removed_colliders: PubSub<RemovedCollider>,
     pub(crate) colliders: Arena<Collider>,
+    pub(crate) modified_colliders: Vec<ColliderHandle>,
+    pub(crate) modified_all_colliders: bool,
 }
 
 impl ColliderSet {
@@ -62,6 +65,8 @@ impl ColliderSet {
         ColliderSet {
             removed_colliders: PubSub::new(),
             colliders: Arena::new(),
+            modified_colliders: Vec::new(),
+            modified_all_colliders: false,
         }
     }
 
@@ -75,6 +80,37 @@ impl ColliderSet {
         self.colliders.iter().map(|(h, c)| (ColliderHandle(h), c))
     }
 
+    /// Iterates mutably through all the colliders on this set.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
+        self.modified_colliders.clear();
+        self.modified_all_colliders = true;
+        self.colliders
+            .iter_mut()
+            .map(|(h, b)| (ColliderHandle(h), b))
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_modified_colliders(&self, mut f: impl FnMut(ColliderHandle, &Collider)) {
+        for handle in &self.modified_colliders {
+            if let Some(rb) = self.colliders.get(handle.0) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_modified_colliders_mut_internal(
+        &mut self,
+        mut f: impl FnMut(ColliderHandle, &mut Collider),
+    ) {
+        for handle in &self.modified_colliders {
+            if let Some(rb) = self.colliders.get_mut(handle.0) {
+                f(*handle, rb)
+            }
+        }
+    }
+
     /// The number of colliders on this set.
     pub fn len(&self) -> usize {
         self.colliders.len()
@@ -90,6 +126,24 @@ impl ColliderSet {
         self.colliders.contains(handle.0)
     }
 
+    pub(crate) fn contains_any_modified_collider(&self) -> bool {
+        self.modified_all_colliders || !self.modified_colliders.is_empty()
+    }
+
+    pub(crate) fn clear_modified_colliders(&mut self) {
+        if self.modified_all_colliders {
+            for collider in self.colliders.iter_mut() {
+                collider.1.changes = ColliderChanges::empty();
+            }
+            self.modified_colliders.clear();
+            self.modified_all_colliders = false;
+        } else {
+            for handle in self.modified_colliders.drain(..) {
+                self.colliders[handle.0].changes = ColliderChanges::empty();
+            }
+        }
+    }
+
     /// Inserts a new collider to this set and retrieve its handle.
     pub fn insert(
         &mut self,
@@ -106,11 +160,12 @@ impl ColliderSet {
         // NOTE: we use `get_mut` instead of `get_mut_internal` so that the
         // modification flag is updated properly.
         let parent = bodies
-            .get_mut(parent_handle)
+            .get_mut_internal_with_modification_tracking(parent_handle)
             .expect("Parent rigid body not found.");
-        coll.prev_position = parent.position * coll.delta;
         coll.position = parent.position * coll.delta;
         let handle = ColliderHandle(self.colliders.insert(coll));
+        self.modified_colliders.push(handle);
+
         let coll = self.colliders.get(handle.0).unwrap();
         parent.add_collider(handle, &coll);
         handle
@@ -133,7 +188,7 @@ impl ColliderSet {
          */
         // NOTE: we use `get_mut` instead of `get_mut_internal` so that the
         // modification flag is updated properly.
-        if let Some(parent) = bodies.get_mut(collider.parent) {
+        if let Some(parent) = bodies.get_mut_internal_with_modification_tracking(collider.parent) {
             parent.remove_collider_internal(handle, &collider);
 
             if wake_up {
@@ -178,10 +233,17 @@ impl ColliderSet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> {
-        self.colliders
-            .get_unknown_gen_mut(i)
-            .map(|(c, h)| (c, ColliderHandle(h)))
+        let (collider, handle) = self.colliders.get_unknown_gen_mut(i)?;
+        let handle = ColliderHandle(handle);
+        Self::mark_as_modified(
+            handle,
+            collider,
+            &mut self.modified_colliders,
+            self.modified_all_colliders,
+        );
+        Some((collider, handle))
     }
 
     /// Get the collider with the given handle.
@@ -189,31 +251,79 @@ impl ColliderSet {
         self.colliders.get(handle.0)
     }
 
+    fn mark_as_modified(
+        handle: ColliderHandle,
+        collider: &mut Collider,
+        modified_colliders: &mut Vec<ColliderHandle>,
+        modified_all_colliders: bool,
+    ) {
+        if !modified_all_colliders && !collider.changes.contains(ColliderChanges::MODIFIED) {
+            collider.changes = ColliderChanges::MODIFIED;
+            modified_colliders.push(handle);
+        }
+    }
+
     /// Gets a mutable reference to the collider with the given handle.
+    #[cfg(not(feature = "dev-remove-slow-accessors"))]
     pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
+        let result = self.colliders.get_mut(handle.0)?;
+        Self::mark_as_modified(
+            handle,
+            result,
+            &mut self.modified_colliders,
+            self.modified_all_colliders,
+        );
+        Some(result)
+    }
+
+    pub(crate) fn get_mut_internal(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
         self.colliders.get_mut(handle.0)
     }
 
-    // pub(crate) fn get2_mut_internal(
-    //     &mut self,
-    //     h1: ColliderHandle,
-    //     h2: ColliderHandle,
-    // ) -> (Option<&mut Collider>, Option<&mut Collider>) {
-    //     self.colliders.get2_mut(h1, h2)
-    // }
+    // Just a very long name instead of `.get_mut` to make sure
+    // this is really the method we wanted to use instead of `get_mut_internal`.
+    pub(crate) fn get_mut_internal_with_modification_tracking(
+        &mut self,
+        handle: ColliderHandle,
+    ) -> Option<&mut Collider> {
+        let result = self.colliders.get_mut(handle.0)?;
+        Self::mark_as_modified(
+            handle,
+            result,
+            &mut self.modified_colliders,
+            self.modified_all_colliders,
+        );
+        Some(result)
+    }
 
-    // pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, ColliderMut)> {
-    //     //        let sender = &self.activation_channel_sender;
-    //     self.colliders.iter_mut().map(move |(h, rb)| {
-    //         (h, ColliderMut::new(h, rb /*sender.clone()*/))
-    //     })
-    // }
+    // Utility function to avoid some borrowing issue in the `maintain` method.
+    fn maintain_one(bodies: &mut RigidBodySet, collider: &mut Collider) {
+        if collider
+            .changes
+            .contains(ColliderChanges::POSITION_WRT_PARENT)
+        {
+            if let Some(parent) = bodies.get_mut_internal(collider.parent()) {
+                let position = parent.position * collider.position_wrt_parent();
+                // NOTE: the set_position method will add the ColliderChanges::POSITION flag,
+                //       which is needed for the broad-phase/narrow-phase to detect the change.
+                collider.set_position(position);
+            }
+        }
+    }
 
-    //    pub(crate) fn iter_mut_internal(
-    //        &mut self,
-    //    ) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
-    //        self.colliders.iter_mut()
-    //    }
+    pub(crate) fn handle_user_changes(&mut self, bodies: &mut RigidBodySet) {
+        if self.modified_all_colliders {
+            for (_, rb) in self.colliders.iter_mut() {
+                Self::maintain_one(bodies, rb)
+            }
+        } else {
+            for handle in self.modified_colliders.drain(..) {
+                if let Some(rb) = self.colliders.get_mut(handle.0) {
+                    Self::maintain_one(bodies, rb)
+                }
+            }
+        }
+    }
 }
 
 impl Index<ColliderHandle> for ColliderSet {
@@ -224,8 +334,16 @@ impl Index<ColliderHandle> for ColliderSet {
     }
 }
 
+#[cfg(not(feature = "dev-remove-slow-accessors"))]
 impl IndexMut<ColliderHandle> for ColliderSet {
-    fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider {
-        &mut self.colliders[index.0]
+    fn index_mut(&mut self, handle: ColliderHandle) -> &mut Collider {
+        let collider = &mut self.colliders[handle.0];
+        Self::mark_as_modified(
+            handle,
+            collider,
+            &mut self.modified_colliders,
+            self.modified_all_colliders,
+        );
+        collider
     }
 }

src/geometry/narrow_phase.rs

@@ -4,9 +4,10 @@ use rayon::prelude::*;
 use crate::data::pubsub::Subscription;
 use crate::data::Coarena;
 use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet};
+use crate::geometry::collider::ColliderChanges;
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderSet, ContactData,
-    ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
+    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet,
+    ContactData, ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
     IntersectionEvent, RemovedCollider, SolverContact, SolverFlags,
 };
 use crate::math::{Real, Vector};
@@ -34,6 +35,13 @@ impl ColliderGraphIndices {
     }
 }
 
+#[derive(Copy, Clone, PartialEq, Eq)]
+enum PairRemovalMode {
+    FromContactGraph,
+    FromIntersectionGraph,
+    Auto,
+}
+
 /// The narrow-phase responsible for computing precise contact information between colliders.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
@@ -164,7 +172,12 @@ impl NarrowPhase {
     // }
 
     /// Maintain the narrow-phase internal state by taking collider removal into account.
-    pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) {
+    pub fn handle_user_changes(
+        &mut self,
+        colliders: &mut ColliderSet,
+        bodies: &mut RigidBodySet,
+        events: &dyn EventHandler,
+    ) {
         // Ensure we already subscribed.
         if self.removed_colliders.is_none() {
             self.removed_colliders = Some(colliders.removed_colliders.subscribe());
@@ -207,6 +220,8 @@ impl NarrowPhase {
 
         colliders.removed_colliders.ack(&cursor);
         self.removed_colliders = Some(cursor);
+
+        self.handle_modified_colliders(colliders, bodies, events);
     }
 
     pub(crate) fn remove_collider(
@@ -248,6 +263,212 @@ impl NarrowPhase {
         }
     }
 
+    pub(crate) fn handle_modified_colliders(
+        &mut self,
+        colliders: &mut ColliderSet,
+        bodies: &mut RigidBodySet,
+        events: &dyn EventHandler,
+    ) {
+        let mut pairs_to_remove = vec![];
+
+        colliders.foreach_modified_colliders(|handle, collider| {
+            if collider.changes.needs_narrow_phase_update() {
+                // No flag relevant to the narrow-phase is enabled for this collider.
+                return;
+            }
+
+            if let Some(gid) = self.graph_indices.get(handle.0) {
+                // For each modified colliders, we need to wake-up the bodies it is in contact with
+                // so that the narrow-phase properly takes into account the change in, e.g.,
+                // collision groups. Waking up the modified collider's parent isn't enough because
+                // it could be a static or kinematic body which don't propagate the wake-up state.
+                bodies.wake_up(collider.parent, true);
+
+                for inter in self
+                    .contact_graph
+                    .interactions_with(gid.contact_graph_index)
+                {
+                    let other_handle = if handle == inter.0 { inter.1 } else { inter.0 };
+                    if let Some(other_collider) = colliders.get(other_handle) {
+                        bodies.wake_up(other_collider.parent, true);
+                    }
+                }
+
+                // For each collider which had their sensor status modified, we need
+                // to transfer their contact/intersection graph edges to the intersection/contact graph.
+                // To achieve this we will remove the relevant contact/intersection pairs form the
+                // contact/intersection graphs, and then add them into the other graph.
+                if collider.changes.contains(ColliderChanges::SENSOR) {
+                    if collider.is_sensor() {
+                        // Find the contact pairs for this collider and
+                        // push them to `pairs_to_remove`.
+                        for inter in self
+                            .contact_graph
+                            .interactions_with(gid.contact_graph_index)
+                        {
+                            pairs_to_remove.push((
+                                ColliderPair::new(inter.0, inter.1),
+                                PairRemovalMode::FromContactGraph,
+                            ));
+                        }
+                    } else {
+                        // Find the contact pairs for this collider and
+                        // push them to `pairs_to_remove` if both involved
+                        // colliders are not sensors.
+                        for inter in self
+                            .intersection_graph
+                            .interactions_with(gid.intersection_graph_index)
+                            .filter(|(h1, h2, _)| {
+                                !colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
+                            })
+                        {
+                            pairs_to_remove.push((
+                                ColliderPair::new(inter.0, inter.1),
+                                PairRemovalMode::FromIntersectionGraph,
+                            ));
+                        }
+                    }
+                }
+            }
+        });
+
+        // Remove the pair from the relevant graph.
+        for pair in &pairs_to_remove {
+            self.remove_pair(colliders, bodies, &pair.0, events, pair.1);
+        }
+
+        // Add the paid removed pair to the relevant graph.
+        for pair in pairs_to_remove {
+            self.add_pair(colliders, &pair.0);
+        }
+    }
+
+    fn remove_pair(
+        &mut self,
+        colliders: &mut ColliderSet,
+        bodies: &mut RigidBodySet,
+        pair: &ColliderPair,
+        events: &dyn EventHandler,
+        mode: PairRemovalMode,
+    ) {
+        if let (Some(co1), Some(co2)) =
+            (colliders.get(pair.collider1), colliders.get(pair.collider2))
+        {
+            // TODO: could we just unwrap here?
+            // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
+            if let (Some(gid1), Some(gid2)) = (
+                self.graph_indices.get(pair.collider1.0),
+                self.graph_indices.get(pair.collider2.0),
+            ) {
+                if mode == PairRemovalMode::FromIntersectionGraph
+                    || (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
+                {
+                    let was_intersecting = self
+                        .intersection_graph
+                        .remove_edge(gid1.intersection_graph_index, gid2.intersection_graph_index);
+
+                    // Emit an intersection lost event if we had an intersection before removing the edge.
+                    if Some(true) == was_intersecting {
+                        let prox_event =
+                            IntersectionEvent::new(pair.collider1, pair.collider2, false);
+                        events.handle_intersection_event(prox_event)
+                    }
+                } else {
+                    let contact_pair = self
+                        .contact_graph
+                        .remove_edge(gid1.contact_graph_index, gid2.contact_graph_index);
+
+                    // Emit a contact stopped event if we had a contact before removing the edge.
+                    // Also wake up the dynamic bodies that were in contact.
+                    if let Some(ctct) = contact_pair {
+                        if ctct.has_any_active_contact {
+                            bodies.wake_up(co1.parent, true);
+                            bodies.wake_up(co2.parent, true);
+
+                            events.handle_contact_event(ContactEvent::Stopped(
+                                pair.collider1,
+                                pair.collider2,
+                            ))
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    fn add_pair(&mut self, colliders: &mut ColliderSet, pair: &ColliderPair) {
+        if let (Some(co1), Some(co2)) =
+            (colliders.get(pair.collider1), colliders.get(pair.collider2))
+        {
+            if co1.parent == co2.parent {
+                // Same parents. Ignore collisions.
+                return;
+            }
+
+            let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
+                pair.collider1.0,
+                pair.collider2.0,
+                ColliderGraphIndices::invalid(),
+            );
+
+            if co1.is_sensor() || co2.is_sensor() {
+                // NOTE: the collider won't have a graph index as long
+                // as it does not interact with anything.
+                if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index)
+                {
+                    gid1.intersection_graph_index =
+                        self.intersection_graph.graph.add_node(pair.collider1);
+                }
+
+                if !InteractionGraph::<(), ()>::is_graph_index_valid(gid2.intersection_graph_index)
+                {
+                    gid2.intersection_graph_index =
+                        self.intersection_graph.graph.add_node(pair.collider2);
+                }
+
+                if self
+                    .intersection_graph
+                    .graph
+                    .find_edge(gid1.intersection_graph_index, gid2.intersection_graph_index)
+                    .is_none()
+                {
+                    let _ = self.intersection_graph.add_edge(
+                        gid1.intersection_graph_index,
+                        gid2.intersection_graph_index,
+                        false,
+                    );
+                }
+            } else {
+                // NOTE: same code as above, but for the contact graph.
+                // TODO: refactor both pieces of code somehow?
+
+                // NOTE: the collider won't have a graph index as long
+                // as it does not interact with anything.
+                if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.contact_graph_index) {
+                    gid1.contact_graph_index = self.contact_graph.graph.add_node(pair.collider1);
+                }
+
+                if !InteractionGraph::<(), ()>::is_graph_index_valid(gid2.contact_graph_index) {
+                    gid2.contact_graph_index = self.contact_graph.graph.add_node(pair.collider2);
+                }
+
+                if self
+                    .contact_graph
+                    .graph
+                    .find_edge(gid1.contact_graph_index, gid2.contact_graph_index)
+                    .is_none()
+                {
+                    let interaction = ContactPair::new(*pair);
+                    let _ = self.contact_graph.add_edge(
+                        gid1.contact_graph_index,
+                        gid2.contact_graph_index,
+                        interaction,
+                    );
+                }
+            }
+        }
+    }
+
     pub(crate) fn register_pairs(
         &mut self,
         colliders: &mut ColliderSet,
@@ -258,135 +479,10 @@ impl NarrowPhase {
         for event in broad_phase_events {
             match event {
                 BroadPhasePairEvent::AddPair(pair) => {
-                    if let (Some(co1), Some(co2)) =
-                        (colliders.get(pair.collider1), colliders.get(pair.collider2))
-                    {
-                        if co1.parent == co2.parent {
-                            // Same parents. Ignore collisions.
-                            continue;
-                        }
-
-                        let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
-                            pair.collider1.0,
-                            pair.collider2.0,
-                            ColliderGraphIndices::invalid(),
-                        );
-
-                        if co1.is_sensor() || co2.is_sensor() {
-                            // NOTE: the collider won't have a graph index as long
-                            // as it does not interact with anything.
-                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
-                                gid1.intersection_graph_index,
-                            ) {
-                                gid1.intersection_graph_index =
-                                    self.intersection_graph.graph.add_node(pair.collider1);
-                            }
-
-                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
-                                gid2.intersection_graph_index,
-                            ) {
-                                gid2.intersection_graph_index =
-                                    self.intersection_graph.graph.add_node(pair.collider2);
-                            }
-
-                            if self
-                                .intersection_graph
-                                .graph
-                                .find_edge(
-                                    gid1.intersection_graph_index,
-                                    gid2.intersection_graph_index,
-                                )
-                                .is_none()
-                            {
-                                let _ = self.intersection_graph.add_edge(
-                                    gid1.intersection_graph_index,
-                                    gid2.intersection_graph_index,
-                                    false,
-                                );
-                            }
-                        } else {
-                            // NOTE: same code as above, but for the contact graph.
-                            // TODO: refactor both pieces of code somehow?
-
-                            // NOTE: the collider won't have a graph index as long
-                            // as it does not interact with anything.
-                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
-                                gid1.contact_graph_index,
-                            ) {
-                                gid1.contact_graph_index =
-                                    self.contact_graph.graph.add_node(pair.collider1);
-                            }
-
-                            if !InteractionGraph::<(), ()>::is_graph_index_valid(
-                                gid2.contact_graph_index,
-                            ) {
-                                gid2.contact_graph_index =
-                                    self.contact_graph.graph.add_node(pair.collider2);
-                            }
-
-                            if self
-                                .contact_graph
-                                .graph
-                                .find_edge(gid1.contact_graph_index, gid2.contact_graph_index)
-                                .is_none()
-                            {
-                                let interaction = ContactPair::new(*pair);
-                                let _ = self.contact_graph.add_edge(
-                                    gid1.contact_graph_index,
-                                    gid2.contact_graph_index,
-                                    interaction,
-                                );
-                            }
-                        }
-                    }
+                    self.add_pair(colliders, pair);
                 }
                 BroadPhasePairEvent::DeletePair(pair) => {
-                    if let (Some(co1), Some(co2)) =
-                        (colliders.get(pair.collider1), colliders.get(pair.collider2))
-                    {
-                        // TODO: could we just unwrap here?
-                        // Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
-                        if let (Some(gid1), Some(gid2)) = (
-                            self.graph_indices.get(pair.collider1.0),
-                            self.graph_indices.get(pair.collider2.0),
-                        ) {
-                            if co1.is_sensor() || co2.is_sensor() {
-                                let was_intersecting = self.intersection_graph.remove_edge(
-                                    gid1.intersection_graph_index,
-                                    gid2.intersection_graph_index,
-                                );
-
-                                // Emit an intersection lost event if we had an intersection before removing the edge.
-                                if Some(true) == was_intersecting {
-                                    let prox_event = IntersectionEvent::new(
-                                        pair.collider1,
-                                        pair.collider2,
-                                        false,
-                                    );
-                                    events.handle_intersection_event(prox_event)
-                                }
-                            } else {
-                                let contact_pair = self.contact_graph.remove_edge(
-                                    gid1.contact_graph_index,
-                                    gid2.contact_graph_index,
-                                );
-
-                                // Emit a contact stopped event if we had a contact before removing the edge.
-                                // Also wake up the dynamic bodies that were in contact.
-                                if let Some(ctct) = contact_pair {
-                                    if ctct.has_any_active_contact {
-                                        bodies.wake_up(co1.parent, true);
-                                        bodies.wake_up(co2.parent, true);
-
-                                        events.handle_contact_event(ContactEvent::Stopped(
-                                            pair.collider1,
-                                            pair.collider2,
-                                        ))
-                                    }
-                                }
-                            }
-                        }
-                    }
+                    self.remove_pair(colliders, bodies, pair, events, PairRemovalMode::Auto);
                 }
             }
         }
@@ -399,17 +495,28 @@ impl NarrowPhase {
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
+        if !colliders.contains_any_modified_collider() {
+            return;
+        }
+
         let nodes = &self.intersection_graph.graph.nodes;
         let query_dispatcher = &*self.query_dispatcher;
         let active_hooks = hooks.active_hooks();
 
+        // TODO: don't iterate on all the edges.
         par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
             let handle1 = nodes[edge.source().index()].weight;
             let handle2 = nodes[edge.target().index()].weight;
             let co1 = &colliders[handle1];
             let co2 = &colliders[handle2];
 
-            // FIXME: avoid lookup into bodies.
+            if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
+            {
+                // No update needed for these colliders.
+                return;
+            }
+
+            // TODO: avoid lookup into bodies.
             let rb1 = &bodies[co1.parent];
             let rb2 = &bodies[co2.parent];
 
@@ -475,15 +582,26 @@ impl NarrowPhase {
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
+        if !colliders.contains_any_modified_collider() {
+            return;
+        }
+
         let query_dispatcher = &*self.query_dispatcher;
         let active_hooks = hooks.active_hooks();
 
+        // TODO: don't iterate on all the edges.
         par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
             let pair = &mut edge.weight;
             let co1 = &colliders[pair.pair.collider1];
             let co2 = &colliders[pair.pair.collider2];
 
-            // FIXME: avoid lookup into bodies.
+            if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
+            {
+                // No update needed for these colliders.
+                return;
+            }
+
+            // TODO: avoid lookup into bodies.
             let rb1 = &bodies[co1.parent];
             let rb2 = &bodies[co2.parent];
 
@@ -533,6 +651,13 @@ impl NarrowPhase {
                 solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
             }
 
+            if co1.changes.contains(ColliderChanges::SHAPE)
+                || co2.changes.contains(ColliderChanges::SHAPE)
+            {
+                // The shape changed so the workspace is no longer valid.
+                pair.workspace = None;
+            }
+
             let pos12 = co1.position().inv_mul(co2.position());
             let _ = query_dispatcher.contact_manifolds(
                 &pos12,
@@ -657,7 +782,7 @@ impl NarrowPhase {
             out_island.clear();
         }
 
-        // FIXME: don't iterate through all the interactions.
+        // TODO: don't iterate through all the interactions.
         for inter in self.contact_graph.graph.edges.iter_mut() {
             for manifold in &mut inter.weight.manifolds {
                 let rb1 = &bodies[manifold.data.body_pair.body1];