Finalize refactoring

4 files changed, 95 insertions, 157 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index 48a11e7..872206b 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -97,7 +97,7 @@ impl CollisionPipeline {
         modified_colliders: &mut Vec<ColliderHandle>,
     ) {
         for handle in modified_colliders.drain(..) {
-            colliders.set_internal(handle.0, ColliderChanges::empty())
+            colliders.index_mut_internal(handle).changes = ColliderChanges::empty();
         }
     }
 
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index ffcb25f..71c559a 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -5,8 +5,7 @@ use crate::counters::Counters;
 use crate::dynamics::IslandSolver;
 use crate::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
-    RigidBodyColliders, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
-    RigidBodyType, RigidBodyVelocity,
+    RigidBodyHandle, RigidBodyPosition, RigidBodyType,
 };
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
@@ -72,7 +71,9 @@ impl PhysicsPipeline {
         modified_colliders: &mut Vec<ColliderHandle>,
     ) {
         for handle in modified_colliders.drain(..) {
-            colliders.set_internal(handle.0, ColliderChanges::empty())
+            if let Some(co) = colliders.get_mut_internal(handle) {
+                co.changes = ColliderChanges::empty();
+            }
         }
     }
 
@@ -187,18 +188,11 @@ impl PhysicsPipeline {
 
         self.counters.stages.update_time.resume();
         for handle in islands.active_dynamic_bodies() {
-            let poss: &RigidBodyPosition = bodies.index(handle.0);
-            let position = poss.position;
-
-            let effective_inv_mass = bodies
-                .map_mut_internal(handle.0, |mprops: &mut RigidBodyMassProps| {
-                    mprops.update_world_mass_properties(&position);
-                    mprops.effective_mass()
-                })
-                .unwrap();
-            bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| {
-                forces.compute_effective_force_and_torque(&gravity, &effective_inv_mass)
-            });
+            let rb = bodies.index_mut_internal(*handle);
+            rb.mprops.update_world_mass_properties(&rb.pos.position);
+            let effective_mass = rb.mprops.effective_mass();
+            rb.forces
+                .compute_effective_force_and_torque(&gravity, &effective_mass);
         }
 
         for multibody in &mut multibody_joints.multibodies {
@@ -319,13 +313,10 @@ impl PhysicsPipeline {
     ) {
         // Set the rigid-bodies and kinematic bodies to their final position.
         for handle in islands.iter_active_bodies() {
-            bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| {
-                poss.position = poss.next_position
-            });
-
-            let (rb_poss, rb_colls): (&RigidBodyPosition, &RigidBodyColliders) =
-                bodies.index_bundle(handle.0);
-            rb_colls.update_positions(colliders, modified_colliders, &rb_poss.position);
+            let rb = bodies.index_mut_internal(handle);
+            rb.pos.position = rb.pos.next_position;
+            rb.colliders
+                .update_positions(colliders, modified_colliders, &rb.pos.position);
         }
     }
 
@@ -341,29 +332,22 @@ 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 (rb_type, rb_pos, rb_vel, rb_mprops): (
-                &RigidBodyType,
-                &RigidBodyPosition,
-                &RigidBodyVelocity,
-                &RigidBodyMassProps,
-            ) = bodies.index_bundle(handle.0);
-
-            match rb_type {
+            let rb = bodies.index_mut_internal(*handle);
+
+            match rb.body_type {
                 RigidBodyType::KinematicPositionBased => {
-                    let rb_pos: &RigidBodyPosition = bodies.index(handle.0);
-                    let new_vel = rb_pos.interpolate_velocity(
+                    rb.vels = rb.pos.interpolate_velocity(
                         integration_parameters.inv_dt(),
-                        &rb_mprops.local_mprops.local_com,
+                        &rb.mprops.local_mprops.local_com,
                     );
-                    bodies.set_internal(handle.0, new_vel);
                 }
                 RigidBodyType::KinematicVelocityBased => {
-                    let new_pos = rb_vel.integrate(
+                    let new_pos = rb.vels.integrate(
                         integration_parameters.dt,
-                        &rb_pos.position,
-                        &rb_mprops.local_mprops.local_com,
+                        &rb.pos.position,
+                        &rb.mprops.local_mprops.local_com,
                     );
-                    bodies.set_internal(handle.0, RigidBodyPosition::from(new_pos));
+                    rb.pos = RigidBodyPosition::from(new_pos);
                 }
                 _ => {}
             }
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 584989e..3735d12 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,10 +1,6 @@
-use crate::dynamics::{
-    IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
-    RigidBodyVelocity,
-};
+use crate::dynamics::IslandManager;
 use crate::geometry::{
-    ColliderFlags, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape,
-    InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
+    ColliderHandle, InteractionGroups, PointProjection, Ray, RayIntersection, AABB, QBVH,
 };
 use crate::math::{Isometry, Point, Real, Vector};
 use parry::partitioning::QBVHDataGenerator;
@@ -75,7 +71,7 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> {
             if co.flags.collision_groups.test(self.query_groups)
                 && self.filter.map(|f| f(shape_id)).unwrap_or(true)
             {
-                f(Some(co.pos), &**co.shape)
+                f(Some(&co.pos), &*co.shape)
             }
         }
     }
@@ -189,54 +185,35 @@ impl QueryPipeline {
             fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) {
                 match self.mode {
                     QueryPipelineMode::CurrentPosition => {
-                        self.colliders.for_each(|h, co_shape: &ColliderShape| {
-                            let co_pos: &ColliderPosition = self.colliders.index(h);
-                            f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
-                        })
+                        for (h, co) in self.colliders.iter() {
+                            f(h, co.shape.compute_aabb(&co.pos))
+                        }
                     }
                     QueryPipelineMode::SweepTestWithNextPosition => {
-                        self.colliders.for_each(|h, co_shape: &ColliderShape| {
-                            let co_parent: Option<&ColliderParent> = self.colliders.get(h);
-                            let co_pos: &ColliderPosition = self.colliders.index(h);
-
-                            if let Some(co_parent) = co_parent {
-                                let rb_pos: &RigidBodyPosition =
-                                    self.bodies.index(co_parent.handle.0);
-                                let next_position = rb_pos.next_position * co_parent.pos_wrt_parent;
-
-                                f(
-                                    ColliderHandle(h),
-                                    co_shape.compute_swept_aabb(&co_pos, &next_position),
-                                )
+                        for (h, co) in self.colliders.iter() {
+                            if let Some(co_parent) = co.parent {
+                                let rb_next_pos = &self.bodies[co_parent.handle].pos.next_position;
+                                let next_position = rb_next_pos * co_parent.pos_wrt_parent;
+                                f(h, co.shape.compute_swept_aabb(&co.pos, &next_position))
                             } else {
-                                f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+                                f(h, co.shape.compute_aabb(&co.pos))
                             }
-                        })
+                        }
                     }
                     QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
-                        self.colliders.for_each(|h, co_shape: &ColliderShape| {
-                            let co_parent: Option<&ColliderParent> = self.colliders.get(h);
-                            let co_pos: &ColliderPosition = self.colliders.index(h);
-
-                            if let Some(co_parent) = co_parent {
-                                let (rb_pos, vels, forces, mprops): (
-                                    &RigidBodyPosition,
-                                    &RigidBodyVelocity,
-                                    &RigidBodyForces,
-                                    &RigidBodyMassProps,
-                                ) = self.bodies.index_bundle(co_parent.handle.0);
-                                let predicted_pos = rb_pos
-                                    .integrate_forces_and_velocities(dt, forces, vels, mprops);
+                        for (h, co) in self.colliders.iter() {
+                            if let Some(co_parent) = co.parent {
+                                let rb = &self.bodies[co_parent.handle];
+                                let predicted_pos = rb.pos.integrate_forces_and_velocities(
+                                    dt, &rb.forces, &rb.vels, &rb.mprops,
+                                );
 
                                 let next_position = predicted_pos * co_parent.pos_wrt_parent;
-                                f(
-                                    ColliderHandle(h),
-                                    co_shape.compute_swept_aabb(&co_pos, &next_position),
-                                )
+                                f(h, co.shape.compute_swept_aabb(&co.pos, &next_position))
                             } else {
-                                f(ColliderHandle(h), co_shape.compute_aabb(&co_pos))
+                                f(h, co.shape.compute_aabb(&co.pos))
                             }
-                        })
+                        }
                     }
                 }
             }
@@ -256,8 +233,8 @@ impl QueryPipeline {
         }
 
         for handle in islands.iter_active_bodies() {
-            let body_colliders: &RigidBodyColliders = bodies.index(handle.0);
-            for handle in &body_colliders.0 {
+            let rb = &bodies[handle];
+            for handle in &rb.colliders.0 {
                 self.qbvh.pre_update(*handle)
             }
         }
@@ -266,9 +243,8 @@ impl QueryPipeline {
             QueryPipelineMode::CurrentPosition => {
                 self.qbvh.update(
                     |handle| {
-                        let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) =
-                            colliders.index_bundle(handle.0);
-                        co_shape.compute_aabb(&co_pos)
+                        let co = &colliders[*handle];
+                        co.shape.compute_aabb(&co.pos)
                     },
                     self.dilation_factor,
                 );
@@ -276,10 +252,10 @@ impl QueryPipeline {
             QueryPipelineMode::SweepTestWithNextPosition => {
                 self.qbvh.update(
                     |handle| {
-                        let co = &colliders[handle];
-                        if let Some(parent) = co.parent {
-                            let rb_pos: &RigidBodyPosition = bodies.index(co.parent.handle.0);
-                            let next_position = rb_pos.next_position * co.parent.pos_wrt_parent;
+                        let co = &colliders[*handle];
+                        if let Some(parent) = &co.parent {
+                            let rb_next_pos = &bodies[parent.handle].pos.next_position;
+                            let next_position = rb_next_pos * parent.pos_wrt_parent;
                             co.shape.compute_swept_aabb(&co.pos, &next_position)
                         } else {
                             co.shape.compute_aabb(&co.pos)
@@ -291,12 +267,12 @@ impl QueryPipeline {
             QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
                 self.qbvh.update(
                     |handle| {
-                        let co = &colliders[handle];
+                        let co = &colliders[*handle];
                         if let Some(parent) = co.parent {
                             let rb = &bodies[parent.handle];
-                            let predicted_pos = rb
-                                .pos
-                                .integrate_forces_and_velocities(dt, rb.forces, rb.vels, rb.mprops);
+                            let predicted_pos = rb.pos.integrate_forces_and_velocities(
+                                dt, &rb.forces, &rb.vels, &rb.mprops,
+                            );
 
                             let next_position = predicted_pos * parent.pos_wrt_parent;
                             co.shape.compute_swept_aabb(&co.pos, &next_position)
@@ -394,7 +370,7 @@ impl QueryPipeline {
     /// * `callback`: function executed on each collider for which a ray intersection has been found.
     ///               There is no guarantees on the order the results will be yielded. If this callback returns `false`,
     ///               this method will exit early, ignore any further raycast.
-    pub fn intersections_with_ray<'a, ColliderSet>(
+    pub fn intersections_with_ray<'a>(
         &self,
         colliders: &'a ColliderSet,
         ray: &Ray,
@@ -405,14 +381,13 @@ impl QueryPipeline {
         mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool,
     ) {
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
-            let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
-            if let Some(co_shape) = co_shape {
-                let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) =
-                    colliders.index_bundle(handle.0);
-                if co_flags.collision_groups.test(query_groups)
+            if let Some(co) = colliders.get(*handle) {
+                if co.flags.collision_groups.test(query_groups)
                     && filter.map(|f| f(*handle)).unwrap_or(true)
                 {
-                    if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid)
+                    if let Some(hit) = co
+                        .shape
+                        .cast_ray_and_get_normal(&co.pos, ray, max_toi, solid)
                     {
                         return callback(*handle, hit);
                     }
@@ -502,7 +477,7 @@ impl QueryPipeline {
     ///             is either `None` or returns `true`.
     /// * `callback` - A function called with each collider with a shape
     ///                containing the `point`.
-    pub fn intersections_with_point<'a, ColliderSet>(
+    pub fn intersections_with_point<'a>(
         &self,
         colliders: &'a ColliderSet,
         point: &Point<Real>,
@@ -511,15 +486,10 @@ impl QueryPipeline {
         mut callback: impl FnMut(ColliderHandle) -> bool,
     ) {
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
-            let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
-
-            if let Some(co_shape) = co_shape {
-                let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) =
-                    colliders.index_bundle(handle.0);
-
-                if co_flags.collision_groups.test(query_groups)
+            if let Some(co) = colliders.get(*handle) {
+                if co.flags.collision_groups.test(query_groups)
                     && filter.map(|f| f(*handle)).unwrap_or(true)
-                    && co_shape.contains_point(co_pos, point)
+                    && co.shape.contains_point(&co.pos, point)
                 {
                     return callback(*handle);
                 }
@@ -677,7 +647,7 @@ impl QueryPipeline {
     ///             its `contact_group` is compatible with the `query_groups`, and if this `filter`
     ///             is either `None` or returns `true`.
     /// * `callback` - A function called with the handles of each collider intersecting the `shape`.
-    pub fn intersections_with_shape<'a, ColliderSet>(
+    pub fn intersections_with_shape<'a>(
         &self,
         colliders: &'a ColliderSet,
         shape_pos: &Isometry<Real>,
@@ -690,18 +660,13 @@ impl QueryPipeline {
         let inv_shape_pos = shape_pos.inverse();
 
         let mut leaf_callback = &mut |handle: &ColliderHandle| {
-            let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
-
-            if let Some(co_shape) = co_shape {
-                let (co_flags, co_pos): (&ColliderFlags, &ColliderPosition) =
-                    colliders.index_bundle(handle.0);
-
-                if co_flags.collision_groups.test(query_groups)
+            if let Some(co) = colliders.get(*handle) {
+                if co.flags.collision_groups.test(query_groups)
                     && filter.map(|f| f(*handle)).unwrap_or(true)
                 {
-                    let pos12 = inv_shape_pos * co_pos.as_ref();
+                    let pos12 = inv_shape_pos * co.pos.as_ref();
 
-                    if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) {
+                    if dispatcher.intersection_test(&pos12, shape, &*co.shape) == Ok(true) {
                         return callback(*handle);
                     }
                 }
diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs
index b999f0f..69f29d9 100644
--- a/src/pipeline/user_changes.rs
+++ b/src/pipeline/user_changes.rs
@@ -1,6 +1,6 @@
 use crate::dynamics::{
     IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyHandle, RigidBodyIds,
-    RigidBodyPosition, RigidBodySet, RigidBodyType,
+    RigidBodySet, RigidBodyType,
 };
 use crate::geometry::{ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet};
 use parry::utils::hashmap::HashMap;
@@ -17,15 +17,13 @@ pub(crate) fn handle_user_changes_to_colliders(
     for handle in modified_colliders {
         // NOTE: we use `get` because the collider may no longer
         //       exist if it has been removed.
-        if let Some(co) = colliders.get(*handle) {
+        if let Some(co) = colliders.get_mut_internal(*handle) {
             if co.changes.contains(ColliderChanges::PARENT) {
                 if let Some(co_parent) = co.parent {
-                    let parent_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0);
+                    let parent_rb = &bodies[co_parent.handle];
 
-                    let new_pos = parent_pos.position * co_parent.pos_wrt_parent;
-                    let new_changes = co.changes | ColliderChanges::POSITION;
-                    colliders.set_internal(handle.0, ColliderPosition(new_pos));
-                    colliders.set_internal(handle.0, new_changes);
+                    co.pos = ColliderPosition(parent_rb.pos.position * co_parent.pos_wrt_parent);
+                    co.changes |= ColliderChanges::POSITION;
                 }
             }
 
@@ -38,18 +36,12 @@ pub(crate) fn handle_user_changes_to_colliders(
     }
 
     for (to_update, _) in mprops_to_update {
-        let rb = &bodies[to_update];
-        let position = rb.position();
-        // FIXME: remove the clone once we remove the ComponentSets.
-        let attached_colliders = rb.colliders().clone();
-
-        bodies.map_mut_internal(to_update.0, |rb_mprops| {
-            rb_mprops.recompute_mass_properties_from_colliders(
-                colliders,
-                &attached_colliders,
-                &position,
-            )
-        });
+        let rb = bodies.index_mut_internal(to_update);
+        rb.mprops.recompute_mass_properties_from_colliders(
+            colliders,
+            &rb.colliders,
+            &rb.pos.position,
+        );
     }
 }
 
@@ -73,7 +65,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
             continue;
         }
 
-        let rb = &bodies[handle];
+        let rb = bodies.index_mut_internal(*handle);
         let mut changes = rb.changes;
         let mut ids: RigidBodyIds = rb.ids;
         let mut activation: RigidBodyActivation = rb.activation;
@@ -83,7 +75,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
             // it is on the correct active set.
             if let Some(islands) = islands.as_deref_mut() {
                 if changes.contains(RigidBodyChanges::TYPE) {
-                    match rb.status {
+                    match rb.body_type {
                         RigidBodyType::Dynamic => {
                             // Remove from the active kinematic set if it was there.
                             if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
@@ -162,20 +154,19 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                 || changes.contains(RigidBodyChanges::TYPE)
             {
                 for handle in rb.colliders.0.iter() {
-                    colliders.map_mut_internal(handle.0, |co_changes: &mut ColliderChanges| {
-                        if !co_changes.contains(ColliderChanges::MODIFIED) {
-                            modified_colliders.push(*handle);
-                        }
+                    let co = colliders.index_mut_internal(*handle);
+                    if !co.changes.contains(ColliderChanges::MODIFIED) {
+                        modified_colliders.push(*handle);
+                    }
 
-                        *co_changes |=
-                            ColliderChanges::MODIFIED | ColliderChanges::PARENT_EFFECTIVE_DOMINANCE;
-                    });
+                    co.changes |=
+                        ColliderChanges::MODIFIED | ColliderChanges::PARENT_EFFECTIVE_DOMINANCE;
                 }
             }
 
-            bodies.set_internal(handle.0, RigidBodyChanges::empty());
-            bodies.set_internal(handle.0, ids);
-            bodies.set_internal(handle.0, activation);
+            rb.changes = RigidBodyChanges::empty();
+            rb.ids = ids;
+            rb.activation = activation;
         }
 
         // Adjust some ids, if needed.
@@ -187,9 +178,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
                 };
 
                 if id < active_set.len() {
-                    bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| {
-                        ids2.active_set_id = id;
-                    });
+                    bodies.index_mut_internal(active_set[id]).ids.active_set_id = id;
                 }
             }
         }