Merge pull request #120 from dimforge/contact_modification

Add the ability to modify the contact points seen by the constraints solver

5 files changed, 98 insertions, 97 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index ce263f8..b006f9e 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,5 +1,5 @@
 use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
-use crate::geometry::{InteractionGroups, SharedShape};
+use crate::geometry::{InteractionGroups, SharedShape, SolverFlags};
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
 use parry::bounding_volume::AABB;
@@ -50,6 +50,7 @@ pub struct Collider {
     shape: SharedShape,
     density: Real,
     pub(crate) flags: ColliderFlags,
+    pub(crate) solver_flags: SolverFlags,
     pub(crate) parent: RigidBodyHandle,
     pub(crate) delta: Isometry<Real>,
     pub(crate) position: Isometry<Real>,
@@ -159,6 +160,9 @@ pub struct ColliderBuilder {
     pub delta: Isometry<Real>,
     /// Is this collider a sensor?
     pub is_sensor: bool,
+    /// Do we have to always call the contact modifier
+    /// on this collider?
+    pub modify_solver_contacts: bool,
     /// The user-data of the collider being built.
     pub user_data: u128,
     /// The collision groups for the collider being built.
@@ -182,6 +186,7 @@ impl ColliderBuilder {
             solver_groups: InteractionGroups::all(),
             friction_combine_rule: CoefficientCombineRule::Average,
             restitution_combine_rule: CoefficientCombineRule::Average,
+            modify_solver_contacts: false,
         }
     }
 
@@ -456,6 +461,13 @@ impl ColliderBuilder {
         self
     }
 
+    /// If set to `true` then the physics hooks will always run to modify
+    /// contacts involving this collider.
+    pub fn modify_solver_contacts(mut self, modify_solver_contacts: bool) -> Self {
+        self.modify_solver_contacts = modify_solver_contacts;
+        self
+    }
+
     /// Sets the friction coefficient of the collider this builder will build.
     pub fn friction(mut self, friction: Real) -> Self {
         self.friction = friction;
@@ -534,6 +546,11 @@ impl ColliderBuilder {
         flags = flags
             .with_friction_combine_rule(self.friction_combine_rule)
             .with_restitution_combine_rule(self.restitution_combine_rule);
+        let mut solver_flags = SolverFlags::default();
+        solver_flags.set(
+            SolverFlags::MODIFY_SOLVER_CONTACTS,
+            self.modify_solver_contacts,
+        );
 
         Collider {
             shape: self.shape.clone(),
@@ -542,6 +559,7 @@ impl ColliderBuilder {
             restitution: self.restitution,
             delta: self.delta,
             flags,
+            solver_flags,
             parent: RigidBodyHandle::invalid(),
             position: Isometry::identity(),
             predicted_position: Isometry::identity(),
diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs
index 462d3ef..50094ca 100644
--- a/src/geometry/contact_pair.rs
+++ b/src/geometry/contact_pair.rs
@@ -9,7 +9,16 @@ bitflags::bitflags! {
     pub struct SolverFlags: u32 {
         /// The constraint solver will take this contact manifold into
         /// account for force computation.
-        const COMPUTE_IMPULSES = 0b01;
+        const COMPUTE_IMPULSES = 0b001;
+        /// The user-defined physics hooks will be used to
+        /// modify the solver contacts of this contact manifold.
+        const MODIFY_SOLVER_CONTACTS = 0b010;
+    }
+}
+
+impl Default for SolverFlags {
+    fn default() -> Self {
+        SolverFlags::COMPUTE_IMPULSES
     }
 }
 
@@ -104,12 +113,16 @@ pub struct ContactManifoldData {
     /// The contacts that will be seen by the constraints solver for computing forces.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     pub solver_contacts: Vec<SolverContact>,
+    /// A user-defined piece of data.
+    pub user_data: u32,
 }
 
 /// A contact seen by the constraints solver for computing forces.
 #[derive(Copy, Clone, Debug)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct SolverContact {
+    /// The index of the manifold contact used to generate this solver contact.
+    pub contact_id: u8,
     /// The world-space contact point.
     pub point: Point<Real>,
     /// The distance between the two original contacts points along the contact normal.
@@ -119,10 +132,11 @@ pub struct SolverContact {
     pub friction: Real,
     /// The effective restitution coefficient at this contact point.
     pub restitution: Real,
-    /// The artificially add relative velocity at the contact point.
+    /// The desired tangent relative velocity at the contact point.
+    ///
     /// This is set to zero by default. Set to a non-zero value to
     /// simulate, e.g., conveyor belts.
-    pub surface_velocity: Vector<Real>,
+    pub tangent_velocity: Vector<Real>,
     /// Associated contact data used to warm-start the constraints
     /// solver.
     pub data: ContactData,
@@ -163,6 +177,7 @@ impl ContactManifoldData {
             solver_flags,
             normal: Vector::zeros(),
             solver_contacts: Vec::new(),
+            user_data: 0,
         }
     }
 
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 861763e..ab04b25 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -10,7 +10,6 @@ pub use self::interaction_graph::{
 };
 pub use self::interaction_groups::InteractionGroups;
 pub use self::narrow_phase::NarrowPhase;
-pub use self::pair_filter::{ContactPairFilter, IntersectionPairFilter, PairFilterContext};
 
 pub use parry::query::TrackedContact;
 
@@ -109,4 +108,3 @@ mod contact_pair;
 mod interaction_graph;
 mod interaction_groups;
 mod narrow_phase;
-mod pair_filter;
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 640ce12..d05b19a 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -5,13 +5,14 @@ use crate::data::pubsub::Subscription;
 use crate::data::Coarena;
 use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet};
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent,
-    ContactManifoldData, ContactPairFilter, IntersectionEvent, IntersectionPairFilter,
-    PairFilterContext, RemovedCollider, SolverContact, SolverFlags,
+    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderSet, ContactData,
+    ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
+    IntersectionEvent, RemovedCollider, SolverContact, SolverFlags,
 };
-use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
 use crate::math::{Real, Vector};
-use crate::pipeline::EventHandler;
+use crate::pipeline::{
+    ContactModificationContext, EventHandler, PairFilterContext, PhysicsHooks, PhysicsHooksFlags,
+};
 use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
 use parry::utils::IsometryOpt;
 use std::collections::HashMap;
@@ -387,11 +388,13 @@ impl NarrowPhase {
         &mut self,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        pair_filter: Option<&dyn IntersectionPairFilter>,
+        hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
         let nodes = &self.intersection_graph.graph.nodes;
         let query_dispatcher = &*self.query_dispatcher;
+        let active_hooks = hooks.active_hooks();
+
         par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
             let handle1 = nodes[edge.source().index()].weight;
             let handle2 = nodes[edge.target().index()].weight;
@@ -415,12 +418,15 @@ impl NarrowPhase {
                 return;
             }
 
-            if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
+            if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR)
+                && !rb1.is_dynamic()
+                && !rb2.is_dynamic()
+            {
                 // Default filtering rule: no intersection between two non-dynamic bodies.
                 return;
             }
 
-            if let Some(filter) = pair_filter {
+            if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) {
                 let context = PairFilterContext {
                     rigid_body1: rb1,
                     rigid_body2: rb2,
@@ -430,7 +436,7 @@ impl NarrowPhase {
                     collider2: co2,
                 };
 
-                if !filter.filter_intersection_pair(&context) {
+                if !hooks.filter_intersection_pair(&context) {
                     // No intersection allowed.
                     return;
                 }
@@ -458,10 +464,11 @@ impl NarrowPhase {
         prediction_distance: Real,
         bodies: &RigidBodySet,
         colliders: &ColliderSet,
-        pair_filter: Option<&dyn ContactPairFilter>,
+        hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
         let query_dispatcher = &*self.query_dispatcher;
+        let active_hooks = hooks.active_hooks();
 
         par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
             let pair = &mut edge.weight;
@@ -485,12 +492,16 @@ impl NarrowPhase {
                 return;
             }
 
-            if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
+            if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
+                && !rb1.is_dynamic()
+                && !rb2.is_dynamic()
+            {
                 // Default filtering rule: no contact between two non-dynamic bodies.
                 return;
             }
 
-            let mut solver_flags = if let Some(filter) = pair_filter {
+            let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
+            {
                 let context = PairFilterContext {
                     rigid_body1: rb1,
                     rigid_body2: rb2,
@@ -500,14 +511,14 @@ impl NarrowPhase {
                     collider2: co2,
                 };
 
-                if let Some(solver_flags) = filter.filter_contact_pair(&context) {
+                if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
                     solver_flags
                 } else {
                     // No contact allowed.
                     return;
                 }
             } else {
-                SolverFlags::COMPUTE_IMPULSES
+                co1.solver_flags | co2.solver_flags
             };
 
             if !co1.solver_groups.test(co2.solver_groups) {
@@ -546,38 +557,58 @@ impl NarrowPhase {
                 manifold.data.solver_flags = solver_flags;
                 manifold.data.normal = world_pos1 * manifold.local_n1;
 
-                // Sort contacts to keep only these with distances bellow
-                // the prediction, and generate solver contacts.
-                let mut first_inactive_index = manifold.points.len();
+                // Generate solver contacts.
+                for (contact_id, contact) in manifold.points.iter().enumerate() {
+                    assert!(
+                        contact_id <= u8::MAX as usize,
+                        "A contact manifold cannot contain more than 255 contacts currently."
+                    );
 
-                while manifold.data.num_active_contacts() != first_inactive_index {
-                    let contact = &manifold.points[manifold.data.num_active_contacts()];
                     if contact.dist < prediction_distance {
                         // Generate the solver contact.
                         let solver_contact = SolverContact {
+                            contact_id: contact_id as u8,
                             point: world_pos1 * contact.local_p1
                                 + manifold.data.normal * contact.dist / 2.0,
                             dist: contact.dist,
                             friction,
                             restitution,
-                            surface_velocity: Vector::zeros(),
+                            tangent_velocity: Vector::zeros(),
                             data: contact.data,
                         };
 
-                        // TODO: apply the user-defined contact modification/removal, if needed.
-
                         manifold.data.solver_contacts.push(solver_contact);
                         has_any_active_contact = true;
-                        continue;
                     }
+                }
 
-                    // If we reach this code, then the contact must be ignored by the constraints solver.
-                    // Swap with the last contact.
-                    manifold.points.swap(
-                        manifold.data.num_active_contacts(),
-                        first_inactive_index - 1,
-                    );
-                    first_inactive_index -= 1;
+                // Apply the user-defined contact modification.
+                if active_hooks.contains(PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS)
+                    && manifold
+                        .data
+                        .solver_flags
+                        .contains(SolverFlags::MODIFY_SOLVER_CONTACTS)
+                {
+                    let mut modifiable_solver_contacts =
+                        std::mem::replace(&mut manifold.data.solver_contacts, Vec::new());
+                    let mut modifiable_user_data = manifold.data.user_data;
+
+                    let mut context = ContactModificationContext {
+                        rigid_body1: rb1,
+                        rigid_body2: rb2,
+                        collider_handle1: pair.pair.collider1,
+                        collider_handle2: pair.pair.collider2,
+                        collider1: co1,
+                        collider2: co2,
+                        manifold,
+                        solver_contacts: &mut modifiable_solver_contacts,
+                        user_data: &mut modifiable_user_data,
+                    };
+
+                    hooks.modify_solver_contacts(&mut context);
+
+                    manifold.data.solver_contacts = modifiable_solver_contacts;
+                    manifold.data.user_data = modifiable_user_data;
                 }
             }
 
diff --git a/src/geometry/pair_filter.rs b/src/geometry/pair_filter.rs
deleted file mode 100644
index 25f300f..0000000
--- a/src/geometry/pair_filter.rs
+++ /dev/null
@@ -1,61 +0,0 @@
-use crate::dynamics::RigidBody;
-use crate::geometry::{Collider, ColliderHandle, SolverFlags};
-
-/// Context given to custom collision filters to filter-out collisions.
-pub struct PairFilterContext<'a> {
-    /// The first collider involved in the potential collision.
-    pub rigid_body1: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub rigid_body2: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub collider_handle1: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider_handle2: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider1: &'a Collider,
-    /// The first collider involved in the potential collision.
-    pub collider2: &'a Collider,
-}
-
-/// User-defined filter for potential contact pairs detected by the broad-phase.
-///
-/// This can be used to apply custom logic in order to decide whether two colliders
-/// should have their contact computed by the narrow-phase, and if these contact
-/// should be solved by the constraints solver
-pub trait ContactPairFilter: Send + Sync {
-    /// Applies the contact pair filter.
-    ///
-    /// Note that using a contact pair filter will replace the default contact filtering
-    /// which consists of preventing contact computation between two non-dynamic bodies.
-    ///
-    /// This filtering method is called after taking into account the colliders collision groups.
-    ///
-    /// If this returns `None`, then the narrow-phase will ignore this contact pair and
-    /// not compute any contact manifolds for it.
-    /// If this returns `Some`, then the narrow-phase will compute contact manifolds for
-    /// this pair of colliders, and configure them with the returned solver flags. For
-    /// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts
-    /// will be taken into account by the constraints solver. If this returns
-    /// `Some(SolverFlags::empty())` then the constraints solver will ignore these
-    /// contacts.
-    fn filter_contact_pair(&self, context: &PairFilterContext) -> Option<SolverFlags>;
-}
-
-/// User-defined filter for potential intersection pairs detected by the broad-phase.
-///
-/// This can be used to apply custom logic in order to decide whether two colliders
-/// should have their intersection computed by the narrow-phase.
-pub trait IntersectionPairFilter: Send + Sync {
-    /// Applies the intersection pair filter.
-    ///
-    /// Note that using an intersection pair filter will replace the default intersection filtering
-    /// which consists of preventing intersection computation between two non-dynamic bodies.
-    ///
-    /// This filtering method is called after taking into account the colliders collision groups.
-    ///
-    /// If this returns `false`, then the narrow-phase will ignore this pair and
-    /// not compute any intersection information for it.
-    /// If this return `true` then the narrow-phase will compute intersection
-    /// information for this pair.
-    fn filter_intersection_pair(&self, context: &PairFilterContext) -> bool;
-}