Allow removing a rigid-body without auto-removing attached colliders

.vscode/tasks.json

@@ -2,6 +2,11 @@
     // See https://go.microsoft.com/fwlink/?LinkId=733558
     // for the documentation about the tasks.json format
     "version": "2.0.0",
+    "options": {
+        "env": {
+            "RUST_BACKTRACE": "1"
+        }
+    },
     "tasks": [
         {
             "label": "run 3d (no-simd - release) ",

examples3d/fountain3.rs

@@ -63,6 +63,7 @@ pub fn init_world(testbed: &mut Testbed) {
                     &mut physics.colliders,
                     &mut physics.impulse_joints,
                     &mut physics.multibody_joints,
+                    true,
                 );
 
                 if let Some(graphics) = &mut graphics {

src/dynamics/rigid_body_set.rs

@@ -141,6 +141,7 @@ impl RigidBodySet {
         colliders: &mut ColliderSet,
         impulse_joints: &mut ImpulseJointSet,
         multibody_joints: &mut MultibodyJointSet,
+        remove_attached_colliders: bool,
     ) -> Option<RigidBody> {
         let rb = self.bodies.remove(handle.0)?;
         /*
@@ -151,9 +152,17 @@ impl RigidBodySet {
         /*
          * Remove colliders attached to this rigid-body.
          */
+        if remove_attached_colliders {
             for collider in rb.colliders() {
                 colliders.remove(*collider, islands, self, false);
             }
+        } else {
+            // If we don’t remove the attached colliders, simply detach them.
+            let colliders_to_detach = rb.colliders().to_vec();
+            for co_handle in colliders_to_detach {
+                colliders.set_parent(co_handle, None, self);
+            }
+        }
 
         /*
          * Remove impulse_joints attached to this rigid-body.

src/geometry/collider_set.rs

@@ -6,6 +6,7 @@ use crate::geometry::{
     ColliderParent, ColliderPosition, ColliderShape, ColliderType,
 };
 use crate::geometry::{ColliderChanges, ColliderHandle};
+use crate::math::Isometry;
 use std::ops::{Index, IndexMut};
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -180,6 +181,54 @@ impl ColliderSet {
         handle
     }
 
+    /// Sets the parent of the given collider.
+    // TODO: find a way to define this as a method of Collider.
+    pub fn set_parent(
+        &mut self,
+        handle: ColliderHandle,
+        new_parent_handle: Option<RigidBodyHandle>,
+        bodies: &mut RigidBodySet,
+    ) {
+        if let Some(collider) = self.get_mut(handle) {
+            let curr_parent = collider.co_parent.map(|p| p.handle);
+            if new_parent_handle == curr_parent {
+                return; // Nothing to do, this is the same parent.
+            }
+
+            collider.co_changes |= ColliderChanges::PARENT;
+
+            if let Some(parent_handle) = curr_parent {
+                if let Some(rb) = bodies.get_mut(parent_handle) {
+                    rb.remove_collider_internal(handle, &*collider);
+                }
+            }
+
+            match new_parent_handle {
+                Some(new_parent_handle) => {
+                    if let Some(co_parent) = &mut collider.co_parent {
+                        co_parent.handle = new_parent_handle;
+                    } else {
+                        collider.co_parent = Some(ColliderParent {
+                            handle: new_parent_handle,
+                            pos_wrt_parent: Isometry::identity(),
+                        })
+                    };
+
+                    if let Some(rb) = bodies.get_mut(new_parent_handle) {
+                        rb.add_collider(
+                            handle,
+                            collider.co_parent.as_ref().unwrap(),
+                            &mut collider.co_pos,
+                            &collider.co_shape,
+                            &collider.co_mprops,
+                        );
+                    }
+                }
+                None => collider.co_parent = None,
+            }
+        }
+    }
+
     /// Remove a collider from this set and update its parent accordingly.
     ///
     /// If `wake_up` is `true`, the rigid-body the removed collider is attached to

src/geometry/contact_pair.rs

@@ -74,6 +74,12 @@ impl ContactPair {
         }
     }
 
+    pub fn clear(&mut self) {
+        self.manifolds.clear();
+        self.has_any_active_contact = false;
+        self.workspace = None;
+    }
+
     /// Finds the contact with the smallest signed distance.
     ///
     /// If the colliders involved in this contact pair are penetrating, then

src/geometry/narrow_phase.rs

@@ -385,7 +385,7 @@ impl NarrowPhase {
             if let Some(co_changes) = co_changes {
                 if co_changes.needs_narrow_phase_update() {
                     // No flag relevant to the narrow-phase is enabled for this collider.
-                    return;
+                    continue;
                 }
 
                 if let Some(gid) = self.graph_indices.get(handle.0) {
@@ -712,7 +712,10 @@ impl NarrowPhase {
         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 mut had_intersection = edge.weight;
 
+            // TODO: remove the `loop` once labels on blocks is stabilized.
+            'emit_events: loop {
                 let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
                 let (co_changes1, co_shape1, co_pos1, co_flags1): (
                     &ColliderChanges,
@@ -729,7 +732,8 @@ impl NarrowPhase {
                     &ColliderFlags,
                 ) = colliders.index_bundle(handle2.0);
 
-            if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
+                if !co_changes1.needs_narrow_phase_update()
+                    && !co_changes2.needs_narrow_phase_update()
                 {
                     // No update needed for these colliders.
                     return;
@@ -751,16 +755,17 @@ impl NarrowPhase {
                 if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
                     && !co_flags2.active_collision_types.test(rb_type1, rb_type2)
                 {
-                return;
+                    edge.weight = false;
+                    break 'emit_events;
                 }
 
                 // Filter based on collision groups.
                 if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
-                return;
+                    edge.weight = false;
+                    break 'emit_events;
                 }
 
                 let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
-            let active_events = co_flags1.active_events | co_flags2.active_events;
 
                 if active_hooks.contains(ActiveHooks::FILTER_INTERSECTION_PAIR) {
                     let context = PairFilterContext {
@@ -774,26 +779,31 @@ impl NarrowPhase {
 
                     if !hooks.filter_intersection_pair(&context) {
                         // No intersection allowed.
-                    return;
+                        edge.weight = false;
+                        break 'emit_events;
                     }
                 }
 
                 let pos12 = co_pos1.inv_mul(co_pos2);
+                edge.weight = query_dispatcher
+                    .intersection_test(&pos12, &**co_shape1, &**co_shape2)
+                    .unwrap_or(false);
+                break 'emit_events;
+            }
+
+            let co_flags1: &ColliderFlags = colliders.index(handle1.0);
+            let co_flags2: &ColliderFlags = colliders.index(handle2.0);
+            let active_events = co_flags1.active_events | co_flags2.active_events;
 
-            if let Ok(intersection) =
-                query_dispatcher.intersection_test(&pos12, &**co_shape1, &**co_shape2)
-            {
             if active_events.contains(ActiveEvents::INTERSECTION_EVENTS)
-                    && intersection != edge.weight
+                && had_intersection != edge.weight
             {
                 events.handle_intersection_event(IntersectionEvent::new(
                     handle1,
                     handle2,
-                        intersection,
+                    edge.weight,
                 ));
             }
-                edge.weight = intersection;
-            }
         });
     }
 
@@ -825,7 +835,10 @@ impl NarrowPhase {
         // 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 had_any_active_contact = pair.has_any_active_contact;
 
+            // TODO: remove the `loop` once labels on blocks are supported.
+            'emit_events: loop {
                 let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
                 let (co_changes1, co_shape1, co_pos1, co_material1, co_flags1): (
                     &ColliderChanges,
@@ -844,7 +857,8 @@ impl NarrowPhase {
                     &ColliderFlags,
                 ) = colliders.index_bundle(pair.collider2.0);
 
-            if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
+                if !co_changes1.needs_narrow_phase_update()
+                    && !co_changes2.needs_narrow_phase_update()
                 {
                     // No update needed for these colliders.
                     return;
@@ -866,16 +880,17 @@ impl NarrowPhase {
                 if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
                     && !co_flags2.active_collision_types.test(rb_type1, rb_type2)
                 {
-                return;
+                    pair.clear();
+                    break 'emit_events;
                 }
 
                 // Filter based on collision groups.
                 if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
-                return;
+                    pair.clear();
+                    break 'emit_events;
                 }
 
                 let active_hooks = co_flags1.active_hooks | co_flags2.active_hooks;
-            let active_events = co_flags1.active_events | co_flags2.active_events;
 
                 let mut solver_flags = if active_hooks.contains(ActiveHooks::FILTER_CONTACT_PAIRS) {
                     let context = PairFilterContext {
@@ -891,7 +906,8 @@ impl NarrowPhase {
                         solver_flags
                     } else {
                         // No contact allowed.
-                    return;
+                        pair.clear();
+                        break 'emit_events;
                     }
                 } else {
                     SolverFlags::default()
@@ -918,8 +934,6 @@ impl NarrowPhase {
                     &mut pair.workspace,
                 );
 
-            let mut has_any_active_contact = false;
-
                 let friction = CoefficientCombineRule::combine(
                     co_material1.friction,
                     co_material2.friction,
@@ -947,11 +961,12 @@ impl NarrowPhase {
                     manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
                     manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
                     manifold.data.solver_flags = solver_flags;
-                manifold.data.relative_dominance =
+                    manifold.data.relative_dominance = dominance1.effective_group(&rb_type1)
-                    dominance1.effective_group(&rb_type1) - dominance2.effective_group(&rb_type2);
+                        - dominance2.effective_group(&rb_type2);
                     manifold.data.normal = world_pos1 * manifold.local_n1;
 
                     // Generate solver contacts.
+                    pair.has_any_active_contact = false;
                     for (contact_id, contact) in manifold.points.iter().enumerate() {
                         assert!(
                             contact_id <= u8::MAX as usize,
@@ -972,7 +987,7 @@ impl NarrowPhase {
                             };
 
                             manifold.data.solver_contacts.push(solver_contact);
-                        has_any_active_contact = true;
+                            pair.has_any_active_contact = true;
                         }
                     }
 
@@ -1004,9 +1019,16 @@ impl NarrowPhase {
                     }
                 }
 
-            if has_any_active_contact != pair.has_any_active_contact {
+                break 'emit_events;
+            }
+
+            let co_flags1: &ColliderFlags = colliders.index(pair.collider1.0);
+            let co_flags2: &ColliderFlags = colliders.index(pair.collider2.0);
+            let active_events = co_flags1.active_events | co_flags2.active_events;
+
+            if pair.has_any_active_contact != had_any_active_contact {
                 if active_events.contains(ActiveEvents::CONTACT_EVENTS) {
-                    if has_any_active_contact {
+                    if pair.has_any_active_contact {
                         events.handle_contact_event(
                             ContactEvent::Started(pair.collider1, pair.collider2),
                             pair,
@@ -1018,8 +1040,6 @@ impl NarrowPhase {
                         );
                     }
                 }
-
-                pair.has_any_active_contact = has_any_active_contact;
             }
         });
     }

src_testbed/testbed.rs

@@ -682,6 +682,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
                             &mut self.harness.physics.colliders,
                             &mut self.harness.physics.impulse_joints,
                             &mut self.harness.physics.multibody_joints,
+                            true,
                         );
                     }
                 }