Merge pull request #353 from dimforge/force-events

Add force reporting

4 files changed, 121 insertions, 33 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 8d9b005..2f2e4ac 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -26,6 +26,7 @@ pub struct Collider {
     pub(crate) material: ColliderMaterial,
     pub(crate) flags: ColliderFlags,
     pub(crate) bf_data: ColliderBroadPhaseData,
+    pub(crate) contact_force_event_threshold: Real,
     /// User-defined data associated to this collider.
     pub user_data: u128,
 }
@@ -124,6 +125,11 @@ impl Collider {
         self.material.restitution_combine_rule = rule;
     }
 
+    /// Sets the total force magnitude beyond which a contact force event can be emitted.
+    pub fn set_contact_force_event_threshold(&mut self, threshold: Real) {
+        self.contact_force_event_threshold = threshold;
+    }
+
     /// Sets whether or not this is a sensor collider.
     pub fn set_sensor(&mut self, is_sensor: bool) {
         if is_sensor != self.is_sensor() {
@@ -283,6 +289,11 @@ impl Collider {
             ColliderMassProps::MassProperties(mass_properties) => **mass_properties,
         }
     }
+
+    /// The total force magnitude beyond which a contact force event can be emitted.
+    pub fn contact_force_event_threshold(&self) -> Real {
+        self.contact_force_event_threshold
+    }
 }
 
 /// A structure responsible for building a new collider.
@@ -321,6 +332,8 @@ pub struct ColliderBuilder {
     pub collision_groups: InteractionGroups,
     /// The solver groups for the collider being built.
     pub solver_groups: InteractionGroups,
+    /// The total force magnitude beyond which a contact force event can be emitted.
+    pub contact_force_event_threshold: Real,
 }
 
 impl ColliderBuilder {
@@ -342,6 +355,7 @@ impl ColliderBuilder {
             active_collision_types: ActiveCollisionTypes::default(),
             active_hooks: ActiveHooks::empty(),
             active_events: ActiveEvents::empty(),
+            contact_force_event_threshold: Real::MAX,
         }
     }
 
@@ -681,6 +695,12 @@ impl ColliderBuilder {
         self
     }
 
+    /// Sets the total force magnitude beyond which a contact force event can be emitted.
+    pub fn contact_force_event_threshold(mut self, threshold: Real) -> Self {
+        self.contact_force_event_threshold = threshold;
+        self
+    }
+
     /// Sets the initial translation of the collider to be created.
     ///
     /// If the collider will be attached to a rigid-body, this sets the translation relative to the
@@ -725,34 +745,6 @@ impl ColliderBuilder {
 
     /// Builds a new collider attached to the given rigid-body.
     pub fn build(&self) -> Collider {
-        let (changes, pos, bf_data, shape, coll_type, material, flags, mprops) = self.components();
-        Collider {
-            shape,
-            mprops,
-            material,
-            parent: None,
-            changes,
-            pos,
-            bf_data,
-            flags,
-            coll_type,
-            user_data: self.user_data,
-        }
-    }
-
-    /// Builds all the components required by a collider.
-    pub fn components(
-        &self,
-    ) -> (
-        ColliderChanges,
-        ColliderPosition,
-        ColliderBroadPhaseData,
-        ColliderShape,
-        ColliderType,
-        ColliderMaterial,
-        ColliderFlags,
-        ColliderMassProps,
-    ) {
         let mass_info = if let Some(mp) = self.mass_properties {
             ColliderMassProps::MassProperties(Box::new(mp))
         } else {
@@ -785,9 +777,19 @@ impl ColliderBuilder {
             ColliderType::Solid
         };
 
-        (
-            changes, pos, bf_data, shape, coll_type, material, flags, mprops,
-        )
+        Collider {
+            shape,
+            mprops,
+            material,
+            parent: None,
+            changes,
+            pos,
+            bf_data,
+            flags,
+            coll_type,
+            contact_force_event_threshold: self.contact_force_event_threshold,
+            user_data: self.user_data,
+        }
     }
 }
 
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 551ffde..3bef40d 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -141,6 +141,36 @@ impl ContactPair {
         self.workspace = None;
     }
 
+    /// The sum of all the impulses applied by contacts on this contact pair.
+    pub fn total_impulse(&self) -> Vector<Real> {
+        self.manifolds
+            .iter()
+            .map(|m| m.total_impulse() * m.data.normal)
+            .sum()
+    }
+
+    /// The sum of the magnitudes of the contacts on this contact pair.
+    pub fn total_impulse_magnitude(&self) -> Real {
+        self.manifolds
+            .iter()
+            .fold(0.0, |a, m| a + m.total_impulse())
+    }
+
+    /// The magnitude and (unit) direction of the maximum impulse on this contact pair.
+    pub fn max_impulse(&self) -> (Real, Vector<Real>) {
+        let mut result = (0.0, Vector::zeros());
+
+        for m in &self.manifolds {
+            let impulse = m.total_impulse();
+
+            if impulse > result.0 {
+                result = (impulse, m.data.normal);
+            }
+        }
+
+        result
+    }
+
     /// Finds the contact with the smallest signed distance.
     ///
     /// If the colliders involved in this contact pair are penetrating, then
@@ -316,3 +346,21 @@ impl ContactManifoldData {
         self.solver_contacts.len()
     }
 }
+
+/// Additional methods for the contact manifold.
+pub trait ContactManifoldExt {
+    /// Computes the sum of all the impulses applied by contacts from this contact manifold.
+    fn total_impulse(&self) -> Real;
+    /// Computes the maximum impulse applied by contacts from this contact manifold.
+    fn max_impulse(&self) -> Real;
+}
+
+impl ContactManifoldExt for ContactManifold {
+    fn total_impulse(&self) -> Real {
+        self.points.iter().map(|pt| pt.data.impulse).sum()
+    }
+
+    fn max_impulse(&self) -> Real {
+        self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse))
+    }
+}
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 34d3707..48d71f8 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -16,6 +16,8 @@ pub use self::collider_set::ColliderSet;
 
 pub use parry::query::TrackedContact;
 
+use crate::math::{Real, Vector};
+
 /// A contact between two colliders.
 pub type Contact = parry::query::TrackedContact<ContactData>;
 /// A contact manifold between two colliders.
@@ -116,6 +118,28 @@ impl CollisionEvent {
     }
 }
 
+#[derive(Copy, Clone, PartialEq, Debug, Default)]
+/// Event occurring when the sum of the magnitudes of the contact forces
+/// between two colliders exceed a threshold.
+pub struct CollisionForceEvent {
+    /// The first collider involved in the contact.
+    pub collider1: ColliderHandle,
+    /// The second collider involved in the contact.
+    pub collider2: ColliderHandle,
+    /// The sum of all the forces between the two colliders.
+    pub total_force: Vector<Real>,
+    /// The sum of the magnitudes of each force between the two colliders.
+    ///
+    /// Note that this is **not** the same as the magnitude of `self.total_force`.
+    /// Here we are summing the magnitude of all the forces, instead of taking
+    /// the magnitude of their sum.
+    pub total_force_magnitude: Real,
+    /// The world-space (unit) direction of the force with strongest magnitude.
+    pub max_force_direction: Vector<Real>,
+    /// The magnitude of the largest force at a contact point of this contact pair.
+    pub max_force_magnitude: Real,
+}
+
 pub(crate) use self::broad_phase_multi_sap::SAPProxyIndex;
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 pub(crate) use parry::partitioning::QBVH;
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));
+            }
         }
     }
 }