First round deleting the component sets.

34 files changed, 717 insertions, 1644 deletions

diff --git a/src/data/component_set.rs b/src/data/component_set.rs
deleted file mode 100644
index 6e0461c..0000000
--- a/src/data/component_set.rs
+++ /dev/null
@@ -1,123 +0,0 @@
-use crate::data::Index;
-
-// TODO ECS: use this to handle optional components properly.
-// pub trait OptionalComponentSet<T> {
-//     fn get(&self, handle: Index) -> Option<&T>;
-// }
-
-/// A set of optional elements of type `T`.
-pub trait ComponentSetOption<T>: Sync {
-    /// Get the element associated to the given `handle`, if there is one.
-    fn get(&self, handle: Index) -> Option<&T>;
-}
-
-/// A set of elements of type `T`.
-pub trait ComponentSet<T>: ComponentSetOption<T> {
-    /// The estimated number of elements in this set.
-    ///
-    /// This value is typically used for preallocating some arrays for
-    /// better performances.
-    fn size_hint(&self) -> usize;
-    // TODO ECS: remove this, its only needed by the query pipeline update
-    //           which should only take the modified colliders into account.
-    /// Iterate through all the elements on this set.
-    fn for_each(&self, f: impl FnMut(Index, &T));
-    /// Get the element associated to the given `handle`.
-    fn index(&self, handle: Index) -> &T {
-        self.get(handle).unwrap()
-    }
-}
-
-/// A set of mutable elements of type `T`.
-pub trait ComponentSetMut<T>: ComponentSet<T> {
-    /// Applies the given closure to the element associated to the given `handle`.
-    ///
-    /// Return `None` if the element doesn't exist.
-    fn map_mut_internal<Result>(
-        &mut self,
-        handle: crate::data::Index,
-        f: impl FnOnce(&mut T) -> Result,
-    ) -> Option<Result>;
-
-    /// Set the value of this element.
-    fn set_internal(&mut self, handle: crate::data::Index, val: T);
-}
-
-/// Helper trait to address multiple elements at once.
-pub trait BundleSet<'a, T> {
-    /// Access multiple elements from this set.
-    fn index_bundle(&'a self, handle: Index) -> T;
-}
-
-impl<'a, T, A, B> BundleSet<'a, (&'a A, &'a B)> for T
-where
-    T: ComponentSet<A> + ComponentSet<B>,
-{
-    #[inline(always)]
-    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B) {
-        (self.index(handle), self.index(handle))
-    }
-}
-
-impl<'a, T, A, B, C> BundleSet<'a, (&'a A, &'a B, &'a C)> for T
-where
-    T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C>,
-{
-    #[inline(always)]
-    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C) {
-        (self.index(handle), self.index(handle), self.index(handle))
-    }
-}
-
-impl<'a, T, A, B, C, D> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D)> for T
-where
-    T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C> + ComponentSet<D>,
-{
-    #[inline(always)]
-    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D) {
-        (
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-        )
-    }
-}
-
-impl<'a, T, A, B, C, D, E> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E)> for T
-where
-    T: ComponentSet<A> + ComponentSet<B> + ComponentSet<C> + ComponentSet<D> + ComponentSet<E>,
-{
-    #[inline(always)]
-    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E) {
-        (
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-        )
-    }
-}
-
-impl<'a, T, A, B, C, D, E, F> BundleSet<'a, (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F)> for T
-where
-    T: ComponentSet<A>
-        + ComponentSet<B>
-        + ComponentSet<C>
-        + ComponentSet<D>
-        + ComponentSet<E>
-        + ComponentSet<F>,
-{
-    #[inline(always)]
-    fn index_bundle(&'a self, handle: Index) -> (&'a A, &'a B, &'a C, &'a D, &'a E, &'a F) {
-        (
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-            self.index(handle),
-        )
-    }
-}
diff --git a/src/data/mod.rs b/src/data/mod.rs
index f20e433..6d9e2ce 100644
--- a/src/data/mod.rs
+++ b/src/data/mod.rs
@@ -2,10 +2,8 @@
 
 pub use self::arena::{Arena, Index};
 pub use self::coarena::Coarena;
-pub use self::component_set::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 
 pub mod arena;
 mod coarena;
-mod component_set;
 pub(crate) mod graph;
 pub mod pubsub;
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index bd3b20b..8fc5a7f 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -1,11 +1,11 @@
 use super::TOIEntry;
-use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
-use crate::dynamics::{IslandManager, RigidBodyColliders, RigidBodyForces};
 use crate::dynamics::{
-    RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
+    IslandManager, RigidBodyCcd, RigidBodyColliders, RigidBodyForces, RigidBodyHandle,
+    RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyVelocity,
 };
 use crate::geometry::{
-    ColliderParent, ColliderPosition, ColliderShape, ColliderType, CollisionEvent, NarrowPhase,
+    ColliderParent, ColliderPosition, ColliderSet, ColliderShape, ColliderType, CollisionEvent,
+    NarrowPhase,
 };
 use crate::math::Real;
 use crate::parry::utils::SortedPair;
@@ -57,13 +57,7 @@ impl CCDSolver {
     /// Apply motion-clamping to the bodies affected by the given `impacts`.
     ///
     /// The `impacts` should be the result of a previous call to `self.predict_next_impacts`.
-    pub fn clamp_motions<Bodies>(&self, dt: Real, bodies: &mut Bodies, impacts: &PredictedImpacts)
-    where
-        Bodies: ComponentSet<RigidBodyCcd>
-            + ComponentSetMut<RigidBodyPosition>
-            + ComponentSet<RigidBodyVelocity>
-            + ComponentSet<RigidBodyMassProps>,
-    {
+    pub fn clamp_motions(&self, dt: Real, bodies: &mut RigidBodySet, impacts: &PredictedImpacts) {
         match impacts {
             PredictedImpacts::Impacts(tois) => {
                 for (handle, toi) in tois {
@@ -93,18 +87,13 @@ impl CCDSolver {
     /// Updates the set of bodies that needs CCD to be resolved.
     ///
     /// Returns `true` if any rigid-body must have CCD resolved.
-    pub fn update_ccd_active_flags<Bodies>(
+    pub fn update_ccd_active_flags(
         &self,
         islands: &IslandManager,
-        bodies: &mut Bodies,
+        bodies: &mut RigidBodySet,
         dt: Real,
         include_forces: bool,
-    ) -> bool
-    where
-        Bodies: ComponentSetMut<RigidBodyCcd>
-            + ComponentSet<RigidBodyVelocity>
-            + ComponentSet<RigidBodyForces>,
-    {
+    ) -> bool {
         let mut ccd_active = false;
 
         // println!("Checking CCD activation");
@@ -128,27 +117,14 @@ impl CCDSolver {
     }
 
     /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider.
-    pub fn find_first_impact<Bodies, Colliders>(
+    pub fn find_first_impact(
         &mut self,
         dt: Real,
         islands: &IslandManager,
-        bodies: &Bodies,
-        colliders: &Colliders,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
         narrow_phase: &NarrowPhase,
-    ) -> Option<Real>
-    where
-        Bodies: ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyVelocity>
-            + ComponentSet<RigidBodyCcd>
-            + ComponentSet<RigidBodyColliders>
-            + ComponentSet<RigidBodyForces>
-            + ComponentSet<RigidBodyMassProps>,
-        Colliders: ComponentSetOption<ColliderParent>
-            + ComponentSet<ColliderPosition>
-            + ComponentSet<ColliderShape>
-            + ComponentSet<ColliderType>
-            + ComponentSet<ColliderFlags>,
-    {
+    ) -> Option<Real> {
         // Update the query pipeline.
         self.query_pipeline.update_with_mode(
             islands,
@@ -266,29 +242,15 @@ impl CCDSolver {
     }
 
     /// Outputs the set of bodies as well as their first time-of-impact event.
-    pub fn predict_impacts_at_next_positions<Bodies, Colliders>(
+    pub fn predict_impacts_at_next_positions(
         &mut self,
         dt: Real,
         islands: &IslandManager,
-        bodies: &Bodies,
-        colliders: &Colliders,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
         narrow_phase: &NarrowPhase,
         events: &dyn EventHandler,
-    ) -> PredictedImpacts
-    where
-        Bodies: ComponentSet<RigidBodyPosition>
-            + ComponentSet<RigidBodyVelocity>
-            + ComponentSet<RigidBodyCcd>
-            + ComponentSet<RigidBodyColliders>
-            + ComponentSet<RigidBodyForces>
-            + ComponentSet<RigidBodyMassProps>,
-        Colliders: ComponentSetOption<ColliderParent>
-            + ComponentSet<ColliderPosition>
-            + ComponentSet<ColliderShape>
-            + ComponentSet<ColliderType>
-            + ComponentSet<ColliderFlags>
-            + ComponentSet<ColliderFlags>,
-    {
+    ) -> PredictedImpacts {
         let mut frozen = HashMap::<_, Real>::default();
         let mut all_toi = BinaryHeap::new();
         let mut pairs_seen = HashMap::default();
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 06f3820..0cb92e9 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -1,9 +1,8 @@
-use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 use crate::dynamics::{
     ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyHandle,
-    RigidBodyIds, RigidBodyType, RigidBodyVelocity,
+    RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity,
 };
-use crate::geometry::{ColliderParent, NarrowPhase};
+use crate::geometry::{ColliderSet, NarrowPhase};
 use crate::math::Real;
 use crate::utils::WDot;
 
@@ -40,10 +39,7 @@ impl IslandManager {
     }
 
     /// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`.
-    pub fn cleanup_removed_rigid_bodies(
-        &mut self,
-        bodies: &mut impl ComponentSetMut<RigidBodyIds>,
-    ) {
+    pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) {
         let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
 
         for active_set in &mut active_sets {
@@ -69,7 +65,7 @@ impl IslandManager {
         &mut self,
         removed_handle: RigidBodyHandle,
         removed_ids: &RigidBodyIds,
-        bodies: &mut impl ComponentSetMut<RigidBodyIds>,
+        bodies: &mut RigidBodySet,
     ) {
         let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
 
@@ -77,10 +73,11 @@ impl IslandManager {
             if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
                 active_set.swap_remove(removed_ids.active_set_id);
 
-                if let Some(replacement) = active_set.get(removed_ids.active_set_id) {
-                    bodies.map_mut_internal(replacement.0, |ids| {
-                        ids.active_set_id = removed_ids.active_set_id;
-                    });
+                if let Some(replacement) = active_set
+                    .get(removed_ids.active_set_id)
+                    .and_then(|h| bodies.get_mut_internal(*h))
+                {
+                    replacement.ids.active_set_id = removed_ids.active_set_id;
                 }
             }
         }
@@ -90,17 +87,11 @@ impl IslandManager {
     ///
     /// If `strong` is `true` then it is assured that the rigid-body will
     /// remain awake during multiple subsequent timesteps.
-    pub fn wake_up<Bodies>(&mut self, bodies: &mut Bodies, handle: RigidBodyHandle, strong: bool)
-    where
-        Bodies: ComponentSetMut<RigidBodyActivation>
-            + ComponentSetOption<RigidBodyType>
-            + ComponentSetMut<RigidBodyIds>,
-    {
+    pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) {
         // NOTE: the use an Option here because there are many legitimate cases (like when
         //       deleting a joint attached to an already-removed body) where we could be
         //       attempting to wake-up a rigid-body that has already been deleted.
-        let rb_type: Option<RigidBodyType> = bodies.get(handle.0).copied();
-        if rb_type == Some(RigidBodyType::Dynamic) {
+        if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) {
             bodies.map_mut_internal(handle.0, |activation: &mut RigidBodyActivation| {
                 activation.wake_up(strong)
             });
@@ -141,23 +132,16 @@ impl IslandManager {
         self.active_islands[island_id]..self.active_islands[island_id + 1]
     }
 
-    pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>(
+    pub(crate) fn update_active_set_with_contacts(
         &mut self,
         dt: Real,
-        bodies: &mut Bodies,
-        colliders: &Colliders,
+        bodies: &mut RigidBodySet,
+        colliders: &ColliderSet,
         narrow_phase: &NarrowPhase,
         impulse_joints: &ImpulseJointSet,
         multibody_joints: &MultibodyJointSet,
         min_island_size: usize,
-    ) where
-        Bodies: ComponentSetMut<RigidBodyIds>
-            + ComponentSetMut<RigidBodyActivation>
-            + ComponentSetMut<RigidBodyVelocity>
-            + ComponentSet<RigidBodyColliders>
-            + ComponentSet<RigidBodyType>,
-        Colliders: ComponentSetOption<ColliderParent>,
-    {
+    ) {
         assert!(
             min_island_size > 0,
             "The minimum island size must be at least 1."
@@ -203,7 +187,7 @@ impl IslandManager {
         #[inline(always)]
         fn push_contacting_bodies(
             rb_colliders: &RigidBodyColliders,
-            colliders: &impl ComponentSetOption<ColliderParent>,
+            colliders: &ColliderSet,
             narrow_phase: &NarrowPhase,
             stack: &mut Vec<RigidBodyHandle>,
         ) {
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 +16