Merge pull request #353 from dimforge/force-events

Add force reporting

2 files changed, 135 insertions, 11 deletions

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.