Merge pull request #120 from dimforge/contact_modification

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

4 files changed, 231 insertions, 26 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 866c3a5..a74a6e5 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -1,12 +1,9 @@
 //! Physics pipeline structures.
 
 use crate::dynamics::{JointSet, RigidBodySet};
-use crate::geometry::{
-    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter,
-    IntersectionPairFilter, NarrowPhase,
-};
+use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
 use crate::math::Real;
-use crate::pipeline::EventHandler;
+use crate::pipeline::{EventHandler, PhysicsHooks};
 
 /// The collision pipeline, responsible for performing collision detection between colliders.
 ///
@@ -44,8 +41,7 @@ impl CollisionPipeline {
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
-        contact_pair_filter: Option<&dyn ContactPairFilter>,
-        proximity_pair_filter: Option<&dyn IntersectionPairFilter>,
+        hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
         bodies.maintain(colliders);
@@ -58,14 +54,8 @@ impl CollisionPipeline {
 
         narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
 
-        narrow_phase.compute_contacts(
-            prediction_distance,
-            bodies,
-            colliders,
-            contact_pair_filter,
-            events,
-        );
-        narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
+        narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events);
+        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
 
         bodies.update_active_set_with_contacts(
             colliders,
diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index 287de9d..fd85cfa 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -2,10 +2,14 @@
 
 pub use collision_pipeline::CollisionPipeline;
 pub use event_handler::{ChannelEventCollector, EventHandler};
+pub use physics_hooks::{
+    ContactModificationContext, PairFilterContext, PhysicsHooks, PhysicsHooksFlags,
+};
 pub use physics_pipeline::PhysicsPipeline;
 pub use query_pipeline::QueryPipeline;
 
 mod collision_pipeline;
 mod event_handler;
+mod physics_hooks;
 mod physics_pipeline;
 mod query_pipeline;
diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs
new file mode 100644
index 0000000..11ca485
--- /dev/null
+++ b/src/pipeline/physics_hooks.rs
@@ -0,0 +1,215 @@
+use crate::dynamics::RigidBody;
+use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags};
+use crate::math::{Real, Vector};
+use na::ComplexField;
+
+/// 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,
+}
+
+/// Context given to custom contact modifiers to modify the contacts seen by the constrainst solver.
+pub struct ContactModificationContext<'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,
+    /// The contact manifold.
+    pub manifold: &'a ContactManifold,
+    /// The solver contacts that can be modified.
+    pub solver_contacts: &'a mut Vec<SolverContact>,
+    /// User-defined data attached to the manifold.
+    // NOTE: we keep this a &'a mut u32 to emphasize the
+    // fact that this can be modified.
+    pub user_data: &'a mut u32,
+}
+
+impl<'a> ContactModificationContext<'a> {
+    /// Helper function to update `self` to emulate a oneway-platform.
+    ///
+    /// The "oneway" behavior will only allow contacts between two colliders
+    /// if the local contact normal of the first collider involved in the contact
+    /// is almost aligned with the provided `allowed_local_n1` direction.
+    ///
+    /// To make this method work properly it must be called as part of the
+    /// `PhysicsHooks::modify_solver_contacts` method at each timestep, for each
+    /// contact manifold involving a one-way platform. The `self.user_data` field
+    /// must not be modified from the outside of this method.
+    pub fn update_as_oneway_platform(
+        &mut self,
+        allowed_local_n1: &Vector<Real>,
+        allowed_angle: Real,
+    ) {
+        const CONTACT_CONFIGURATION_UNKNOWN: u32 = 0;
+        const CONTACT_CURRENTLY_ALLOWED: u32 = 1;
+        const CONTACT_CURRENTLY_FORBIDDEN: u32 = 2;
+
+        let cang = ComplexField::cos(allowed_angle);
+
+        // Test the allowed normal with the local-space contact normal that
+        // points towards the exterior of context.collider1.
+        let contact_is_ok = self.manifold.local_n1.dot(&allowed_local_n1) >= cang;
+
+        match *self.user_data {
+            CONTACT_CONFIGURATION_UNKNOWN => {
+                if contact_is_ok {
+                    // The contact is close enough to the allowed normal.
+                    *self.user_data = CONTACT_CURRENTLY_ALLOWED;
+                } else {
+                    // The contact normal isn't close enough to the allowed
+                    // normal, so remove all the contacts and mark further contacts
+                    // as forbidden.
+                    self.solver_contacts.clear();
+                    *self.user_data = CONTACT_CURRENTLY_FORBIDDEN;
+                }
+            }
+            CONTACT_CURRENTLY_FORBIDDEN => {
+                // Contacts are forbidden so we need to continue forbidding contacts
+                // until all the contacts are non-penetrating again. In that case, if
+                // the contacts are OK wrt. the contact normal, then we can mark them as allowed.
+                if contact_is_ok && self.solver_contacts.iter().all(|c| c.dist > 0.0) {
+                    *self.user_data = CONTACT_CURRENTLY_ALLOWED;
+                } else {
+                    // Discard all the contacts.
+                    self.solver_contacts.clear();
+                }
+            }
+            CONTACT_CURRENTLY_ALLOWED => {
+                // We allow all the contacts right now. The configuration becomes
+                // uncertain again when the contact manifold no longer contains any contact.
+                if self.solver_contacts.is_empty() {
+                    *self.user_data = CONTACT_CONFIGURATION_UNKNOWN;
+                }
+            }
+            _ => unreachable!(),
+        }
+    }
+}
+
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+    pub struct PhysicsHooksFlags: u32 {
+        /// If set, Rapier will call `PhysicsHooks::filter_contact_pair` whenever relevant.
+        const FILTER_CONTACT_PAIR = 0b0001;
+        /// If set, Rapier will call `PhysicsHooks::filter_intersection_pair` whenever relevant.
+        const FILTER_INTERSECTION_PAIR = 0b0010;
+        /// If set, Rapier will call `PhysicsHooks::modify_solver_contact` whenever relevant.
+        const MODIFY_SOLVER_CONTACTS = 0b0100;
+    }
+}
+
+/// User-defined functions called by the physics engines during one timestep in order to customize its behavior.
+pub trait PhysicsHooks: Send + Sync {
+    /// The sets of hooks that must be taken into account.
+    fn active_hooks(&self) -> PhysicsHooksFlags;
+
+    /// Applies the contact pair filter.
+    ///
+    /// Note that this method will only be called if `self.active_hooks()`
+    /// contains the `PhysicsHooksFlags::FILTER_CONTACT_PAIR` flags.
+    ///
+    /// 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
+    ///
+    /// 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> {
+        None
+    }
+
+    /// Applies the intersection pair filter.
+    ///
+    /// Note that this method will only be called if `self.active_hooks()`
+    /// contains the `PhysicsHooksFlags::FILTER_INTERSECTION_PAIR` flags.
+    ///
+    /// 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.
+    ///
+    /// 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 {
+        false
+    }
+
+    /// Modifies the set of contacts seen by the constraints solver.
+    ///
+    /// Note that this method will only be called if `self.active_hooks()`
+    /// contains the `PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS` flags.
+    ///
+    /// By default, the content of `solver_contacts` is computed from `manifold.points`.
+    /// This method will be called on each contact manifold which have the flag `SolverFlags::modify_solver_contacts` set.
+    /// This method can be used to modify the set of solver contacts seen by the constraints solver: contacts
+    /// can be removed and modified.
+    ///
+    /// Note that if all the contacts have to be ignored by the constraint solver, you may simply
+    /// do `context.solver_contacts.clear()`.
+    ///
+    /// Modifying the solver contacts allow you to achieve various effects, including:
+    /// - Simulating conveyor belts by setting the `surface_velocity` of a solver contact.
+    /// - Simulating shapes with multiply materials by modifying the friction and restitution
+    ///   coefficient depending of the features in contacts.
+    /// - Simulating one-way platforms depending on the contact normal.
+    ///
+    /// Each contact manifold is given a `u32` user-defined data that is persistent between
+    /// timesteps (as long as the contact manifold exists). This user-defined data is initialized
+    /// as 0 and can be modified in `context.user_data`.
+    fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {}
+}
+
+impl PhysicsHooks for () {
+    fn active_hooks(&self) -> PhysicsHooksFlags {
+        PhysicsHooksFlags::empty()
+    }
+
+    fn filter_contact_pair(&self, _: &PairFilterContext) -> Option<SolverFlags> {
+        None
+    }
+
+    fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool {
+        false
+    }
+
+    fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {}
+}
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 293fa9d..198c4be 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -7,11 +7,10 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
-    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex,
-    ContactPairFilter, IntersectionPairFilter, NarrowPhase,
+    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
 };
 use crate::math::{Real, Vector};
-use crate::pipeline::EventHandler;
+use crate::pipeline::{EventHandler, PhysicsHooks};
 
 /// The physics pipeline, responsible for stepping the whole physics simulation.
 ///
@@ -69,8 +68,7 @@ impl PhysicsPipeline {
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
-        contact_pair_filter: Option<&dyn ContactPairFilter>,
-        proximity_pair_filter: Option<&dyn IntersectionPairFilter>,
+        hooks: &dyn PhysicsHooks,
         events: &dyn EventHandler,
     ) {
         self.counters.step_started();
@@ -115,10 +113,10 @@ impl PhysicsPipeline {
             integration_parameters.prediction_distance,
             bodies,
             colliders,
-            contact_pair_filter,
+            hooks,
             events,
         );
-        narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
+        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
         //        println!("Compute contact time: {}", instant::now() - t);
 
         self.counters.stages.island_construction_time.start();
@@ -280,8 +278,7 @@ mod test {
             &mut bodies,
             &mut colliders,
             &mut joints,
-            None,
-            None,
+            &(),
             &(),
         );
     }
@@ -324,8 +321,7 @@ mod test {
             &mut bodies,
             &mut colliders,
             &mut joints,
-            None,
-            None,
+            &(),
             &(),
         );
     }