Merge pull request #43 from dimforge/interaction_groups

Add collision filtering based in bit masks

8 files changed, 171 insertions, 142 deletions

diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 40b59ae..f53d75a 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity,
-    Segment, Shape, ShapeType, Triangle, Trimesh,
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
+    InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh,
 };
 #[cfg(feature = "dim3")]
 use crate::geometry::{Cone, Cylinder, RoundCylinder};
@@ -203,6 +203,8 @@ pub struct Collider {
     pub friction: f32,
     /// The restitution coefficient of this collider.
     pub restitution: f32,
+    pub(crate) collision_groups: InteractionGroups,
+    pub(crate) solver_groups: InteractionGroups,
     pub(crate) contact_graph_index: ColliderGraphIndex,
     pub(crate) proximity_graph_index: ColliderGraphIndex,
     pub(crate) proxy_index: usize,
@@ -255,6 +257,16 @@ impl Collider {
         &self.delta
     }
 
+    /// The collision groups used by this collider.
+    pub fn collision_groups(&self) -> InteractionGroups {
+        self.collision_groups
+    }
+
+    /// The solver groups used by this collider.
+    pub fn solver_groups(&self) -> InteractionGroups {
+        self.solver_groups
+    }
+
     /// The density of this collider.
     pub fn density(&self) -> f32 {
         self.density
@@ -298,8 +310,12 @@ pub struct ColliderBuilder {
     pub delta: Isometry<f32>,
     /// Is this collider a sensor?
     pub is_sensor: bool,
-    /// The user-data of the collider beind built.
+    /// The user-data of the collider being built.
     pub user_data: u128,
+    /// The collision groups for the collider being built.
+    pub collision_groups: InteractionGroups,
+    /// The solver groups for the collider being built.
+    pub solver_groups: InteractionGroups,
 }
 
 impl ColliderBuilder {
@@ -313,6 +329,8 @@ impl ColliderBuilder {
             delta: Isometry::identity(),
             is_sensor: false,
             user_data: 0,
+            collision_groups: InteractionGroups::all(),
+            solver_groups: InteractionGroups::all(),
         }
     }
 
@@ -418,12 +436,30 @@ impl ColliderBuilder {
         0.5
     }
 
-    /// An arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
+    /// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
     pub fn user_data(mut self, data: u128) -> Self {
         self.user_data = data;
         self
     }
 
+    /// Sets the collision groups used by this collider.
+    ///
+    /// Two colliders will interact iff. their collision groups are compatible.
+    /// See [InteractionGroups::test] for details.
+    pub fn collision_groups(mut self, groups: InteractionGroups) -> Self {
+        self.collision_groups = groups;
+        self
+    }
+
+    /// Sets the solver groups used by this collider.
+    ///
+    /// Forces between two colliders in contact will be computed iff their solver groups are
+    /// compatible. See [InteractionGroups::test] for details.
+    pub fn solver_groups(mut self, groups: InteractionGroups) -> Self {
+        self.solver_groups = groups;
+        self
+    }
+
     /// Sets whether or not the collider built by this builder is a sensor.
     pub fn sensor(mut self, is_sensor: bool) -> Self {
         self.is_sensor = is_sensor;
@@ -505,6 +541,8 @@ impl ColliderBuilder {
             contact_graph_index: InteractionGraph::<Contact>::invalid_graph_index(),
             proximity_graph_index: InteractionGraph::<Proximity>::invalid_graph_index(),
             proxy_index: crate::INVALID_USIZE,
+            collision_groups: self.collision_groups,
+            solver_groups: self.solver_groups,
             user_data: self.user_data,
         }
     }
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs
index d211cf1..1f50e43 100644
--- a/src/geometry/contact.rs
+++ b/src/geometry/contact.rs
@@ -9,6 +9,16 @@ use {
     simba::simd::SimdValue,
 };
 
+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 SolverFlags: u32 {
+        /// The constraint solver will take this contact manifold into
+        /// account for force computation.
+        const COMPUTE_FORCES = 0b01;
+    }
+}
+
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// The type local linear approximation of the neighborhood of a pair contact points on two shapes
@@ -206,6 +216,7 @@ impl ContactPair {
     pub(crate) fn single_manifold<'a, 'b>(
         &'a mut self,
         colliders: &'b ColliderSet,
+        flags: SolverFlags,
     ) -> (
         &'b Collider,
         &'b Collider,
@@ -216,7 +227,7 @@ impl ContactPair {
         let coll2 = &colliders[self.pair.collider2];
 
         if self.manifolds.len() == 0 {
-            let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2);
+            let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags);
             self.manifolds.push(manifold);
         }
 
@@ -288,6 +299,8 @@ pub struct ContactManifold {
     /// The relative position between the second collider and its parent at the time the
     /// contact points were generated.
     pub delta2: Isometry<f32>,
+    /// Flags used to control some aspects of the constraints solver for this contact manifold.
+    pub solver_flags: SolverFlags,
 }
 
 impl ContactManifold {
@@ -299,6 +312,7 @@ impl ContactManifold {
         delta2: Isometry<f32>,
         friction: f32,
         restitution: f32,
+        solver_flags: SolverFlags,
     ) -> ContactManifold {
         Self {
             #[cfg(feature = "dim2")]
@@ -319,6 +333,7 @@ impl ContactManifold {
             delta2,
             constraint_index: 0,
             position_constraint_index: 0,
+            solver_flags,
         }
     }
 
@@ -342,11 +357,17 @@ impl ContactManifold {
             delta2: self.delta2,
             constraint_index: self.constraint_index,
             position_constraint_index: self.position_constraint_index,
+            solver_flags: self.solver_flags,
         }
     }
 
-    pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self {
-        Self::with_subshape_indices(pair, coll1, coll2, 0, 0)
+    pub(crate) fn from_colliders(
+        pair: ColliderPair,
+        coll1: &Collider,
+        coll2: &Collider,
+        flags: SolverFlags,
+    ) -> Self {
+        Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags)
     }
 
     pub(crate) fn with_subshape_indices(
@@ -355,6 +376,7 @@ impl ContactManifold {
         coll2: &Collider,
         subshape1: usize,
         subshape2: usize,
+        solver_flags: SolverFlags,
     ) -> Self {
         Self::new(
             pair,
@@ -364,6 +386,7 @@ impl ContactManifold {
             *coll2.position_wrt_parent(),
             (coll1.friction + coll2.friction) * 0.5,
             (coll1.restitution + coll2.restitution) * 0.5,
+            solver_flags,
         )
     }
 
diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs
index b034760..728794d 100644
--- a/src/geometry/contact_generator/contact_generator.rs
+++ b/src/geometry/contact_generator/contact_generator.rs
@@ -1,5 +1,6 @@
 use crate::geometry::{
     Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
+    SolverFlags,
 };
 use crate::math::Isometry;
 #[cfg(feature = "simd-is-enabled")]
@@ -26,8 +27,9 @@ impl ContactPhase {
             Self::NearPhase(gen) => (gen.generate_contacts)(&mut context),
             Self::ExactPhase(gen) => {
                 // Build the primitive context from the non-primitive context and dispatch.
-                let (collider1, collider2, manifold, workspace) =
-                    context.pair.single_manifold(context.colliders);
+                let (collider1, collider2, manifold, workspace) = context
+                    .pair
+                    .single_manifold(context.colliders, context.solver_flags);
                 let mut context2 = PrimitiveContactGenerationContext {
                     prediction_distance: context.prediction_distance,
                     collider1,
@@ -85,9 +87,11 @@ impl ContactPhase {
                     [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH],
                 > = ArrayVec::new();
 
-                for pair in context.pairs.iter_mut() {
+                for (pair, solver_flags) in
+                    context.pairs.iter_mut().zip(context.solver_flags.iter())
+                {
                     let (collider1, collider2, manifold, workspace) =
-                        pair.single_manifold(context.colliders);
+                        pair.single_manifold(context.colliders, *solver_flags);
                     colliders_arr.push((collider1, collider2));
                     manifold_arr.push(manifold);
                     workspace_arr.push(workspace);
@@ -188,6 +192,7 @@ pub struct ContactGenerationContext<'a> {
     pub prediction_distance: f32,
     pub colliders: &'a ColliderSet,
     pub pair: &'a mut ContactPair,
+    pub solver_flags: SolverFlags,
 }
 
 #[cfg(feature = "simd-is-enabled")]
@@ -196,6 +201,7 @@ pub struct ContactGenerationContextSimd<'a, 'b> {
     pub prediction_distance: f32,
     pub colliders: &'a ColliderSet,
     pub pairs: &'a mut [&'b mut ContactPair],
+    pub solver_flags: &'a [SolverFlags],
 }
 
 #[derive(Copy, Clone)]
diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
index 9224d4e..f291fa0 100644
--- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
@@ -104,6 +104,7 @@ fn do_generate_contacts(
     let manifolds = &mut ctxt.pair.manifolds;
     let prediction_distance = ctxt.prediction_distance;
     let dispatcher = ctxt.dispatcher;
+    let solver_flags = ctxt.solver_flags;
     let shape_type2 = collider2.shape().shape_type();
 
     heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| {
@@ -131,8 +132,14 @@ fn do_generate_contacts(
                     timestamp: new_timestamp,
                     workspace: workspace2,
                 };
-                let manifold =
-                    ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0);
+                let manifold = ContactManifold::with_subshape_indices(
+                    coll_pair,
+                    collider1,
+                    collider2,
+                    i,
+                    0,
+                    solver_flags,
+                );
                 manifolds.push(manifold);
 
                 entry.insert(sub_detector)
diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
index 502658d..9474516 100644
--- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
@@ -149,6 +149,7 @@ fn do_generate_contacts(
                     collider2,
                     *triangle_id,
                     0,
+                    ctxt.solver_flags,
                 )
             } else {
                 // We already have a manifold for this triangle.
diff --git a/src/geometry/interaction_groups.rs b/src/geometry/interaction_groups.rs
new file mode 100644
index 0000000..48808de
--- /dev/null
+++ b/src/geometry/interaction_groups.rs
@@ -0,0 +1,60 @@
+#[repr(transparent)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)]
+/// Pairwise filtering using bit masks.
+///
+/// This filtering method is based on two 16-bit values:
+/// - The interaction groups (the 16 left-most bits of `self.0`).
+/// - The interaction mask (the 16 right-most bits of `self.0`).
+///
+/// An interaction is allowed between two filters `a` and `b` two conditions
+/// are met simultaneously:
+/// - The interaction groups of `a` has at least one bit set to `1` in common with the interaction mask of `b`.
+/// - The interaction groups of `b` has at least one bit set to `1` in common with the interaction mask of `a`.
+/// In other words, interactions are allowed between two filter iff. the following condition is met:
+/// ```ignore
+/// ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0
+/// ```
+pub struct InteractionGroups(pub u32);
+
+impl InteractionGroups {
+    /// Initializes with the given interaction groups and interaction mask.
+    pub const fn new(groups: u16, masks: u16) -> Self {
+        Self::none().with_groups(groups).with_mask(masks)
+    }
+
+    /// Allow interaction with everything.
+    pub const fn all() -> Self {
+        Self(u32::MAX)
+    }
+
+    /// Prevent all interactions.
+    pub const fn none() -> Self {
+        Self(0)
+    }
+
+    /// Sets the group this filter is part of.
+    pub const fn with_groups(self, groups: u16) -> Self {
+        Self((self.0 & 0x0000ffff) | ((groups as u32) << 16))
+    }
+
+    /// Sets the interaction mask of this filter.
+    pub const fn with_mask(self, mask: u16) -> Self {
+        Self((self.0 & 0xffff0000) | (mask as u32))
+    }
+
+    /// Check if interactions should be allowed based on the interaction groups and mask.
+    ///
+    /// An interaction is allowed iff. the groups of `self` contain at least one bit set to 1 in common
+    /// with the mask of `rhs`, and vice-versa.
+    #[inline]
+    pub const fn test(self, rhs: Self) -> bool {
+        ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0
+    }
+}
+
+impl Default for InteractionGroups {
+    fn default() -> Self {
+        Self::all()
+    }
+}
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index abb9844..9da35d9 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -5,7 +5,7 @@ pub use self::capsule::Capsule;
 pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
 pub use self::collider_set::{ColliderHandle, ColliderSet};
 pub use self::contact::{
-    Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory,
+    Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags,
 };
 pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
 #[cfg(feature = "dim2")]
@@ -70,6 +70,7 @@ pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
 pub(crate) use self::waabb::{WRay, WAABB};
 pub(crate) use self::wquadtree::WQuadtree;
 //pub(crate) use self::z_order::z_cmp_floats;
+pub use self::interaction_groups::InteractionGroups;
 pub use self::shape::{Shape, ShapeType};
 
 mod ball;
@@ -97,6 +98,7 @@ mod waabb;
 mod wquadtree;
 //mod z_order;
 mod capsule;
+mod interaction_groups;
 #[cfg(feature = "dim3")]
 mod polygonal_feature_map;
 #[cfg(feature = "dim3")]
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index e95709c..f5517ba 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -15,7 +15,7 @@ use crate::geometry::proximity_detector::{
 //};
 use crate::geometry::{
     BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent,
-    ProximityPair, RemovedCollider,
+    ProximityPair, RemovedCollider, SolverFlags,
 };
 use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
 //#[cfg(feature = "simd-is-enabled")]
@@ -306,6 +306,11 @@ impl NarrowPhase {
                 return;
             }
 
+            if !co1.collision_groups.test(co2.collision_groups) {
+                // The collision is not allowed.
+                return;
+            }
+
             let dispatcher = DefaultProximityDispatcher;
             if pair.detector.is_none() {
                 // We need a redispatch for this detector.
@@ -329,69 +334,6 @@ impl NarrowPhase {
                 .unwrap()
                 .detect_proximity(context, events);
         });
-
-        /*
-        // First, group pairs.
-        // NOTE: the transmutes here are OK because the Vec are all cleared
-        // before we leave this method.
-        // We do this in order to avoid reallocating those vecs each time
-        // we compute the contacts. Unsafe is necessary because we can't just
-        // store a Vec<&mut ProximityPair> into the NarrowPhase struct without
-        // polluting the World with lifetimes.
-        let ball_ball_prox: &mut Vec<&mut ProximityPair> =
-            unsafe { std::mem::transmute(&mut self.ball_ball_prox) };
-        let shape_shape_prox: &mut Vec<&mut ProximityPair> =
-            unsafe { std::mem::transmute(&mut self.shape_shape_prox) };
-
-        let bodies = &bodies.bodies;
-
-        // FIXME: don't iterate through all the interactions.
-        for pair in &mut self.proximity_graph.interactions {
-            let co1 = &colliders[pair.pair.collider1];
-            let co2 = &colliders[pair.pair.collider2];
-
-            // FIXME: avoid lookup into bodies.
-            let rb1 = &bodies[co1.parent];
-            let rb2 = &bodies[co2.parent];
-
-            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
-            {
-                // No need to update this proximity because nothing moved.
-                continue;
-            }
-
-            match (co1.shape(), co2.shape()) {
-                (Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair),
-                _ => shape_shape_prox.push(pair),
-            }
-        }
-
-        par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| {
-            let context = ProximityDetectionContextSimd {
-                dispatcher: &DefaultProximityDispatcher,
-                prediction_distance,
-                colliders,
-                pairs,
-            };
-            context.pairs[0]
-                .detector
-                .detect_proximity_simd(context, events);
-        });
-
-        par_iter_mut!(shape_shape_prox).for_each(|pair| {
-            let context = ProximityDetectionContext {
-                dispatcher: &DefaultProximityDispatcher,
-                prediction_distance,
-                colliders,
-                pair,
-            };
-
-            context.pair.detector.detect_proximity(context, events);
-        });
-
-        ball_ball_prox.clear();
-        shape_shape_prox.clear();
-        */
     }
 
     pub(crate) fn compute_contacts(
@@ -417,6 +359,11 @@ impl NarrowPhase {
                 return;
             }
 
+            if !co1.collision_groups.test(co2.collision_groups) {
+                // The collision is not allowed.
+                return;
+            }
+
             let dispatcher = DefaultContactDispatcher;
             if pair.generator.is_none() {
                 // We need a redispatch for this generator.
@@ -427,11 +374,18 @@ impl NarrowPhase {
                 pair.generator_workspace = workspace;
             }
 
+            let solver_flags = if co1.solver_groups.test(co2.solver_groups) {
+                SolverFlags::COMPUTE_FORCES
+            } else {
+                SolverFlags::empty()
+            };
+
             let context = ContactGenerationContext {
                 dispatcher: &dispatcher,
                 prediction_distance,
                 colliders,
                 pair,
+                solver_flags,
             };
 
             context
@@ -440,69 +394,6 @@ impl NarrowPhase {
                 .unwrap()
                 .generate_contacts(context, events);
         });
-
-        /*
-        // First, group pairs.
-        // NOTE: the transmutes here are OK because the Vec are all cleared
-        // before we leave this method.
-        // We do this in order to avoid reallocating those vecs each time
-        // we compute the contacts. Unsafe is necessary because we can't just
-        // store a Vec<&mut ContactPair> into the NarrowPhase struct without
-        // polluting the World with lifetimes.
-        let ball_ball: &mut Vec<&mut ContactPair> =
-            unsafe { std::mem::transmute(&mut self.ball_ball) };
-        let shape_shape: &mut Vec<&mut ContactPair> =
-            unsafe { std::mem::transmute(&mut self.shape_shape) };
-
-        let bodies = &bodies.bodies;
-
-        // FIXME: don't iterate through all the interactions.
-        for pair in &mut self.contact_graph.interactions {
-            let co1 = &colliders[pair.pair.collider1];
-            let co2 = &colliders[pair.pair.collider2];
-
-            // FIXME: avoid lookup into bodies.
-            let rb1 = &bodies[co1.parent];
-            let rb2 = &bodies[co2.parent];
-
-            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
-            {
-                // No need to update this contact because nothing moved.
-                continue;
-            }
-
-            match (co1.shape(), co2.shape()) {
-                (Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair),
-                _ => shape_shape.push(pair),
-            }
-        }
-
-        par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| {
-            let context = ContactGenerationContextSimd {
-                dispatcher: &DefaultContactDispatcher,
-                prediction_distance,
-                colliders,
-                pairs,
-            };
-            context.pairs[0]
-                .generator
-                .generate_contacts_simd(context, events);
-        });
-
-        par_iter_mut!(shape_shape).for_each(|pair| {
-            let context = ContactGenerationContext {
-                dispatcher: &DefaultContactDispatcher,
-                prediction_distance,
-                colliders,
-                pair,
-            };
-
-            context.pair.generator.generate_contacts(context, events);
-        });
-
-        ball_ball.clear();
-        shape_shape.clear();
-        */
     }
 
     /// Retrieve all the interactions with at least one contact point, happening between two active bodies.
@@ -522,7 +413,8 @@ impl NarrowPhase {
             for manifold in &mut inter.weight.manifolds {
                 let rb1 = &bodies[manifold.body_pair.body1];
                 let rb2 = &bodies[manifold.body_pair.body2];
-                if manifold.num_active_contacts() != 0
+                if manifold.solver_flags.contains(SolverFlags::COMPUTE_FORCES)
+                    && manifold.num_active_contacts() != 0
                     && (!rb1.is_dynamic() || !rb1.is_sleeping())
                     && (!rb2.is_dynamic() || !rb2.is_sleeping())
                 {