Add contact force events generated above a user-defined threshold

1 files changed, 86 insertions, 5 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);
     }
 }