Merge pull request #183 from dimforge/bundles

Make Rapier accept any kind of data storage instead of RigidBodySet/ColliderSet

1 files changed, 96 insertions, 50 deletions

diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 6b8de5a..c6baea1 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -1,24 +1,32 @@
-use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::data::ComponentSet;
+#[cfg(feature = "parallel")]
+use crate::dynamics::RigidBodyHandle;
+use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use {
+    crate::data::BundleSet,
     crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
     vec_map::VecMap,
 };
-
+#[cfg(feature = "parallel")]
 pub(crate) trait PairInteraction {
-    fn body_pair(&self) -> BodyPair;
+    fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>);
 }
+#[cfg(any(feature = "parallel", feature = "simd-is-enabled"))]
+use crate::dynamics::RigidBodyType;
 
+#[cfg(feature = "parallel")]
 impl<'a> PairInteraction for &'a mut ContactManifold {
-    fn body_pair(&self) -> BodyPair {
-        self.data.body_pair
+    fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>) {
+        (self.data.rigid_body1, self.data.rigid_body2)
     }
 }
 
+#[cfg(feature = "parallel")]
 impl<'a> PairInteraction for JointGraphEdge {
-    fn body_pair(&self) -> BodyPair {
-        BodyPair::new(self.weight.body1, self.weight.body2)
+    fn body_pair(&self) -> (Option<RigidBodyHandle>, Option<RigidBodyHandle>) {
+        (Some(self.weight.body1), Some(self.weight.body2))
     }
 }
 
@@ -51,14 +59,17 @@ impl ParallelInteractionGroups {
         self.groups.len() - 1
     }
 
-    pub fn group_interactions<Interaction: PairInteraction>(
+    pub fn group_interactions<Bodies, Interaction: PairInteraction>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[Interaction],
         interaction_indices: &[usize],
-    ) {
-        let num_island_bodies = bodies.active_island(island_id).len();
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSet<RigidBodyType>,
+    {
+        let num_island_bodies = islands.active_island(island_id).len();
         self.bodies_color.clear();
         self.interaction_indices.clear();
         self.groups.clear();
@@ -78,29 +89,39 @@ impl ParallelInteractionGroups {
             .zip(self.interaction_colors.iter_mut())
         {
             let body_pair = interactions[*interaction_id].body_pair();
-            let rb1 = &bodies[body_pair.body1];
-            let rb2 = &bodies[body_pair.body2];
-
-            match (rb1.is_static(), rb2.is_static()) {
+            let is_static1 = body_pair
+                .0
+                .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
+                .unwrap_or(true);
+            let is_static2 = body_pair
+                .1
+                .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
+                .unwrap_or(true);
+
+            match (is_static1, is_static2) {
                 (false, false) => {
+                    let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
+                    let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
                     let color_mask =
-                        bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset];
+                        bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset];
                     *color = (!color_mask).trailing_zeros() as usize;
                     color_len[*color] += 1;
-                    bcolors[rb1.active_set_offset] |= 1 << *color;
-                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids1.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids2.active_set_offset] |= 1 << *color;
                 }
                 (true, false) => {
-                    let color_mask = bcolors[rb2.active_set_offset];
+                    let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
+                    let color_mask = bcolors[rb_ids2.active_set_offset];
                     *color = (!color_mask).trailing_zeros() as usize;
                     color_len[*color] += 1;
-                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids2.active_set_offset] |= 1 << *color;
                 }
                 (false, true) => {
-                    let color_mask = bcolors[rb1.active_set_offset];
+                    let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
+                    let color_mask = bcolors[rb_ids1.active_set_offset];
                     *color = (!color_mask).trailing_zeros() as usize;
                     color_len[*color] += 1;
-                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                    bcolors[rb_ids1.active_set_offset] |= 1 << *color;
                 }
                 (true, true) => unreachable!(),
             }
@@ -168,14 +189,15 @@ impl InteractionGroups {
         self.nongrouped_interactions.clear();
     }
 
-    // FIXME: there is a lot of duplicated code with group_manifolds here.
+    // TODO: there is a lot of duplicated code with group_manifolds here.
     // But we don't refactor just now because we may end up with distinct
     // grouping strategies in the future.
     #[cfg(not(feature = "simd-is-enabled"))]
     pub fn group_joints(
         &mut self,
         _island_id: usize,
-        _bodies: &RigidBodySet,
+        _islands: &IslandManager,
+        _bodies: &impl ComponentSet<RigidBodyIds>,
         _interactions: &[JointGraphEdge],
         interaction_indices: &[JointIndex],
     ) {
@@ -184,13 +206,16 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn group_joints(
+    pub fn group_joints<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[JointGraphEdge],
         interaction_indices: &[JointIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         // NOTE: in 3D we have up to 10 different joint types.
         // In 2D we only have 5 joint types.
         #[cfg(feature = "dim3")]
@@ -204,11 +229,11 @@ impl InteractionGroups {
 
         // Note: each bit of a body mask indicates what bucket already contains
         // a constraints involving this body.
-        // FIXME: currently, this is a bit overconservative because when a bucket
+        // TODO: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped constraints.
         self.body_masks
-            .resize(bodies.active_island(island_id).len(), 0u128);
+            .resize(islands.active_island(island_id).len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -216,10 +241,14 @@ impl InteractionGroups {
 
         for interaction_i in interaction_indices {
             let interaction = &interactions[*interaction_i].weight;
-            let body1 = &bodies[interaction.body1];
-            let body2 = &bodies[interaction.body2];
-            let is_static1 = !body1.is_dynamic();
-            let is_static2 = !body2.is_dynamic();
+
+            let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
+                bodies.index_bundle(interaction.body1.0);
+            let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
+                bodies.index_bundle(interaction.body2.0);
+
+            let is_static1 = !status1.is_dynamic();
+            let is_static2 = !status2.is_dynamic();
 
             if is_static1 && is_static2 {
                 continue;
@@ -232,8 +261,8 @@ impl InteractionGroups {
             }
 
             let ijoint = interaction.params.type_id();
-            let i1 = body1.active_set_offset;
-            let i2 = body2.active_set_offset;
+            let i1 = ids1.active_set_offset;
+            let i2 = ids2.active_set_offset;
             let conflicts =
                 self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
             let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
@@ -325,7 +354,8 @@ impl InteractionGroups {
     pub fn group_manifolds(
         &mut self,
         _island_id: usize,
-        _bodies: &RigidBodySet,
+        _islands: &IslandManager,
+        _bodies: &impl ComponentSet<RigidBodyIds>,
         _interactions: &[&mut ContactManifold],
         interaction_indices: &[ContactManifoldIndex],
     ) {
@@ -334,21 +364,24 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
-    pub fn group_manifolds(
+    pub fn group_manifolds<Bodies>(
         &mut self,
         island_id: usize,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         interactions: &[&mut ContactManifold],
         interaction_indices: &[ContactManifoldIndex],
-    ) {
+    ) where
+        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
+    {
         // Note: each bit of a body mask indicates what bucket already contains
         // a constraints involving this body.
-        // FIXME: currently, this is a bit overconservative because when a bucket
+        // TODO: currently, this is a bit overconservative because when a bucket
         // is full, we don't clear the corresponding body mask bit. This may result
         // in less grouped contacts.
         // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
         self.body_masks
-            .resize(bodies.active_island(island_id).len(), 0u128);
+            .resize(islands.active_island(island_id).len(), 0u128);
 
         // NOTE: each bit of the occupied mask indicates what bucket already
         // contains at least one constraint.
@@ -359,31 +392,44 @@ impl InteractionGroups {
             .max()
             .unwrap_or(1);
 
-        // FIXME: find a way to reduce the number of iteration.
+        // TODO: find a way to reduce the number of iteration.
         // There must be a way to iterate just once on every interaction indices
         // instead of MAX_MANIFOLD_POINTS times.
         for k in 1..=max_interaction_points {
             for interaction_i in interaction_indices {
                 let interaction = &interactions[*interaction_i];
 
-                // FIXME: how could we avoid iterating
+                // TODO: how could we avoid iterating
                 // on each interaction at every iteration on k?
                 if interaction.data.num_active_contacts() != k {
                     continue;
                 }
 
-                let body1 = &bodies[interaction.data.body_pair.body1];
-                let body2 = &bodies[interaction.data.body_pair.body2];
-                let is_static1 = !body1.is_dynamic();
-                let is_static2 = !body2.is_dynamic();
+                let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
+                {
+                    let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
+                    (*data.0, data.1.active_set_offset)
+                } else {
+                    (RigidBodyType::Static, 0)
+                };
+                let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
+                {
+                    let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
+                    (*data.0, data.1.active_set_offset)
+                } else {
+                    (RigidBodyType::Static, 0)
+                };
+
+                let is_static1 = !status1.is_dynamic();
+                let is_static2 = !status2.is_dynamic();
 
-                // FIXME: don't generate interactions between static bodies in the first place.
+                // TODO: don't generate interactions between static bodies in the first place.
                 if is_static1 && is_static2 {
                     continue;
                 }
 
-                let i1 = body1.active_set_offset;
-                let i2 = body2.active_set_offset;
+                let i1 = active_set_offset1;
+                let i2 = active_set_offset2;
                 let conflicts = self.body_masks[i1] | self.body_masks[i2];
                 let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
                 let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;