Emit collision stopped events after a collider is removed.

6 files changed, 146 insertions, 71 deletions

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 4b0459e..b1b2aad 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -14,6 +14,7 @@
   to simplify the user’s event handling.
 - The `ActiveEvents::CONTACT_EVENTS` and `ActiveEvents::INTERSECTION_EVENTS` flags have been replaced by a single
   flag `ActiveEvents::COLLISION_EVENTS`.
+- Events `CollisionEvent::Stopped` are now generated after a collider is removed. 
 
 ## v0.12.0-alpha.0 (2 Jan. 2022)
 ### Fixed
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index a4b1419..bd3b20b 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -616,8 +616,8 @@ impl CCDSolver {
                     .contains(ActiveEvents::COLLISION_EVENTS)
             {
                 // Emit one intersection-started and one intersection-stopped event.
-                events.handle_intersection_event(CollisionEvent::Started(toi.c1, toi.c2));
-                events.handle_intersection_event(CollisionEvent::Stopped(toi.c1, toi.c2));
+                events.handle_collision_event(CollisionEvent::Started(toi.c1, toi.c2), None);
+                events.handle_collision_event(CollisionEvent::Stopped(toi.c1, toi.c2, false), None);
             }
         }
 
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 5826c7a..a75d58d 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -1,8 +1,11 @@
 use crate::dynamics::RigidBodyHandle;
 use crate::geometry::{ColliderHandle, Contact, ContactManifold};
 use crate::math::{Point, Real, Vector};
+use crate::pipeline::EventHandler;
 use parry::query::ContactManifoldsWorkspace;
 
+use super::CollisionEvent;
+
 bitflags::bitflags! {
     #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
     /// Flags affecting the behavior of the constraints solver for a given contact manifold.
@@ -47,6 +50,45 @@ impl Default for ContactData {
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug)]
+/// The description of all the contacts between a pair of colliders.
+pub struct IntersectionPair {
+    /// Are the colliders intersecting?
+    pub intersecting: bool,
+    /// Was a `CollisionEvent::Started` emitted for this collider?
+    pub(crate) start_event_emited: bool,
+}
+
+impl IntersectionPair {
+    pub(crate) fn new() -> Self {
+        Self {
+            intersecting: false,
+            start_event_emited: false,
+        }
+    }
+
+    pub(crate) fn emit_start_event(
+        &mut self,
+        collider1: ColliderHandle,
+        collider2: ColliderHandle,
+        events: &dyn EventHandler,
+    ) {
+        self.start_event_emited = true;
+        events.handle_collision_event(CollisionEvent::new(collider1, collider2, true), None);
+    }
+
+    pub(crate) fn emit_stop_event(
+        &mut self,
+        collider1: ColliderHandle,
+        collider2: ColliderHandle,
+        events: &dyn EventHandler,
+    ) {
+        self.start_event_emited = false;
+        events.handle_collision_event(CollisionEvent::new(collider1, collider2, false), None);
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// The description of all the contacts between a pair of colliders.
 pub struct ContactPair {
@@ -60,6 +102,8 @@ pub struct ContactPair {
     pub manifolds: Vec<ContactManifold>,
     /// Is there any active contact in this contact pair?
     pub has_any_active_contact: bool,
+    /// Was a `CollisionEvent::Started` emitted for this collider?
+    pub(crate) start_event_emited: bool,
     pub(crate) workspace: Option<ContactManifoldsWorkspace>,
 }
 
@@ -70,6 +114,7 @@ impl ContactPair {
             collider2,
             has_any_active_contact: false,
             manifolds: Vec::new(),
+            start_event_emited: false,
             workspace: None,
         }
     }
@@ -109,6 +154,24 @@ impl ContactPair {
 
         deepest
     }
+
+    pub(crate) fn emit_start_event(&mut self, events: &dyn EventHandler) {
+        self.start_event_emited = true;
+
+        events.handle_collision_event(
+            CollisionEvent::new(self.collider1, self.collider2, true),
+            Some(self),
+        );
+    }
+
+    pub(crate) fn emit_stop_event(&mut self, events: &dyn EventHandler) {
+        self.start_event_emited = false;
+
+        events.handle_collision_event(
+            CollisionEvent::new(self.collider1, self.collider2, false),
+            Some(self),
+        );
+    }
 }
 
 #[derive(Clone, Debug)]
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 83c687b..5a56f2b 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -2,8 +2,9 @@
 
 pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair};
 pub use self::collider_components::*;
-pub use self::contact_pair::{ContactData, ContactManifoldData};
-pub use self::contact_pair::{ContactPair, SolverContact, SolverFlags};
+pub use self::contact_pair::{
+    ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
+};
 pub use self::interaction_graph::{
     ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
 };
@@ -56,8 +57,11 @@ pub use parry::shape::SharedShape;
 pub enum CollisionEvent {
     /// Event occurring when two colliders start being in contact (or intersecting)
     Started(ColliderHandle, ColliderHandle),
-    /// Event occurring when two colliders stop being in contact (or intersecting)
-    Stopped(ColliderHandle, ColliderHandle),
+    /// Event occurring when two colliders stop being in contact (or intersecting).
+    ///
+    /// The boolean is set to `true` of this event originates from at least one of
+    /// the colliders being removed from the `ColliderSet`.
+    Stopped(ColliderHandle, ColliderHandle, bool),
 }
 
 impl CollisionEvent {
@@ -65,31 +69,31 @@ impl CollisionEvent {
         if start {
             Self::Started(h1, h2)
         } else {
-            Self::Stopped(h1, h2)
+            Self::Stopped(h1, h2, false)
         }
     }
 
     /// Is this a `Started` collision event?
     pub fn started(self) -> bool {
-        matches!(self, CollisionEvent::Started(_, _))
+        matches!(self, CollisionEvent::Started(..))
     }
 
     /// Is this a `Stopped` collision event?
     pub fn stopped(self) -> bool {
-        matches!(self, CollisionEvent::Stopped(_, _))
+        matches!(self, CollisionEvent::Stopped(..))
     }
 
     /// The handle of the first collider involved in this collision event.
     pub fn collider1(self) -> ColliderHandle {
         match self {
-            Self::Started(h, _) | Self::Stopped(h, _) => h,
+            Self::Started(h, _) | Self::Stopped(h, _, _) => h,
         }
     }
 
     /// The handle of the second collider involved in this collision event.
     pub fn collider2(self) -> ColliderHandle {
         match self {
-            Self::Started(_, h) | Self::Stopped(_, h) => h,
+            Self::Started(_, h) | Self::Stopped(_, h, _) => h,
         }
     }
 }
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 8a57ef4..d737bfc 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -10,7 +10,7 @@ use crate::geometry::{
     BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderMaterial,
     ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType, CollisionEvent,
     ContactData, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
-    SolverContact, SolverFlags,
+    IntersectionPair, SolverContact, SolverFlags,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{
@@ -56,7 +56,7 @@ pub struct NarrowPhase {
     )]
     query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>,
     contact_graph: InteractionGraph<ColliderHandle, ContactPair>,
-    intersection_graph: InteractionGraph<ColliderHandle, bool>,
+    intersection_graph: InteractionGraph<ColliderHandle, IntersectionPair>,
     graph_indices: Coarena<ColliderGraphIndices>,
 }
 
@@ -101,7 +101,7 @@ impl NarrowPhase {
     }
 
     /// The intersection graph containing all intersection pairs and their intersection information.
-    pub fn intersection_graph(&self) -> &InteractionGraph<ColliderHandle, bool> {
+    pub fn intersection_graph(&self) -> &InteractionGraph<ColliderHandle, IntersectionPair> {
         &self.intersection_graph
     }
 
@@ -146,7 +146,7 @@ impl NarrowPhase {
             .flat_map(move |id| {
                 self.intersection_graph
                     .interactions_with(id)
-                    .map(|e| (e.0, e.1, *e.2))
+                    .map(|e| (e.0, e.1, e.2.intersecting))
             })
     }
 
@@ -162,7 +162,7 @@ impl NarrowPhase {
             .flat_map(move |id| {
                 self.intersection_graph
                     .interactions_with(id)
-                    .map(|e| (e.0, e.1, *e.2))
+                    .map(|e| (e.0, e.1, e.2.intersecting))
             })
     }
 
@@ -211,7 +211,7 @@ impl NarrowPhase {
         let id2 = self.graph_indices.get_unknown_gen(collider2)?;
         self.intersection_graph
             .interaction_pair(id1.intersection_graph_index, id2.intersection_graph_index)
-            .map(|c| *c.2)
+            .map(|c| c.2.intersecting)
     }
 
     /// The intersection pair involving two specific colliders.
@@ -227,7 +227,7 @@ impl NarrowPhase {
         let id2 = self.graph_indices.get(collider2.0)?;
         self.intersection_graph
             .interaction_pair(id1.intersection_graph_index, id2.intersection_graph_index)
-            .map(|c| *c.2)
+            .map(|c| c.2.intersecting)
     }
 
     /// All the contact pairs maintained by this narrow-phase.
@@ -241,7 +241,7 @@ impl NarrowPhase {
     ) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, bool)> + '_ {
         self.intersection_graph
             .interactions_with_endpoints()
-            .map(|e| (e.0, e.1, *e.2))
+            .map(|e| (e.0, e.1, e.2.intersecting))
     }
 
     // #[cfg(feature = "parallel")]
@@ -297,6 +297,7 @@ impl NarrowPhase {
                     bodies,
                     &mut prox_id_remap,
                     &mut contact_id_remap,
+                    events,
                 );
             }
         }
@@ -308,20 +309,21 @@ impl NarrowPhase {
         &mut self,
         intersection_graph_id: ColliderGraphIndex,
         contact_graph_id: ColliderGraphIndex,
-        islands: Option<&mut IslandManager>,
+        mut islands: Option<&mut IslandManager>,
         colliders: &mut Colliders,
         bodies: &mut Bodies,
         prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
         contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
+        events: &dyn EventHandler,
     ) where
         Bodies: ComponentSetMut<RigidBodyActivation>
             + ComponentSet<RigidBodyType>
             + ComponentSetMut<RigidBodyIds>,
         Colliders: ComponentSetOption<ColliderParent>,
     {
-        // Wake up every body in contact with the deleted collider.
-        if let Some(islands) = islands {
-            for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
+        // Wake up every body in contact with the deleted collider and generate Stopped collision events.
+        if let Some(islands) = islands.as_deref_mut() {
+            for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
                 if let Some(parent) = colliders.get(a.0).map(|c| c.handle) {
                     islands.wake_up(bodies, parent, true)
                 }
@@ -329,6 +331,24 @@ impl NarrowPhase {
                 if let Some(parent) = colliders.get(b.0).map(|c| c.handle) {
                     islands.wake_up(bodies, parent, true)
                 }
+
+                if pair.start_event_emited {
+                    events.handle_collision_event(CollisionEvent::Stopped(a, b, true), Some(pair));
+                }
+            }
+        } else {
+            // If there is no island, don’t wake-up bodies, but do send the Stopped collision event.
+            for (a, b, pair) in self.contact_graph.interactions_with(contact_graph_id) {
+                if pair.start_event_emited {
+                    events.handle_collision_event(CollisionEvent::Stopped(a, b, true), Some(pair));
+                }
+            }
+        }
+
+        // Generate Stopped collision events for intersections.
+        for (a, b, pair) in self.intersection_graph.interactions_with(contact_graph_id) {
+            if pair.start_event_emited {
+                events.handle_collision_event(CollisionEvent::Stopped(a, b, true), None);
             }
         }
 
@@ -506,21 +526,21 @@ impl NarrowPhase {
                     || (mode == PairRemovalMode::Auto
                         && (co_type1.is_sensor() || co_type2.is_sensor()))
                 {
-                    let was_intersecting = self
+                    let intersection = 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 co_flag1: &ColliderFlags = colliders.index(pair.collider1.0);
-                        let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
+                    if let Some(mut intersection) = intersection {
+                        if intersection.intersecting {
+                            let co_flag1: &ColliderFlags = colliders.index(pair.collider1.0);
+                            let co_flag2: &ColliderFlags = colliders.index(pair.collider2.0);
 
-                        if (co_flag1.active_events | co_flag2.active_events)
-                            .contains(ActiveEvents::COLLISION_EVENTS)
-                        {
-                            let prox_event =
-                                CollisionEvent::Stopped(pair.collider1, pair.collider2);
-                            events.handle_intersection_event(prox_event)
+                            if (co_flag1.active_events | co_flag2.active_events)
+                                .contains(ActiveEvents::COLLISION_EVENTS)
+                            {
+                                intersection.emit_stop_event(pair.collider1, pair.collider2, events)
+                            }
                         }
                     }
                 } else {
@@ -530,7 +550,7 @@ impl NarrowPhase {
 
                     // 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 let Some(mut ctct) = contact_pair {
                         if ctct.has_any_active_contact {
                             let co_parent1: Option<&ColliderParent> =
                                 colliders.get(pair.collider1.0);
@@ -553,10 +573,7 @@ impl NarrowPhase {
                             if (co_flag1.active_events | co_flag2.active_events)
                                 .contains(ActiveEvents::COLLISION_EVENTS)
                             {
-                                events.handle_contact_event(
-                                    CollisionEvent::Stopped(pair.collider1, pair.collider2),
-                                    &ctct,
-                                )
+                                ctct.emit_stop_event(events);
                             }
                         }
                     }
@@ -615,7 +632,7 @@ impl NarrowPhase {
                     let _ = self.intersection_graph.add_edge(
                         gid1.intersection_graph_index,
                         gid2.intersection_graph_index,
-                        false,
+                        IntersectionPair::new(),
                     );
                 }
             } else {
@@ -712,7 +729,7 @@ 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 had_intersection = edge.weight;
+            let had_intersection = edge.weight.intersecting;
 
             // TODO: remove the `loop` once labels on blocks is stabilized.
             'emit_events: loop {
@@ -755,13 +772,13 @@ impl NarrowPhase {
                 if !co_flags1.active_collision_types.test(rb_type1, rb_type2)
                     && !co_flags2.active_collision_types.test(rb_type1, rb_type2)
                 {
-                    edge.weight = false;
+                    edge.weight.intersecting = false;
                     break 'emit_events;
                 }
 
                 // Filter based on collision groups.
                 if !co_flags1.collision_groups.test(co_flags2.collision_groups) {
-                    edge.weight = false;
+                    edge.weight.intersecting = false;
                     break 'emit_events;
                 }
 
@@ -779,13 +796,13 @@ impl NarrowPhase {
 
                     if !hooks.filter_intersection_pair(&context) {
                         // No intersection allowed.
-                        edge.weight = false;
+                        edge.weight.intersecting = false;
                         break 'emit_events;
                     }
                 }
 
                 let pos12 = co_pos1.inv_mul(co_pos2);
-                edge.weight = query_dispatcher
+                edge.weight.intersecting = query_dispatcher
                     .intersection_test(&pos12, &**co_shape1, &**co_shape2)
                     .unwrap_or(false);
                 break 'emit_events;
@@ -796,13 +813,13 @@ impl NarrowPhase {
             let active_events = co_flags1.active_events | co_flags2.active_events;
 
             if active_events.contains(ActiveEvents::COLLISION_EVENTS)
-                && had_intersection != edge.weight
+                && had_intersection != edge.weight.intersecting
             {
-                events.handle_intersection_event(CollisionEvent::new(
-                    handle1,
-                    handle2,
-                    edge.weight,
-                ));
+                if edge.weight.intersecting {
+                    edge.weight.emit_start_event(handle1, handle2, events);
+                } else {
+                    edge.weight.emit_stop_event(handle1, handle2, events);
+                }
             }
         });
     }
@@ -1029,15 +1046,9 @@ impl NarrowPhase {
             if pair.has_any_active_contact != had_any_active_contact {
                 if active_events.contains(ActiveEvents::COLLISION_EVENTS) {
                     if pair.has_any_active_contact {
-                        events.handle_contact_event(
-                            CollisionEvent::Started(pair.collider1, pair.collider2),
-                            pair,
-                        );
+                        pair.emit_start_event(events);
                     } else {
-                        events.handle_contact_event(
-                            CollisionEvent::Stopped(pair.collider1, pair.collider2),
-                            pair,
-                        );
+                        pair.emit_stop_event(events);
                     }
                 }
             }
diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs
index 5fa90c6..f08a383 100644
--- a/src/pipeline/event_handler.rs
+++ b/src/pipeline/event_handler.rs
@@ -23,18 +23,18 @@ impl Default for ActiveEvents {
 pub trait EventHandler: Send + Sync {
     /// Handle a collision event.
     ///
-    /// A intersection event is emitted when the state of intersection between two colliders changes.
-    fn handle_intersection_event(&self, event: CollisionEvent);
-    /// Handle a contact event.
+    /// A collision event is emitted when the state of intersection between two colliders changes.
     ///
-    /// A contact event is emitted when two collider start or stop touching, independently from the
-    /// number of contact points involved.
-    fn handle_contact_event(&self, event: CollisionEvent, contact_pair: &ContactPair);
+    /// # Parameters
+    /// * `event` - The collision event.
+    /// * `contact_pair` - The current state of contacts between the two colliders. This is set ot `None`
+    ///                    if at least one of the collider is a sensor (in which case no contact information
+    ///                    is ever computed).
+    fn handle_collision_event(&self, event: CollisionEvent, contact_pair: Option<&ContactPair>);
 }
 
 impl EventHandler for () {
-    fn handle_intersection_event(&self, _event: CollisionEvent) {}
-    fn handle_contact_event(&self, _event: CollisionEvent, _contact_pair: &ContactPair) {}
+    fn handle_collision_event(&self, _event: CollisionEvent, _contact_pair: Option<&ContactPair>) {}
 }
 
 /// A collision event handler that collects events into a crossbeam channel.
@@ -50,11 +50,7 @@ impl ChannelEventCollector {
 }
 
 impl EventHandler for ChannelEventCollector {
-    fn handle_intersection_event(&self, event: CollisionEvent) {
-        let _ = self.event_sender.send(event);
-    }
-
-    fn handle_contact_event(&self, event: CollisionEvent, _: &ContactPair) {
+    fn handle_collision_event(&self, event: CollisionEvent, _: Option<&ContactPair>) {
         let _ = self.event_sender.send(event);
     }
 }