Merge pull request #353 from dimforge/force-events

Add force reporting

1 files changed, 16 insertions, 2 deletions

diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index e7b9a34..a1256b8 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -1,6 +1,7 @@
 #[cfg(feature = "parallel")]
 use rayon::prelude::*;
 
+use crate::data::graph::EdgeIndex;
 use crate::data::Coarena;
 use crate::dynamics::{
     CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType,
@@ -8,7 +9,7 @@ use crate::dynamics::{
 use crate::geometry::{
     BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair,
     ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair,
-    InteractionGraph, IntersectionPair, SolverContact, SolverFlags,
+    InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{
@@ -164,6 +165,11 @@ impl NarrowPhase {
             })
     }
 
+    /// Returns the contact pair at the given temporary index.
+    pub fn contact_pair_at_index(&self, id: TemporaryInteractionIndex) -> &ContactPair {
+        &self.contact_graph.graph.edges[id.index()].weight
+    }
+
     /// The contact pair involving two specific colliders.
     ///
     /// It is strongly recommended to use the [`NarrowPhase::contact_pair`] method instead. This
@@ -975,6 +981,7 @@ impl NarrowPhase {
         &'a mut self,
         islands: &IslandManager,
         bodies: &RigidBodySet,
+        out_contact_pairs: &mut Vec<TemporaryInteractionIndex>,
         out_manifolds: &mut Vec<&'a mut ContactManifold>,
         out: &mut Vec<Vec<ContactManifoldIndex>>,
     ) {
@@ -983,7 +990,9 @@ impl NarrowPhase {
         }
 
         // TODO: don't iterate through all the interactions.
-        for inter in self.contact_graph.graph.edges.iter_mut() {
+        for (pair_id, inter) in self.contact_graph.graph.edges.iter_mut().enumerate() {
+            let mut push_pair = false;
+
             for manifold in &mut inter.weight.manifolds {
                 if manifold
                     .data
@@ -1027,9 +1036,14 @@ impl NarrowPhase {
 
                         out[island_index].push(out_manifolds.len());
                         out_manifolds.push(manifold);
+                        push_pair = true;
                     }
                 }
             }
+
+            if push_pair {
+                out_contact_pairs.push(EdgeIndex::new(pair_id as u32));
+            }
         }
     }
 }