First round deleting the component sets.

3 files changed, 55 insertions, 109 deletions

diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 448b87d..6b6d980 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -2,9 +2,11 @@ use super::ImpulseJoint;
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
 
 use crate::data::arena::Arena;
-use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
-use crate::dynamics::{GenericJoint, RigidBodyHandle};
-use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
+use crate::data::Coarena;
+use crate::dynamics::{
+    GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet,
+    RigidBodyType,
+};
 
 /// The unique identifier of a joint added to the joint set.
 /// The unique identifier of a collider added to a collider set.
@@ -215,16 +217,12 @@ impl ImpulseJointSet {
 
     /// Retrieve all the impulse_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<Bodies>(
+    pub(crate) fn select_active_interactions(
         &self,
         islands: &IslandManager,
-        bodies: &Bodies,
+        bodies: &RigidBodySet,
         out: &mut Vec<Vec<JointIndex>>,
-    ) where
-        Bodies: ComponentSet<RigidBodyType>
-            + ComponentSet<RigidBodyActivation>
-            + ComponentSet<RigidBodyIds>,
-    {
+    ) {
         for out_island in &mut out[..islands.num_islands()] {
             out_island.clear();
         }
@@ -263,18 +261,13 @@ impl ImpulseJointSet {
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up.
-    pub fn remove<Bodies>(
+    pub fn remove(
         &mut self,
         handle: ImpulseJointHandle,
         islands: &mut IslandManager,
-        bodies: &mut Bodies,
+        bodies: &mut RigidBodySet,
         wake_up: bool,
-    ) -> Option<ImpulseJoint>
-    where
-        Bodies: ComponentSetMut<RigidBodyActivation>
-            + ComponentSet<RigidBodyType>
-            + ComponentSetMut<RigidBodyIds>,
-    {
+    ) -> Option<ImpulseJoint> {
         let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
 
@@ -302,17 +295,12 @@ impl ImpulseJointSet {
     /// The provided rigid-body handle is not required to identify a rigid-body that
     /// is still contained by the `bodies` component set.
     /// Returns the (now invalid) handles of the removed impulse_joints.
-    pub fn remove_joints_attached_to_rigid_body<Bodies>(
+    pub fn remove_joints_attached_to_rigid_body(
         &mut self,
         handle: RigidBodyHandle,
         islands: &mut IslandManager,
-        bodies: &mut Bodies,
-    ) -> Vec<ImpulseJointHandle>
-    where
-        Bodies: ComponentSetMut<RigidBodyActivation>
-            + ComponentSet<RigidBodyType>
-            + ComponentSetMut<RigidBodyIds>,
-    {
+        bodies: &mut RigidBodySet,
+    ) -> Vec<ImpulseJointHandle> {
         let mut deleted = vec![];
 
         if let Some(deleted_id) = self
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index c4cc85f..5bd790a 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -1,10 +1,8 @@
 use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
 use super::multibody_workspace::MultibodyWorkspace;
-use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
-use crate::dynamics::solver::AnyJointVelocityConstraint;
 use crate::dynamics::{
-    IntegrationParameters, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
-    RigidBodyType, RigidBodyVelocity,
+    solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle,
+    RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity,
 };
 #[cfg(feature = "dim3")]
 use crate::math::Matrix;
@@ -369,12 +367,7 @@ impl Multibody {
             .extend((0..num_jacobians).map(|_| Jacobian::zeros(0)));
     }
 
-    pub(crate) fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
-    where
-        Bodies: ComponentSet<RigidBodyMassProps>
-            + ComponentSet<RigidBodyForces>
-            + ComponentSet<RigidBodyVelocity>,
-    {
+    pub(crate) fn update_acceleration(&mut self, bodies: &RigidBodySet) {
         if self.ndofs == 0 {
             return; // Nothing to do.
         }
@@ -452,10 +445,7 @@ impl Multibody {
     }
 
     /// Computes the constant terms of the dynamics.
-    pub(crate) fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
-    where
-        Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
-    {
+    pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut RigidBodySet) {
         /*
          * Compute velocities.
          * NOTE: this is needed for kinematic bodies too.
@@ -545,10 +535,7 @@ impl Multibody {
         }
     }
 
-    fn update_inertias<Bodies>(&mut self, dt: Real, bodies: &Bodies)
-    where
-        Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyVelocity>,
-    {
+    fn update_inertias(&mut self, dt: Real, bodies: &RigidBodySet) {
         if self.ndofs == 0 {
             return; // Nothing to do.
         }
@@ -790,17 +777,11 @@ impl Multibody {
         }
     }
 
-    pub(crate) fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
-    where
-        Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
-    {
-        let rb_type: Option<&RigidBodyType> = bodies.get(self.links[0].rigid_body.0);
-        if let Some(rb_type) = rb_type {
-            let rb_pos: &RigidBodyPosition = bodies.index(self.links[0].rigid_body.0);
-
-            if rb_type.is_dynamic() != self.root_is_dynamic {
-                if rb_type.is_dynamic() {
-                    let free_joint = MultibodyJoint::free(rb_pos.position);
+    pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
+        if let Some(rb) = bodies.get(self.links[0].rigid_body) {
+            if rb.is_dynamic() != self.root_is_dynamic {
+                if rb.is_dynamic() {
+                    let free_joint = MultibodyJoint::free(*rb.position());
                     let prev_root_ndofs = self.links[0].joint().ndofs();
                     self.links[0].joint = free_joint;
                     self.links[0].assembly_id = 0;
@@ -819,7 +800,7 @@ impl Multibody {
                     assert!(self.damping.len() >= SPATIAL_DIM);
                     assert!(self.accelerations.len() >= SPATIAL_DIM);
 
-                    let fixed_joint = MultibodyJoint::fixed(rb_pos.position);
+                    let fixed_joint = MultibodyJoint::fixed(*rb.position());
                     let prev_root_ndofs = self.links[0].joint().ndofs();
                     self.links[0].joint = fixed_joint;
                     self.links[0].assembly_id = 0;
@@ -844,39 +825,31 @@ impl Multibody {
                     }
                 }
 
-                self.root_is_dynamic = rb_type.is_dynamic();
+                self.root_is_dynamic = rb.is_dynamic();
             }
 
             // Make sure the positions are properly set to match the rigid-body’s.
             if self.links[0].joint.data.locked_axes.is_empty() {
-                self.links[0].joint.set_free_pos(rb_pos.position);
+                self.links[0].joint.set_free_pos(*rb.position());
             } else {
-                self.links[0].joint.data.local_frame1 = rb_pos.position;
+                self.links[0].joint.data.local_frame1 = *rb.position();
             }
         }
     }
 
     /// Apply forward-kinematics to this multibody and its related rigid-bodies.
-    pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
-    where
-        Bodies: ComponentSet<RigidBodyType>
-            + ComponentSetMut<RigidBodyMassProps>
-            + ComponentSetMut<RigidBodyPosition>,
-    {
+    pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
         // Special case for the root, which has no parent.
         {
             let link = &mut self.links[0];
             link.local_to_parent = link.joint.body_to_parent();
             link.local_to_world = link.local_to_parent;
 
-            bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
-                rb_pos.next_position = link.local_to_world;
-            });
-
-            if update_mass_props {
-                bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
-                    mprops.update_world_mass_properties(&link.local_to_world)
-                });
+            if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
+                rb.pos.next_position = link.local_to_world;
+                if update_mass_props {
+                    rb.mprops.update_world_mass_properties(&link.local_to_world);
+                }
             }
         }
 
@@ -888,32 +861,30 @@ impl Multibody {
             link.local_to_world = parent_link.local_to_world * link.local_to_parent;
 
             {
-                let parent_rb_mprops: &RigidBodyMassProps = bodies.index(parent_link.rigid_body.0);
-                let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
-                let c0 = parent_link.local_to_world * parent_rb_mprops.local_mprops.local_com;
+                let parent_rb = &bodies[parent_link.rigid_body];
+                let link_rb = &bodies[link.rigid_body];
+                let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
                 let c2 = link.local_to_world
                     * Point::from(link.joint.data.local_frame2.translation.vector);
-                let c3 = link.local_to_world * rb_mprops.local_mprops.local_com;
+                let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
 
                 link.shift02 = c2 - c0;
                 link.shift23 = c3 - c2;
             }
 
-            bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
-                rb_pos.next_position = link.local_to_world;
-            });
+            let link_rb = bodies.index_mut_internal(link.rigid_body);
+            link_rb.pos.next_position = link.local_to_world;
 
-            let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0);
             assert_eq!(
-                *rb_type,
+                link_rb.body_type,
                 RigidBodyType::Dynamic,
                 "A rigid-body that is not at the root of a multibody must be dynamic."
             );
 
             if update_mass_props {
-                bodies.map_mut_internal(link.rigid_body.0, |rb_mprops: &mut RigidBodyMassProps| {
-                    rb_mprops.update_world_mass_properties(&link.local_to_world)
-                });
+                link_rb
+                    .mprops
+                    .update_world_mass_properties(&link.local_to_world);
             }
         }
 
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index 0365062..748530f 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -1,8 +1,7 @@
-use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
+use crate::data::{Arena, Coarena, Index};
 use crate::dynamics::joint::MultibodyLink;
 use crate::dynamics::{
-    GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
-    RigidBodyIds, RigidBodyType,
+    GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet,
 };
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
 use crate::parry::partitioning::IndexedData;
@@ -163,17 +162,13 @@ impl MultibodyJointSet {
     }
 
     /// Removes an multibody_joint from this set.
-    pub fn remove<Bodies>(
+    pub fn remove(
         &mut self,
         handle: MultibodyJointHandle,
         islands: &mut IslandManager,
-        bodies: &mut Bodies,
+        bodies: &mut RigidBodySet,
         wake_up: bool,
-    ) where
-        Bodies: ComponentSetMut<RigidBodyActivation>
-            + ComponentSet<RigidBodyType>
-            + ComponentSetMut<RigidBodyIds>,
-    {
+    ) {
         if let Some(removed) = self.rb2mb.get(handle.0).copied() {
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
 
@@ -216,17 +211,13 @@ impl MultibodyJointSet {
     }
 
     /// Removes all the multibody_joints from the multibody the given rigid-body is part of.
-    pub fn remove_multibody_articulations<Bodies>(
+    pub fn remove_multibody_articulations(
         &mut self,
         handle: RigidBodyHandle,
         islands: &mut IslandManager,
-        bodies: &mut Bodies,
+        bodies: &mut RigidBodySet,
         wake_up: bool,
-    ) where
-        Bodies: ComponentSetMut<RigidBodyActivation>
-            + ComponentSet<RigidBodyType>
-            + ComponentSetMut<RigidBodyIds>,
-    {
+    ) {
         if let Some(removed) = self.rb2mb.get(handle.0).copied() {
             // Remove the multibody.
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -248,16 +239,12 @@ impl MultibodyJointSet {
     }
 
     /// Removes all the multibody joints attached to a rigid-body.
-    pub fn remove_joints_attached_to_rigid_body<Bodies>(
+    pub fn remove_joints_attached_to_rigid_body(
         &mut self,
         rb_to_remove: RigidBodyHandle,
         islands: &mut IslandManager,
-        bodies: &mut Bodies,
-    ) where
-        Bodies: ComponentSetMut<RigidBodyActivation>
-            + ComponentSet<RigidBodyType>
-            + ComponentSetMut<RigidBodyIds>,
-    {
+        bodies: &mut RigidBodySet,
+    ) {
         // TODO: optimize this.
         if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
             let mut articulations_to_remove = vec![];