Split rigid-bodies and colliders into multiple components

1 files changed, 68 insertions, 48 deletions

diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs
index 2e4b394..8e4d9b8 100644
--- a/src/dynamics/joint/joint_set.rs
+++ b/src/dynamics/joint/joint_set.rs
@@ -2,31 +2,33 @@ use super::Joint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
 use crate::data::arena::Arena;
-use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
+use crate::dynamics::{JointParams, RigidBodyHandle};
 
 /// The unique identifier of a joint added to the joint set.
 /// The unique identifier of a collider added to a collider set.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[repr(transparent)]
-pub struct JointHandle(pub(crate) crate::data::arena::Index);
+pub struct JointHandle(pub crate::data::arena::Index);
 
 impl JointHandle {
     /// Converts this handle into its (index, generation) components.
-    pub fn into_raw_parts(self) -> (usize, u64) {
+    pub fn into_raw_parts(self) -> (u32, u32) {
         self.0.into_raw_parts()
     }
 
     /// Reconstructs an handle from its (index, generation) components.
-    pub fn from_raw_parts(id: usize, generation: u64) -> Self {
+    pub fn from_raw_parts(id: u32, generation: u32) -> Self {
         Self(crate::data::arena::Index::from_raw_parts(id, generation))
     }
 
     /// An always-invalid joint handle.
     pub fn invalid() -> Self {
         Self(crate::data::arena::Index::from_raw_parts(
-            crate::INVALID_USIZE,
-            crate::INVALID_U64,
+            crate::INVALID_U32,
+            crate::INVALID_U32,
         ))
     }
 }
@@ -157,7 +159,7 @@ impl JointSet {
     /// Inserts a new joint into this set and retrieve its handle.
     pub fn insert<J>(
         &mut self,
-        bodies: &mut RigidBodySet,
+        bodies: &mut impl ComponentSetMut<RigidBodyIds>,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
         joint_params: J,
@@ -177,57 +179,64 @@ impl JointSet {
             params: joint_params.into(),
         };
 
-        let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
-        let (rb1, rb2) = (
-            rb1.expect("Attempt to attach a joint to a non-existing body."),
-            rb2.expect("Attempt to attach a joint to a non-existing body."),
-        );
+        let mut graph_index1 = bodies.index(joint.body1.0).joint_graph_index;
+        let mut graph_index2 = bodies.index(joint.body2.0).joint_graph_index;
 
         // NOTE: the body won't have a graph index if it does not
         // have any joint attached.
-        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb1.joint_graph_index)
-        {
-            rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index1) {
+            graph_index1 = self.joint_graph.graph.add_node(joint.body1);
+            bodies.map_mut_internal(joint.body1.0, |ids| ids.joint_graph_index = graph_index1);
         }
 
-        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(rb2.joint_graph_index)
-        {
-            rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
+        if !InteractionGraph::<RigidBodyHandle, Joint>::is_graph_index_valid(graph_index2) {
+            graph_index2 = self.joint_graph.graph.add_node(joint.body2);
+            bodies.map_mut_internal(joint.body2.0, |ids| ids.joint_graph_index = graph_index2);
         }
 
-        let id = self
-            .joint_graph
-            .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
-
-        self.joint_ids[handle] = id;
+        self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint);
         JointHandle(handle)
     }
 
     /// Retrieve all the joints happening between two active bodies.
     // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
-    pub(crate) fn select_active_interactions(
+    pub(crate) fn select_active_interactions<Bodies>(
         &self,
-        bodies: &RigidBodySet,
+        islands: &IslandManager,
+        bodies: &Bodies,
         out: &mut Vec<Vec<JointIndex>>,
-    ) {
-        for out_island in &mut out[..bodies.num_islands()] {
+    ) where
+        Bodies: ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyActivation>
+            + ComponentSet<RigidBodyIds>,
+    {
+        for out_island in &mut out[..islands.num_islands()] {
             out_island.clear();
         }
 
         // FIXME: don't iterate through all the interactions.
         for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
             let joint = &edge.weight;
-            let rb1 = &bodies[joint.body1];
-            let rb2 = &bodies[joint.body2];
 
-            if (rb1.is_dynamic() || rb2.is_dynamic())
-                && (!rb1.is_dynamic() || !rb1.is_sleeping())
-                && (!rb2.is_dynamic() || !rb2.is_sleeping())
+            let (status1, activation1, ids1): (
+                &RigidBodyType,
+                &RigidBodyActivation,
+                &RigidBodyIds,
+            ) = bodies.index_bundle(joint.body1.0);
+            let (status2, activation2, ids2): (
+                &RigidBodyType,
+                &RigidBodyActivation,
+                &RigidBodyIds,
+            ) = bodies.index_bundle(joint.body2.0);
+
+            if (status1.is_dynamic() || status2.is_dynamic())
+                && (!status1.is_dynamic() || !activation1.sleeping)
+                && (!status2.is_dynamic() || !activation2.sleeping)
             {
-                let island_index = if !rb1.is_dynamic() {
-                    rb2.active_island_id
+                let island_index = if !status1.is_dynamic() {
+                    ids2.active_island_id
                 } else {
-                    rb1.active_island_id
+                    ids1.active_island_id
                 };
 
                 out[island_index].push(i);
@@ -239,22 +248,28 @@ impl JointSet {
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up.
-    pub fn remove(
+    pub fn remove<Bodies>(
         &mut self,
         handle: JointHandle,
-        bodies: &mut RigidBodySet,
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
         wake_up: bool,
-    ) -> Option<Joint> {
+    ) -> Option<Joint>
+    where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
         let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
         if wake_up {
             // Wake-up the bodies attached to this joint.
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) {
-                bodies.wake_up(*rb_handle, true);
+                islands.wake_up(bodies, *rb_handle, true);
             }
             if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) {
-                bodies.wake_up(*rb_handle, true);
+                islands.wake_up(bodies, *rb_handle, true);
             }
         }
 
@@ -267,11 +282,16 @@ impl JointSet {
         removed_joint
     }
 
-    pub(crate) fn remove_rigid_body(
+    pub(crate) fn remove_rigid_body<Bodies>(
         &mut self,
         deleted_id: RigidBodyGraphIndex,
-        bodies: &mut RigidBodySet,
-    ) {
+        islands: &mut IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>,
+    {
         if InteractionGraph::<(), ()>::is_graph_index_valid(deleted_id) {
             // We have to delete each joint one by one in order to:
             // - Wake-up the attached bodies.
@@ -292,16 +312,16 @@ impl JointSet {
                 }
 
                 // Wake up the attached bodies.
-                bodies.wake_up(h1, true);
-                bodies.wake_up(h2, true);
+                islands.wake_up(bodies, h1, true);
+                islands.wake_up(bodies, h2, true);
             }
 
             if let Some(other) = self.joint_graph.remove_node(deleted_id) {
                 // One rigid-body joint graph index may have been invalidated
                 // so we need to update it.
-                if let Some(replacement) = bodies.get_mut_internal(other) {
-                    replacement.joint_graph_index = deleted_id;
-                }
+                bodies.map_mut_internal(other.0, |ids: &mut RigidBodyIds| {
+                    ids.joint_graph_index = deleted_id;
+                });
             }
         }
     }