CCD: take collision groups into account

6 files changed, 107 insertions, 48 deletions

diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index 354f183..dab4f73 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -10,7 +10,7 @@ use crate::geometry::{
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
 use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
-use crate::prelude::{ActiveEvents, ColliderFlags};
+use crate::prelude::{ActiveEvents, ColliderFlags, ColliderGroups};
 use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
 use parry::utils::hashmap::HashMap;
 use std::collections::BinaryHeap;
@@ -140,7 +140,8 @@ impl CCDSolver {
         Colliders: ComponentSetOption<ColliderParent>
             + ComponentSet<ColliderPosition>
             + ComponentSet<ColliderShape>
-            + ComponentSet<ColliderType>,
+            + ComponentSet<ColliderType>
+            + ComponentSet<ColliderGroups>,
     {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
@@ -201,16 +202,21 @@ impl CCDSolver {
                             {
                                 let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
                                 let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
-                                let c1: (_, _, _) = colliders.index_bundle(ch1.0);
-                                let c2: (_, _, _) = colliders.index_bundle(ch2.0);
+                                let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0);
+                                let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0);
                                 let co_type1: &ColliderType = colliders.index(ch1.0);
                                 let co_type2: &ColliderType = colliders.index(ch1.0);
 
                                 let bh1 = co_parent1.map(|p| p.handle);
                                 let bh2 = co_parent2.map(|p| p.handle);
 
-                                if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) {
-                                    // Ignore self-intersection and sensors.
+                                // Ignore self-intersection and sensors and apply collision groups filter.
+                                if bh1 == bh2                                                 // Ignore self-intersection.
+                                    || (co_type1.is_sensor() || co_type2.is_sensor())         // Ignore sensors.
+                                    || !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups.
+                                    || !c1.3.solver_groups.test(c2.3.solver_groups)
+                                // Apply solver groups.
+                                {
                                     return true;
                                 }
 
@@ -226,8 +232,8 @@ impl CCDSolver {
                                     self.query_pipeline.query_dispatcher(),
                                     *ch1,
                                     *ch2,
-                                    (c1.0, c1.1, c1.2, co_parent1),
-                                    (c2.0, c2.1, c2.2, co_parent2),
+                                    (c1.0, c1.1, c1.2, c1.3, co_parent1),
+                                    (c2.0, c2.1, c2.2, c2.3, co_parent2),
                                     Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
                                     b2,
                                     None,
@@ -274,7 +280,8 @@ impl CCDSolver {
             + ComponentSet<ColliderPosition>
             + ComponentSet<ColliderShape>
             + ComponentSet<ColliderType>
-            + ComponentSet<ColliderFlags>,
+            + ComponentSet<ColliderFlags>
+            + ComponentSet<ColliderGroups>,
     {
         let mut frozen = HashMap::<_, Real>::default();
         let mut all_toi = BinaryHeap::new();
@@ -336,14 +343,15 @@ impl CCDSolver {
                             {
                                 let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
                                 let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
-                                let c1: (_, _, _) = colliders.index_bundle(ch1.0);
-                                let c2: (_, _, _) = colliders.index_bundle(ch2.0);
+                                let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0);
+                                let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0);
 
                                 let bh1 = co_parent1.map(|p| p.handle);
                                 let bh2 = co_parent2.map(|p| p.handle);
 
-                                if bh1 == bh2 {
-                                    // Ignore self-intersection.
+                                // Ignore self-intersections and apply groups filter.
+                                if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups)
+                                {
                                     return true;
                                 }
 
@@ -360,8 +368,8 @@ impl CCDSolver {
                                     self.query_pipeline.query_dispatcher(),
                                     *ch1,
                                     *ch2,
-                                    (c1.0, c1.1, c1.2, co_parent1),
-                                    (c2.0, c2.1, c2.2, co_parent2),
+                                    (c1.0, c1.1, c1.2, c1.3, co_parent1),
+                                    (c2.0, c2.1, c2.2, c2.3, co_parent2),
                                     b1,
                                     b2,
                                     None,
@@ -400,7 +408,7 @@ impl CCDSolver {
 
         // NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this
         // may avoid some resweeps.
-        let mut intersections_to_check = vec![];
+        let mut pseudo_intersections_to_check = vec![];
 
         while let Some(toi) = all_toi.pop() {
             assert!(toi.toi <= dt);
@@ -422,7 +430,7 @@ impl CCDSolver {
                 continue;
             }
 
-            if toi.is_intersection_test {
+            if toi.is_pseudo_intersection_test {
                 // NOTE: this test is redundant with the previous `if !should_freeze && ...`
                 //       but let's keep it to avoid tricky regressions if we end up swapping both
                 //       `if` for some reasons in the future.
@@ -430,7 +438,7 @@ impl CCDSolver {
                     // This is only an intersection so we don't have to freeze and there is no
                     // need to resweep. However we will need to see if we have to generate
                     // intersection events, so push the TOI for further testing.
-                    intersections_to_check.push(toi);
+                    pseudo_intersections_to_check.push(toi);
                 }
                 continue;
             }
@@ -462,14 +470,14 @@ impl CCDSolver {
                     .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
                         let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
                         let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
-                        let c1: (_, _, _) = colliders.index_bundle(ch1.0);
-                        let c2: (_, _, _) = colliders.index_bundle(ch2.0);
+                        let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0);
+                        let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0);
 
                         let bh1 = co_parent1.map(|p| p.handle);
                         let bh2 = co_parent2.map(|p| p.handle);
 
-                        if bh1 == bh2 {
-                            // Ignore self-intersection.
+                        // Ignore self-intersection and apply groups filter.
+                        if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) {
                             return true;
                         }
 
@@ -498,8 +506,8 @@ impl CCDSolver {
                             self.query_pipeline.query_dispatcher(),
                             *ch1,
                             *ch2,
-                            (c1.0, c1.1, c1.2, co_parent1),
-                            (c2.0, c2.1, c2.2, co_parent2),
+                            (c1.0, c1.1, c1.2, c1.3, co_parent1),
+                            (c2.0, c2.1, c2.2, c2.3, co_parent2),
                             b1,
                             b2,
                             frozen1.copied(),
@@ -516,7 +524,7 @@ impl CCDSolver {
             }
         }
 
-        for toi in intersections_to_check {
+        for toi in pseudo_intersections_to_check {
             // See if the intersection is still active once the bodies
             // reach their final positions.
             // - If the intersection is still active, don't report it yet. It will be
@@ -524,17 +532,30 @@ impl CCDSolver {
             // - If the intersection isn't active anymore, and it wasn't intersecting
             //   before, then we need to generate one interaction-start and one interaction-stop
             //   events because it will never be detected by the narrow-phase because of tunneling.
-            let (co_pos1, co_shape1, co_flags1): (
+            let (co_type1, co_pos1, co_shape1, co_flags1): (
+                &ColliderType,
                 &ColliderPosition,
                 &ColliderShape,
                 &ColliderFlags,
             ) = colliders.index_bundle(toi.c1.0);
-            let (co_pos2, co_shape2, co_flags2): (
+            let (co_type2, co_pos2, co_shape2, co_flags2): (
+                &ColliderType,
                 &ColliderPosition,
                 &ColliderShape,
                 &ColliderFlags,
             ) = colliders.index_bundle(toi.c2.0);
 
+            if !co_type1.is_sensor() && !co_type2.is_sensor() {
+                // TODO: this happens if we found a TOI between two non-sensor
+                //       colliders with mismatching solver_flags. It is not clear
+                //       what we should do in this case: we could report a
+                //       contact started/contact stopped event for example. But in
+                //       that case, what contact pair should be pass to these events?
+                // For now we just ignore this special case. Let's wait for an actual
+                // use-case to come up before we determine what we want to do here.
+                continue;
+            }
+
             let co_next_pos1 = if let Some(b1) = toi.b1 {
                 let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap();
                 let (rb_pos1, rb_vels1, rb_mprops1): (
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 78396c0..918e7c4 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -2,7 +2,7 @@ use crate::dynamics::{
     RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
 };
 use crate::geometry::{
-    ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
+    ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
 };
 use crate::math::Real;
 use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -14,7 +14,9 @@ pub struct TOIEntry {
     pub b1: Option<RigidBodyHandle>,
     pub c2: ColliderHandle,
     pub b2: Option<RigidBodyHandle>,
-    pub is_intersection_test: bool,
+    // We call this "pseudo" intersection because this also
+    // includes colliders pairs with mismatching solver_groups.
+    pub is_pseudo_intersection_test: bool,
     pub timestamp: usize,
 }
 
@@ -25,7 +27,7 @@ impl TOIEntry {
         b1: Option<RigidBodyHandle>,
         c2: ColliderHandle,
         b2: Option<RigidBodyHandle>,
-        is_intersection_test: bool,
+        is_pseudo_intersection_test: bool,
         timestamp: usize,
     ) -> Self {
         Self {
@@ -34,7 +36,7 @@ impl TOIEntry {
             b1,
             c2,
             b2,
-            is_intersection_test,
+            is_pseudo_intersection_test,
             timestamp,
         }
     }
@@ -47,12 +49,14 @@ impl TOIEntry {
             &ColliderType,
             &ColliderShape,
             &ColliderPosition,
+            &ColliderGroups,
             Option<&ColliderParent>,
         ),
         c2: (
             &ColliderType,
             &ColliderShape,
             &ColliderPosition,
+            &ColliderGroups,
             Option<&ColliderParent>,
         ),
         b1: Option<(
@@ -78,8 +82,8 @@ impl TOIEntry {
             return None;
         }
 
-        let (co_type1, co_shape1, co_pos1, co_parent1) = c1;
-        let (co_type2, co_shape2, co_pos2, co_parent2) = c2;
+        let (co_type1, co_shape1, co_pos1, co_groups1, co_parent1) = c1;
+        let (co_type2, co_shape2, co_pos2, co_groups2, co_parent2) = c2;
 
         let linvel1 =
             frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
@@ -104,7 +108,9 @@ impl TOIEntry {
         // keep it since more conservatism is good at this stage.
         let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
             + smallest_contact_dist.max(0.0);
-        let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor();
+        let is_pseudo_intersection_test = co_type1.is_sensor()
+            || co_type2.is_sensor()
+            || !co_groups1.solver_groups.test(co_groups2.solver_groups);
 
         if (end_time - start_time) * vel12 < thickness {
             return None;
@@ -135,7 +141,7 @@ impl TOIEntry {
         // If the TOI search involves two non-sensor colliders then
         // we don't want to stop the TOI search at the first penetration
         // because the colliders may be in a separating trajectory.
-        let stop_at_penetration = is_intersection_test;
+        let stop_at_penetration = is_pseudo_intersection_test;
 
         let res_toi = query_dispatcher
             .nonlinear_time_of_impact(
@@ -157,7 +163,7 @@ impl TOIEntry {
             co_parent1.map(|p| p.handle),
             ch2,
             co_parent2.map(|p| p.handle),
-            is_intersection_test,
+            is_pseudo_intersection_test,
             0,
         ))
     }
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 612df3c..b9007bf 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -33,7 +33,6 @@ pub struct Collider {
 
 impl Collider {
     pub(crate) fn reset_internal_references(&mut self) {
-        self.co_parent = None;
         self.co_bf_data.proxy_index = crate::INVALID_U32;
         self.co_changes = ColliderChanges::all();
     }
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index d8603cd..dc0abdc 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -134,6 +134,7 @@ impl ColliderSet {
         // Make sure the internal links are reset, they may not be
         // if this rigid-body was obtained by cloning another one.
         coll.reset_internal_references();
+        coll.co_parent = None;
         let handle = ColliderHandle(self.colliders.insert(coll));
         self.modified_colliders.push(handle);
         handle
@@ -147,12 +148,17 @@ impl ColliderSet {
         bodies: &mut RigidBodySet,
     ) -> ColliderHandle {
         // Make sure the internal links are reset, they may not be
-        // if this rigid-body was obtained by cloning another one.
+        // if this collider was obtained by cloning another one.
         coll.reset_internal_references();
-        coll.co_parent = Some(ColliderParent {
-            handle: parent_handle,
-            pos_wrt_parent: coll.co_pos.0,
-        });
+
+        if let Some(prev_parent) = &mut coll.co_parent {
+            prev_parent.handle = parent_handle;
+        } else {
+            coll.co_parent = Some(ColliderParent {
+                handle: parent_handle,
+                pos_wrt_parent: coll.co_pos.0,
+            });
+        }
 
         // NOTE: we use `get_mut` instead of `get_mut_internal` so that the
         // modification flag is updated properly.
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 74249af..f5d9b27 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -367,7 +367,8 @@ impl PhysicsPipeline {
             + ComponentSet<ColliderPosition>
             + ComponentSet<ColliderShape>
             + ComponentSet<ColliderType>
-            + ComponentSet<ColliderFlags>,
+            + ComponentSet<ColliderFlags>
+            + ComponentSet<ColliderGroups>,
     {
         self.counters.ccd.toi_computation_time.start();
         // Handle CCD
@@ -430,7 +431,10 @@ impl PhysicsPipeline {
         islands: &IslandManager,
         bodies: &mut Bodies,
     ) where
-        Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyPosition>,
+        Bodies: ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyMassProps>,
     {
         // Update kinematic bodies velocities.
         // TODO: what is the best place for this? It should at least be
@@ -438,9 +442,31 @@ impl PhysicsPipeline {
         // there to determine if this kinematic body should wake-up dynamic
         // bodies it is touching.
         for handle in islands.active_kinematic_bodies() {
-            let ppos: &RigidBodyPosition = bodies.index(handle.0);
-            let new_vel = ppos.interpolate_velocity(integration_parameters.inv_dt());
-            bodies.set_internal(handle.0, new_vel);
+            let (rb_type, rb_pos, rb_vel, rb_mprops): (
+                &RigidBodyType,
+                &RigidBodyPosition,
+                &RigidBodyVelocity,
+                &RigidBodyMassProps,
+            ) = bodies.index_bundle(handle.0);
+
+            match rb_type {
+                RigidBodyType::KinematicPositionBased => {
+                    let rb_pos: &RigidBodyPosition = bodies.index(handle.0);
+                    let new_vel = rb_pos.interpolate_velocity(integration_parameters.inv_dt());
+                    bodies.set_internal(handle.0, new_vel);
+                }
+                RigidBodyType::KinematicVelocityBased => {
+                    let new_pos = rb_vel.integrate(
+                        integration_parameters.dt,
+                        &rb_pos.position,
+                        // NOTE: we don't use the `world_com` here because it is not
+                        //       really updated for kinematic bodies.
+                        &(rb_pos.position * rb_mprops.local_mprops.local_com),
+                    );
+                    bodies.set_internal(handle.0, RigidBodyPosition::from(new_pos));
+                }
+                _ => {}
+            }
         }
     }
 
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index 99c5cfe..699361c 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -100,7 +100,8 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
                             // the active_dynamic_set.
                             changes.set(RigidBodyChanges::SLEEP, true);
                         }
-                        RigidBodyType::Kinematic => {
+                        RigidBodyType::KinematicVelocityBased
+                        | RigidBodyType::KinematicPositionBased => {
                             // Remove from the active dynamic set if it was there.
                             if islands.active_dynamic_set.get(ids.active_set_id) == Some(&handle) {
                                 islands.active_dynamic_set.swap_remove(ids.active_set_id);