Merge pull request #353 from dimforge/force-events

Add force reporting

6 files changed, 256 insertions, 44 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));
+            }
         }
     }
 }
diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs
index 24b415c..55ede93 100644
--- a/src/pipeline/event_handler.rs
+++ b/src/pipeline/event_handler.rs
@@ -1,5 +1,6 @@
 use crate::dynamics::RigidBodySet;
-use crate::geometry::{ColliderSet, CollisionEvent, ContactPair};
+use crate::geometry::{ColliderSet, CollisionEvent, CollisionForceEvent, ContactPair};
+use crate::math::Real;
 use crossbeam::channel::Sender;
 
 bitflags::bitflags! {
@@ -25,6 +26,8 @@ pub trait EventHandler: Send + Sync {
     /// Handle a collision event.
     ///
     /// A collision event is emitted when the state of intersection between two colliders changes.
+    /// At least one of the involved colliders must have the `ActiveEvents::COLLISION_EVENTS` flag
+    /// set.
     ///
     /// # Parameters
     /// * `event` - The collision event.
@@ -40,6 +43,26 @@ pub trait EventHandler: Send + Sync {
         event: CollisionEvent,
         contact_pair: Option<&ContactPair>,
     );
+
+    /// Handle a force event.
+    ///
+    /// A force event is generated whenever the total force magnitude applied between two
+    /// colliders is `> Collider::contact_force_event_threshold` value of any of these
+    /// colliders.
+    ///
+    /// The "total force magnitude" here means "the sum of the magnitudes of the forces applied at
+    /// all the contact points in a contact pair". Therefore, if the contact pair involves two
+    /// forces `{0.0, 1.0, 0.0}` and `{0.0, -1.0, 0.0}`, then the total force magnitude tested
+    /// against the `contact_force_event_threshold` is `2.0` even if the sum of these forces is actually the
+    /// zero vector.
+    fn handle_contact_force_event(
+        &self,
+        dt: Real,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        contact_pair: &ContactPair,
+        total_force_magnitude: Real,
+    );
 }
 
 impl EventHandler for () {
@@ -51,17 +74,34 @@ impl EventHandler for () {
         _contact_pair: Option<&ContactPair>,
     ) {
     }
+
+    fn handle_contact_force_event(
+        &self,
+        _dt: Real,
+        _bodies: &RigidBodySet,
+        _colliders: &ColliderSet,
+        _contact_pair: &ContactPair,
+        _total_force_magnitude: Real,
+    ) {
+    }
 }
 
 /// A collision event handler that collects events into a crossbeam channel.
 pub struct ChannelEventCollector {
-    event_sender: Sender<CollisionEvent>,
+    collision_event_sender: Sender<CollisionEvent>,
+    contact_force_event_sender: Sender<CollisionForceEvent>,
 }
 
 impl ChannelEventCollector {
     /// Initialize a new collision event handler from crossbeam channel senders.
-    pub fn new(event_sender: Sender<CollisionEvent>) -> Self {
-        Self { event_sender }
+    pub fn new(
+        collision_event_sender: Sender<CollisionEvent>,
+        contact_force_event_sender: Sender<CollisionForceEvent>,
+    ) -> Self {
+        Self {
+            collision_event_sender,
+            contact_force_event_sender,
+        }
     }
 }
 
@@ -73,6 +113,47 @@ impl EventHandler for ChannelEventCollector {
         event: CollisionEvent,
         _: Option<&ContactPair>,
     ) {
-        let _ = self.event_sender.send(event);
+        let _ = self.collision_event_sender.send(event);
+    }
+
+    fn handle_contact_force_event(
+        &self,
+        dt: Real,
+        _bodies: &RigidBodySet,
+        _colliders: &ColliderSet,
+        contact_pair: &ContactPair,
+        total_force_magnitude: Real,
+    ) {
+        let mut result = CollisionForceEvent {
+            collider1: contact_pair.collider1,
+            collider2: contact_pair.collider2,
+            total_force_magnitude,
+            ..CollisionForceEvent::default()
+        };
+
+        for m in &contact_pair.manifolds {
+            let mut total_manifold_impulse = 0.0;
+            for pt in m.contacts() {
+                total_manifold_impulse += pt.data.impulse;
+
+                if pt.data.impulse > result.max_force_magnitude {
+                    result.max_force_magnitude = pt.data.impulse;
+                    result.max_force_direction = m.data.normal;
+                }
+            }
+
+            result.total_force += m.data.normal * total_manifold_impulse;
+        }
+
+        let inv_dt = crate::utils::inv(dt);
+        // NOTE: convert impulses to forces. Note that we
+        //       don’t need to convert the `total_force_magnitude`
+        //       because it’s an input of this function already
+        //       assumed to be a force instead of an impulse.
+        result.total_force *= inv_dt;
+        result.max_force_direction *= inv_dt;
+        result.max_force_magnitude *= inv_dt;
+
+        let _ = self.contact_force_event_sender.send(result);
     }
 }
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 19749be..d1c3b65 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -11,7 +11,7 @@ use crate::dynamics::{
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
     BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair,
-    ContactManifoldIndex, NarrowPhase,
+    ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{EventHandler, PhysicsHooks};
@@ -31,6 +31,7 @@ use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
 pub struct PhysicsPipeline {
     /// Counters used for benchmarking only.
     pub counters: Counters,
+    contact_pair_indices: Vec<TemporaryInteractionIndex>,
     manifold_indices: Vec<Vec<ContactManifoldIndex>>,
     joint_constraint_indices: Vec<Vec<ContactManifoldIndex>>,
     broadphase_collider_pairs: Vec<ColliderPair>,
@@ -55,11 +56,12 @@ impl PhysicsPipeline {
     pub fn new() -> PhysicsPipeline {
         PhysicsPipeline {
             counters: Counters::new(true),
-            solvers: Vec::new(),
-            manifold_indices: Vec::new(),
-            joint_constraint_indices: Vec::new(),
-            broadphase_collider_pairs: Vec::new(),
-            broad_phase_events: Vec::new(),
+            solvers: vec![],
+            contact_pair_indices: vec![],
+            manifold_indices: vec![],
+            joint_constraint_indices: vec![],
+            broadphase_collider_pairs: vec![],
+            broad_phase_events: vec![],
         }
     }
 
@@ -148,6 +150,7 @@ impl PhysicsPipeline {
         colliders: &mut ColliderSet,
         impulse_joints: &mut ImpulseJointSet,
         multibody_joints: &mut MultibodyJointSet,
+        events: &dyn EventHandler,
     ) {
         self.counters.stages.island_construction_time.resume();
         islands.update_active_set_with_contacts(
@@ -175,6 +178,7 @@ impl PhysicsPipeline {
         narrow_phase.select_active_contacts(
             islands,
             bodies,
+            &mut self.contact_pair_indices,
             &mut manifolds,
             &mut self.manifold_indices,
         );
@@ -275,6 +279,34 @@ impl PhysicsPipeline {
                     });
             });
         }
+
+        // Generate contact force events if needed.
+        let inv_dt = crate::utils::inv(integration_parameters.dt);
+        for pair_id in self.contact_pair_indices.drain(..) {
+            let pair = narrow_phase.contact_pair_at_index(pair_id);
+            let co1 = &colliders[pair.collider1];
+            let co2 = &colliders[pair.collider2];
+            let threshold = co1
+                .contact_force_event_threshold
+                .min(co2.contact_force_event_threshold);
+
+            if threshold < Real::MAX {
+                let total_magnitude = pair.total_impulse_magnitude() * inv_dt;
+
+                // NOTE: the strict inequality is important here, so we don’t
+                //       trigger an event if the force is 0.0 and the threshold is 0.0.
+                if total_magnitude > threshold {
+                    events.handle_contact_force_event(
+                        integration_parameters.dt,
+                        bodies,
+                        colliders,
+                        pair,
+                        total_magnitude,
+                    );
+                }
+            }
+        }
+
         self.counters.stages.solver_time.pause();
     }
 
@@ -371,6 +403,16 @@ impl PhysicsPipeline {
         hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
+        // Apply some of delayed wake-ups.
+        for handle in impulse_joints
+            .to_wake_up
+            .drain(..)
+            .chain(multibody_joints.to_wake_up.drain(..))
+        {
+            islands.wake_up(bodies, handle, true);
+        }
+
+        // Apply modifications.
         let modified_bodies = bodies.take_modified();
         let mut modified_colliders = colliders.take_modified();
         let mut removed_colliders = colliders.take_removed();
@@ -497,6 +539,7 @@ impl PhysicsPipeline {
                 colliders,
                 impulse_joints,
                 multibody_joints,
+                events,
             );
 
             // If CCD is enabled, execute the CCD motion clamping.