Merge pull request #68 from dimforge/read_contacts

Allow access to contact information

9 files changed, 270 insertions, 105 deletions

diff --git a/src/data/coarena.rs b/src/data/coarena.rs
new file mode 100644
index 0000000..78cbfa7
--- /dev/null
+++ b/src/data/coarena.rs
@@ -0,0 +1,71 @@
+use crate::data::arena::Index;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone, Debug)]
+/// A container for data associated to item existing into another Arena.
+pub struct Coarena<T> {
+    data: Vec<(u64, T)>,
+}
+
+impl<T> Coarena<T> {
+    /// A coarena with no element.
+    pub fn new() -> Self {
+        Self { data: Vec::new() }
+    }
+
+    /// Gets a specific element from the coarena, if it exists.
+    pub fn get(&self, index: Index) -> Option<&T> {
+        let (i, g) = index.into_raw_parts();
+        self.data
+            .get(i)
+            .and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
+    }
+
+    /// Gets a mutable reference to a specific element from the coarena, if it exists.
+    pub fn get_mut(&mut self, index: Index) -> Option<&mut T> {
+        let (i, g) = index.into_raw_parts();
+        self.data
+            .get_mut(i)
+            .and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
+    }
+
+    /// Ensure that elements at the two given indices exist in this coarena, and return their reference.
+    ///
+    /// Missing elements are created automatically and initialized with the `default` value.
+    pub fn ensure_pair_exists(&mut self, a: Index, b: Index, default: T) -> (&mut T, &mut T)
+    where
+        T: Clone,
+    {
+        let (i1, g1) = a.into_raw_parts();
+        let (i2, g2) = b.into_raw_parts();
+
+        assert_ne!(i1, i2, "Cannot index the same object twice.");
+
+        let (elt1, elt2) = if i1 > i2 {
+            if self.data.len() <= i1 {
+                self.data.resize(i1 + 1, (u32::MAX as u64, default.clone()));
+            }
+
+            let (left, right) = self.data.split_at_mut(i1);
+            (&mut right[0], &mut left[i2])
+        } else {
+            // i2 > i1
+            if self.data.len() <= i2 {
+                self.data.resize(i2 + 1, (u32::MAX as u64, default.clone()));
+            }
+
+            let (left, right) = self.data.split_at_mut(i2);
+            (&mut left[i1], &mut right[0])
+        };
+
+        if elt1.0 != g1 {
+            *elt1 = (g1, default.clone());
+        }
+
+        if elt2.0 != g2 {
+            *elt2 = (g2, default);
+        }
+
+        (&mut elt1.1, &mut elt2.1)
+    }
+}
diff --git a/src/data/mod.rs b/src/data/mod.rs
index 0e0459e..672bf94 100644
--- a/src/data/mod.rs
+++ b/src/data/mod.rs
@@ -1,8 +1,10 @@
 //! Data structures modified with guaranteed deterministic behavior after deserialization.
 
+pub use self::coarena::Coarena;
 pub use self::maybe_serializable_data::MaybeSerializableData;
 
 pub mod arena;
+mod coarena;
 pub(crate) mod graph;
 pub(crate) mod hashmap;
 mod maybe_serializable_data;
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index ec4d388..48d558f 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -3,7 +3,7 @@ use rayon::prelude::*;
 
 use crate::data::arena::Arena;
 use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
-use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
+use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph, NarrowPhase};
 use crossbeam::channel::{Receiver, Sender};
 use std::ops::{Deref, DerefMut, Index, IndexMut};
 
@@ -452,7 +452,7 @@ impl RigidBodySet {
     pub(crate) fn update_active_set_with_contacts(
         &mut self,
         colliders: &ColliderSet,
-        contact_graph: &InteractionGraph<ContactPair>,
+        narrow_phase: &NarrowPhase,
         joint_graph: &InteractionGraph<Joint>,
         min_island_size: usize,
     ) {
@@ -491,20 +491,22 @@ impl RigidBodySet {
         fn push_contacting_colliders(
             rb: &RigidBody,
             colliders: &ColliderSet,
-            contact_graph: &InteractionGraph<ContactPair>,
+            narrow_phase: &NarrowPhase,
             stack: &mut Vec<ColliderHandle>,
         ) {
             for collider_handle in &rb.colliders {
-                let collider = &colliders[*collider_handle];
-
-                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
-                    for manifold in &inter.2.manifolds {
-                        if manifold.num_active_contacts() > 0 {
-                            let other =
-                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
-                            let other_body = colliders[other].parent;
-                            stack.push(other_body);
-                            break;
+                if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
+                    for inter in contacts {
+                        for manifold in &inter.2.manifolds {
+                            if manifold.num_active_contacts() > 0 {
+                                let other = crate::utils::other_handle(
+                                    (inter.0, inter.1),
+                                    *collider_handle,
+                                );
+                                let other_body = colliders[other].parent;
+                                stack.push(other_body);
+                                break;
+                            }
                         }
                     }
                 }
@@ -522,7 +524,7 @@ impl RigidBodySet {
                 continue;
             }
 
-            push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
+            push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack);
         }
 
         //        println!("Selection: {}", instant::now() - t);
@@ -565,7 +567,7 @@ impl RigidBodySet {
 
             // Transmit the active state to all the rigid-bodies with colliders
             // in contact or joined with this collider.
-            push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
+            push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack);
 
             for inter in joint_graph.interactions_with(rb.joint_graph_index) {
                 let other = crate::utils::other_handle((inter.0, inter.1), handle);
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index c2adc59..3789cca 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -11,6 +11,7 @@ use ncollide::bounding_volume::AABB;
 use std::ops::Deref;
 use std::sync::Arc;
 
+// TODO: move this to its own file.
 /// The shape of a collider.
 #[derive(Clone)]
 pub struct ColliderShape(pub Arc<dyn Shape>);
@@ -206,8 +207,6 @@ pub struct Collider {
     pub restitution: f32,
     pub(crate) collision_groups: InteractionGroups,
     pub(crate) solver_groups: InteractionGroups,
-    pub(crate) contact_graph_index: ColliderGraphIndex,
-    pub(crate) proximity_graph_index: ColliderGraphIndex,
     pub(crate) proxy_index: usize,
     /// User-defined data associated to this rigid-body.
     pub user_data: u128,
@@ -216,8 +215,6 @@ pub struct Collider {
 impl Collider {
     pub(crate) fn reset_internal_references(&mut self) {
         self.parent = RigidBodySet::invalid_handle();
-        self.contact_graph_index = InteractionGraph::<Contact>::invalid_graph_index();
-        self.proximity_graph_index = InteractionGraph::<Proximity>::invalid_graph_index();
         self.proxy_index = crate::INVALID_USIZE;
     }
 
@@ -533,8 +530,6 @@ impl ColliderBuilder {
             parent: RigidBodySet::invalid_handle(),
             position: Isometry::identity(),
             predicted_position: Isometry::identity(),
-            contact_graph_index: InteractionGraph::<Contact>::invalid_graph_index(),
-            proximity_graph_index: InteractionGraph::<Proximity>::invalid_graph_index(),
             proxy_index: crate::INVALID_USIZE,
             collision_groups: self.collision_groups,
             solver_groups: self.solver_groups,
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index 60b9225..5411ec1 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -11,8 +11,6 @@ pub type ColliderHandle = crate::data::arena::Index;
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub(crate) struct RemovedCollider {
     pub handle: ColliderHandle,
-    pub(crate) contact_graph_index: ColliderGraphIndex,
-    pub(crate) proximity_graph_index: ColliderGraphIndex,
     pub(crate) proxy_index: usize,
 }
 
@@ -105,8 +103,6 @@ impl ColliderSet {
          */
         let message = RemovedCollider {
             handle,
-            contact_graph_index: collider.contact_graph_index,
-            proximity_graph_index: collider.proximity_graph_index,
             proxy_index: collider.proxy_index,
         };
 
diff --git a/src/geometry/interaction_graph.rs b/src/geometry/interaction_graph.rs
index 2abb6d1..cae8095 100644
--- a/src/geometry/interaction_graph.rs
+++ b/src/geometry/interaction_graph.rs
@@ -74,15 +74,9 @@ impl<T> InteractionGraph<T> {
         self.graph.node_weight(id).cloned()
     }
 
-    /// All the interactions pairs on this graph.
-    pub fn interaction_pairs(&self) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
-        self.graph.raw_edges().iter().map(move |edge| {
-            (
-                self.graph[edge.source()],
-                self.graph[edge.target()],
-                &edge.weight,
-            )
-        })
+    /// All the interactions on this graph.
+    pub fn interactions(&self) -> impl Iterator<Item = &T> {
+        self.graph.raw_edges().iter().map(move |edge| &edge.weight)
     }
 
     /// The interaction between the two collision objects identified by their graph index.
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 059a640..a21d3c6 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -22,17 +22,35 @@ use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGrap
 //#[cfg(feature = "simd-is-enabled")]
 //use crate::math::{SimdFloat, SIMD_WIDTH};
 use crate::data::pubsub::Subscription;
+use crate::data::Coarena;
 use crate::ncollide::query::Proximity;
 use crate::pipeline::EventHandler;
 use std::collections::HashMap;
 //use simba::simd::SimdValue;
 
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+struct ColliderGraphIndices {
+    contact_graph_index: ColliderGraphIndex,
+    proximity_graph_index: ColliderGraphIndex,
+}
+
+impl ColliderGraphIndices {
+    fn invalid() -> Self {
+        Self {
+            contact_graph_index: InteractionGraph::<ContactPair>::invalid_graph_index(),
+            proximity_graph_index: InteractionGraph::<ProximityPair>::invalid_graph_index(),
+        }
+    }
+}
+
 /// The narrow-phase responsible for computing precise contact information between colliders.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 pub struct NarrowPhase {
     contact_graph: InteractionGraph<ContactPair>,
     proximity_graph: InteractionGraph<ProximityPair>,
+    graph_indices: Coarena<ColliderGraphIndices>,
     removed_colliders: Option<Subscription<RemovedCollider>>,
     //    ball_ball: Vec<usize>,        // Workspace: Vec<*mut ContactPair>,
     //    shape_shape: Vec<usize>,      // Workspace: Vec<*mut ContactPair>,
@@ -48,6 +66,7 @@ impl NarrowPhase {
         Self {
             contact_graph: InteractionGraph::new(),
             proximity_graph: InteractionGraph::new(),
+            graph_indices: Coarena::new(),
             removed_colliders: None,
             //            ball_ball: Vec::new(),
             //            shape_shape: Vec::new(),
@@ -66,14 +85,70 @@ impl NarrowPhase {
         &self.proximity_graph
     }
 
-    // #[cfg(feature = "parallel")]
-    // pub fn contact_pairs(&self) -> &[ContactPair] {
-    //     &self.contact_graph.interactions
-    // }
+    /// All the contacts involving the given collider.
+    pub fn contacts_with(
+        &self,
+        collider: ColliderHandle,
+    ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>> {
+        let id = self.graph_indices.get(collider)?;
+        Some(self.contact_graph.interactions_with(id.contact_graph_index))
+    }
 
-    // pub fn contact_pairs_mut(&mut self) -> &mut [ContactPair] {
-    //     &mut self.contact_graph.interactions
-    // }
+    /// All the proximities involving the given collider.
+    pub fn proximities_with(
+        &self,
+        collider: ColliderHandle,
+    ) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ProximityPair)>> {
+        let id = self.graph_indices.get(collider)?;
+        Some(
+            self.proximity_graph
+                .interactions_with(id.proximity_graph_index),
+        )
+    }
+
+    /// The contact pair involving two specific colliders.
+    ///
+    /// If this returns `None`, there is no contact between the two colliders.
+    /// If this returns `Some`, then there may be a contact between the two colliders. Check the
+    /// result [`ContactPair::has_any_active_collider`] method to see if there is an actual contact.
+    pub fn contact_pair(
+        &self,
+        collider1: ColliderHandle,
+        collider2: ColliderHandle,
+    ) -> Option<&ContactPair> {
+        let id1 = self.graph_indices.get(collider1)?;
+        let id2 = self.graph_indices.get(collider2)?;
+        self.contact_graph
+            .interaction_pair(id1.contact_graph_index, id2.contact_graph_index)
+            .map(|c| c.2)
+    }
+
+    /// The proximity pair involving two specific colliders.
+    ///
+    /// If this returns `None`, there is no intersection between the two colliders.
+    /// If this returns `Some`, then there may be an intersection between the two colliders. Check the
+    /// value of [`ProximityPair::proximity`] method to see if there is an actual intersection.
+    pub fn proximity_pair(
+        &self,
+        collider1: ColliderHandle,
+        collider2: ColliderHandle,
+    ) -> Option<&ProximityPair> {
+        let id1 = self.graph_indices.get(collider1)?;
+        let id2 = self.graph_indices.get(collider2)?;
+        self.proximity_graph
+            .interaction_pair(id1.proximity_graph_index, id2.proximity_graph_index)
+            .map(|c| c.2)
+    }
+
+    /// All the contact pairs maintained by this narrow-phase.
+    pub fn contact_pairs(&self) -> impl Iterator<Item = &ContactPair> {
+        self.contact_graph.interactions()
+    }
+
+    /// All the proximity pairs maintained by this narrow-phase.
+    pub fn proximity_pairs(&self) -> impl Iterator<Item = &ProximityPair> {
+        self.proximity_graph.interactions()
+    }
 
     // #[cfg(feature = "parallel")]
     // pub(crate) fn contact_pairs_vec_mut(&mut self) -> &mut Vec<ContactPair> {
@@ -94,17 +169,20 @@ impl NarrowPhase {
         // by the contact/proximity graphs when a node is removed.
         let mut prox_id_remap = HashMap::new();
         let mut contact_id_remap = HashMap::new();
+        let mut i = 0;
 
-        for i in 0.. {
-            if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
+        while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
+            // NOTE: if the collider does not have any graph indices currently, there is nothing
+            // to remove in the narrow-phase for this collider.
+            if let Some(graph_idx) = self.graph_indices.get(collider.handle) {
                 let proximity_graph_id = prox_id_remap
                     .get(&collider.handle)
                     .copied()
-                    .unwrap_or(collider.proximity_graph_index);
+                    .unwrap_or(graph_idx.proximity_graph_index);
                 let contact_graph_id = contact_id_remap
                     .get(&collider.handle)
                     .copied()
-                    .unwrap_or(collider.contact_graph_index);
+                    .unwrap_or(graph_idx.contact_graph_index);
 
                 self.remove_collider(
                     proximity_graph_id,
@@ -114,9 +192,9 @@ impl NarrowPhase {
                     &mut prox_id_remap,
                     &mut contact_id_remap,
                 );
-            } else {
-                break;
             }
+
+            i += 1;
         }
 
         colliders.removed_colliders.ack(&mut cursor);
@@ -146,7 +224,7 @@ impl NarrowPhase {
         // We have to manage the fact that one other collider will
         // have its graph index changed because of the node's swap-remove.
         if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) {
-            if let Some(replacement) = colliders.get_mut(replacement) {
+            if let Some(replacement) = self.graph_indices.get_mut(replacement) {
                 replacement.proximity_graph_index = proximity_graph_id;
             } else {
                 prox_id_remap.insert(replacement, proximity_graph_id);
@@ -154,7 +232,7 @@ impl NarrowPhase {
         }
 
         if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
-            if let Some(replacement) = colliders.get_mut(replacement) {
+            if let Some(replacement) = self.graph_indices.get_mut(replacement) {
                 replacement.contact_graph_index = contact_graph_id;
             } else {
                 contact_id_remap.insert(replacement, contact_graph_id);
@@ -172,69 +250,87 @@ impl NarrowPhase {
         for event in broad_phase_events {
             match event {
                 BroadPhasePairEvent::AddPair(pair) => {
-                    // println!("Adding pair: {:?}", *pair);
                     if let (Some(co1), Some(co2)) =
-                        colliders.get2_mut_internal(pair.collider1, pair.collider2)
+                        (colliders.get(pair.collider1), colliders.get(pair.collider2))
                     {
                         if co1.parent == co2.parent {
                             // Same parents. Ignore collisions.
                             continue;
                         }
 
-                        if co1.is_sensor() || co2.is_sensor() {
-                            let gid1 = co1.proximity_graph_index;
-                            let gid2 = co2.proximity_graph_index;
+                        let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
+                            pair.collider1,
+                            pair.collider2,
+                            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::<ProximityPair>::is_graph_index_valid(gid1) {
-                                co1.proximity_graph_index =
+                            if !InteractionGraph::<ProximityPair>::is_graph_index_valid(
+                                gid1.proximity_graph_index,
+                            ) {
+                                gid1.proximity_graph_index =
                                     self.proximity_graph.graph.add_node(pair.collider1);
                             }
 
-                            if !InteractionGraph::<ProximityPair>::is_graph_index_valid(gid2) {
-                                co2.proximity_graph_index =
+                            if !InteractionGraph::<ProximityPair>::is_graph_index_valid(
+                                gid2.proximity_graph_index,
+                            ) {
+                                gid2.proximity_graph_index =
                                     self.proximity_graph.graph.add_node(pair.collider2);
                             }
 
-                            if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() {
+                            if self
+                                .proximity_graph
+                                .graph
+                                .find_edge(gid1.proximity_graph_index, gid2.proximity_graph_index)
+                                .is_none()
+                            {
                                 let dispatcher = DefaultProximityDispatcher;
                                 let generator = dispatcher
                                     .dispatch(co1.shape().shape_type(), co2.shape().shape_type());
                                 let interaction =
                                     ProximityPair::new(*pair, generator.0, generator.1);
                                 let _ = self.proximity_graph.add_edge(
-                                    co1.proximity_graph_index,
-                                    co2.proximity_graph_index,
+                                    gid1.proximity_graph_index,
+                                    gid2.proximity_graph_index,
                                     interaction,
                                 );
                             }
                         } else {
                             // NOTE: same code as above, but for the contact graph.
                             // TODO: refactor both pieces of code somehow?
-                            let gid1 = co1.contact_graph_index;
-                            let gid2 = co2.contact_graph_index;
 
                             // NOTE: the collider won't have a graph index as long
                             // as it does not interact with anything.
-                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(gid1) {
-                                co1.contact_graph_index =
+                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(
+                                gid1.contact_graph_index,
+                            ) {
+                                gid1.contact_graph_index =
                                     self.contact_graph.graph.add_node(pair.collider1);
                             }
 
-                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(gid2) {
-                                co2.contact_graph_index =
+                            if !InteractionGraph::<ContactPair>::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, gid2).is_none() {
+                            if self
+                                .contact_graph
+                                .graph
+                                .find_edge(gid1.contact_graph_index, gid2.contact_graph_index)
+                                .is_none()
+                            {
                                 let dispatcher = DefaultContactDispatcher;
                                 let generator = dispatcher
                                     .dispatch(co1.shape().shape_type(), co2.shape().shape_type());
                                 let interaction = ContactPair::new(*pair, generator.0, generator.1);
                                 let _ = self.contact_graph.add_edge(
-                                    co1.contact_graph_index,
-                                    co2.contact_graph_index,
+                                    gid1.contact_graph_index,
+                                    gid2.contact_graph_index,
                                     interaction,
                                 );
                             }
@@ -243,41 +339,50 @@ impl NarrowPhase {
                 }
                 BroadPhasePairEvent::DeletePair(pair) => {
                     if let (Some(co1), Some(co2)) =
-                        colliders.get2_mut_internal(pair.collider1, pair.collider2)
+                        (colliders.get(pair.collider1), colliders.get(pair.collider2))
                     {
-                        if co1.is_sensor() || co2.is_sensor() {
-                            let prox_pair = self
-                                .proximity_graph
-                                .remove_edge(co1.proximity_graph_index, co2.proximity_graph_index);
-
-                            // Emit a proximity lost event if we had a proximity before removing the edge.
-                            if let Some(prox) = prox_pair {
-                                if prox.proximity != Proximity::Disjoint {
-                                    let prox_event = ProximityEvent::new(
-                                        pair.collider1,
-                                        pair.collider2,
-                                        prox.proximity,
-                                        Proximity::Disjoint,
-                                    );
-                                    events.handle_proximity_event(prox_event)
+                        // 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),
+                            self.graph_indices.get(pair.collider2),
+                        ) {
+                            if co1.is_sensor() || co2.is_sensor() {
+                                let prox_pair = self.proximity_graph.remove_edge(
+                                    gid1.proximity_graph_index,
+                                    gid2.proximity_graph_index,
+                                );
+
+                                // Emit a proximity lost event if we had a proximity before removing the edge.
+                                if let Some(prox) = prox_pair {
+                                    if prox.proximity != Proximity::Disjoint {
+                                        let prox_event = ProximityEvent::new(
+                                            pair.collider1,
+                                            pair.collider2,
+                                            prox.proximity,
+                                            Proximity::Disjoint,
+                                        );
+                                        events.handle_proximity_event(prox_event)
+                                    }
                                 }
-                            }
-                        } else {
-                            let contact_pair = self
-                                .contact_graph
-                                .remove_edge(co1.contact_graph_index, co2.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,
-                                    ))
+                            } 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,
+                                        ))
+                                    }
                                 }
                             }
                         }
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index b8896e8..845bb86 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -74,7 +74,7 @@ impl CollisionPipeline {
 
         bodies.update_active_set_with_contacts(
             colliders,
-            narrow_phase.contact_graph(),
+            narrow_phase,
             self.empty_joints.joint_graph(),
             0,
         );
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 0720ff1..3a4851d 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -130,7 +130,7 @@ impl PhysicsPipeline {
         self.counters.stages.island_construction_time.start();
         bodies.update_active_set_with_contacts(
             colliders,
-            narrow_phase.contact_graph(),
+            narrow_phase,
             joints.joint_graph(),
             integration_parameters.min_island_size,
         );