2 files changed, 47 insertions, 1 deletions

diff --git a/src/data/coarena.rs b/src/data/coarena.rs
index ac6e43f..549c5f2 100644
--- a/src/data/coarena.rs
+++ b/src/data/coarena.rs
@@ -21,6 +21,20 @@ impl<T> Coarena<T> {
         self.data.get(index as usize).map(|(_, t)| t)
     }
 
+    /// Deletes an element for the coarena and returns its value.
+    ///
+    /// We can't really remove an element from the coarena. So instead of actually removing
+    /// it, this method will reset the value to the given `removed_value`.
+    pub fn remove(&mut self, index: Index, removed_value: T) -> Option<T> {
+        let (i, g) = index.into_raw_parts();
+        let data = self.data.get_mut(i as usize)?;
+        if g == data.0 {
+            Some(std::mem::replace(&mut data.1, removed_value))
+        } else {
+            None
+        }
+    }
+
     /// 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();
@@ -51,6 +65,27 @@ impl<T> Coarena<T> {
         self.data[i1 as usize] = (g1, value);
     }
 
+    /// Ensure that the given element exists in thihs coarena, and return its mutable reference.
+    pub fn ensure_element_exist(&mut self, a: Index, default: T) -> &mut T
+    where
+        T: Clone,
+    {
+        let (i1, g1) = a.into_raw_parts();
+
+        if self.data.len() <= i1 as usize {
+            self.data
+                .resize(i1 as usize + 1, (u32::MAX, default.clone()));
+        }
+
+        let data = &mut self.data[i1 as usize];
+
+        if data.0 != g1 {
+            *data = (g1, default);
+        }
+
+        &mut data.1
+    }
+
     /// 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.
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 185cf0c..bbabc95 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -270,7 +270,10 @@ impl NarrowPhase {
         for collider in removed_colliders {
             // 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.0) {
+            if let Some(graph_idx) = self
+                .graph_indices
+                .remove(collider.0, ColliderGraphIndices::invalid())
+            {
                 let intersection_graph_id = prox_id_remap
                     .get(collider)
                     .copied()
@@ -330,6 +333,10 @@ impl NarrowPhase {
                 replacement.intersection_graph_index = intersection_graph_id;
             } else {
                 prox_id_remap.insert(replacement, intersection_graph_id);
+                // I feel like this should never happen now that the narrow-phase is the one owning
+                // the graph_indices. Let's put an unreachable in there and see if anybody still manages
+                // to reach it. If nobody does, we will remove this.
+                unreachable!();
             }
         }
 
@@ -338,6 +345,10 @@ impl NarrowPhase {
                 replacement.contact_graph_index = contact_graph_id;
             } else {
                 contact_id_remap.insert(replacement, contact_graph_id);
+                // I feel like this should never happen now that the narrow-phase is the one owning
+                // the graph_indices. Let's put an unreachable in there and see if anybody still manages
+                // to reach it. If nobody does, we will remove this.
+                unreachable!();
             }
         }
     }