Split rigid-bodies and colliders into multiple components

6 files changed, 820 insertions, 271 deletions

diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs
index da572f3..074ba75 100644
--- a/src/pipeline/collision_pipeline.rs
+++ b/src/pipeline/collision_pipeline.rs
@@ -1,7 +1,11 @@
 //! Physics pipeline structures.
 
-use crate::dynamics::{JointSet, RigidBodySet};
-use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
+use crate::data::{ComponentSet, ComponentSetMut};
+use crate::dynamics::{
+    IslandManager, JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance,
+    RigidBodyIds, RigidBodyType, RigidBodyVelocity,
+};
+use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderShape, NarrowPhase};
 use crate::math::Real;
 use crate::pipeline::{EventHandler, PhysicsHooks};
 
@@ -34,46 +38,25 @@ impl CollisionPipeline {
     }
 
     /// Executes one step of the collision detection.
-    pub fn step(
+    pub fn step<Bodies, Colliders>(
         &mut self,
-        prediction_distance: Real,
-        broad_phase: &mut BroadPhase,
-        narrow_phase: &mut NarrowPhase,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
-        hooks: &dyn PhysicsHooks,
-        events: &dyn EventHandler,
-    ) {
-        colliders.handle_user_changes(bodies);
-        bodies.handle_user_changes(colliders);
-        self.broadphase_collider_pairs.clear();
-
-        self.broad_phase_events.clear();
-        broad_phase.update(prediction_distance, colliders, &mut self.broad_phase_events);
-
-        narrow_phase.handle_user_changes(colliders, bodies, events);
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
-        narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events);
-        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
-
-        bodies.update_active_set_with_contacts(
-            colliders,
-            narrow_phase,
-            self.empty_joints.joint_graph(),
-            128,
-        );
-
-        // Update colliders positions and kinematic bodies positions.
-        bodies.foreach_active_body_mut_internal(|_, rb| {
-            rb.position = rb.next_position;
-            rb.update_colliders_positions(colliders);
-
-            for handle in &rb.colliders {
-                let collider = colliders.get_mut_internal(*handle).unwrap();
-                collider.position = rb.position * collider.delta;
-            }
-        });
-
-        bodies.modified_inactive_set.clear();
+        _prediction_distance: Real,
+        _broad_phase: &mut BroadPhase,
+        _narrow_phase: &mut NarrowPhase,
+        _islands: &mut IslandManager,
+        _bodies: &mut Bodies,
+        _colliders: &mut Colliders,
+        _hooks: &dyn PhysicsHooks<Bodies, Colliders>,
+        _events: &dyn EventHandler,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSet<RigidBodyDominance>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetMut<ColliderShape>,
+    {
+        unimplemented!()
     }
 }
diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs
index 5fad9bc..ad288d6 100644
--- a/src/pipeline/mod.rs
+++ b/src/pipeline/mod.rs
@@ -13,3 +13,4 @@ mod event_handler;
 mod physics_hooks;
 mod physics_pipeline;
 mod query_pipeline;
+mod user_changes;
diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs
index 72b635f..c4ef245 100644
--- a/src/pipeline/physics_hooks.rs
+++ b/src/pipeline/physics_hooks.rs
@@ -1,38 +1,38 @@
-use crate::dynamics::RigidBody;
-use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags};
+use crate::dynamics::RigidBodyHandle;
+use crate::geometry::{ColliderHandle, ContactManifold, SolverContact, SolverFlags};
 use crate::math::{Real, Vector};
 use na::ComplexField;
 
 /// Context given to custom collision filters to filter-out collisions.
-pub struct PairFilterContext<'a> {
-    /// The first collider involved in the potential collision.
-    pub rigid_body1: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub rigid_body2: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub collider_handle1: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider_handle2: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider1: &'a Collider,
-    /// The first collider involved in the potential collision.
-    pub collider2: &'a Collider,
+pub struct PairFilterContext<'a, Bodies, Colliders> {
+    /// The set of rigid-bodies.
+    pub bodies: &'a Bodies,
+    /// The set of colliders.
+    pub colliders: &'a Colliders,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider1: ColliderHandle,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider2: ColliderHandle,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body1: Option<RigidBodyHandle>,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body2: Option<RigidBodyHandle>,
 }
 
 /// Context given to custom contact modifiers to modify the contacts seen by the constraints solver.
-pub struct ContactModificationContext<'a> {
-    /// The first collider involved in the potential collision.
-    pub rigid_body1: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub rigid_body2: &'a RigidBody,
-    /// The first collider involved in the potential collision.
-    pub collider_handle1: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider_handle2: ColliderHandle,
-    /// The first collider involved in the potential collision.
-    pub collider1: &'a Collider,
-    /// The first collider involved in the potential collision.
-    pub collider2: &'a Collider,
+pub struct ContactModificationContext<'a, Bodies, Colliders> {
+    /// The set of rigid-bodies.
+    pub bodies: &'a Bodies,
+    /// The set of colliders.
+    pub colliders: &'a Colliders,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider1: ColliderHandle,
+    /// The handle of the first collider involved in the potential collision.
+    pub collider2: ColliderHandle,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body1: Option<RigidBodyHandle>,
+    /// The handle of the first body involved in the potential collision.
+    pub rigid_body2: Option<RigidBodyHandle>,
     /// The contact manifold.
     pub manifold: &'a ContactManifold,
     /// The solver contacts that can be modified.
@@ -45,7 +45,7 @@ pub struct ContactModificationContext<'a> {
     pub user_data: &'a mut u32,
 }
 
-impl<'a> ContactModificationContext<'a> {
+impl<'a, Bodies, Colliders> ContactModificationContext<'a, Bodies, Colliders> {
     /// Helper function to update `self` to emulate a oneway-platform.
     ///
     /// The "oneway" behavior will only allow contacts between two colliders
@@ -127,9 +127,14 @@ bitflags::bitflags! {
         const MODIFY_SOLVER_CONTACTS = 0b0100;
     }
 }
+impl Default for PhysicsHooksFlags {
+    fn default() -> Self {
+        PhysicsHooksFlags::empty()
+    }
+}
 
 /// User-defined functions called by the physics engines during one timestep in order to customize its behavior.
-pub trait PhysicsHooks: Send + Sync {
+pub trait PhysicsHooks<Bodies, Colliders>: Send + Sync {
     /// The sets of hooks that must be taken into account.
     fn active_hooks(&self) -> PhysicsHooksFlags;
 
@@ -156,7 +161,10 @@ pub trait PhysicsHooks: Send + Sync {
     /// will be taken into account by the constraints solver. If this returns
     /// `Some(SolverFlags::empty())` then the constraints solver will ignore these
     /// contacts.
-    fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option<SolverFlags> {
+    fn filter_contact_pair(
+        &self,
+        _context: &PairFilterContext<Bodies, Colliders>,
+    ) -> Option<SolverFlags> {
         None
     }
 
@@ -179,7 +187,7 @@ pub trait PhysicsHooks: Send + Sync {
     /// not compute any intersection information for it.
     /// If this return `true` then the narrow-phase will compute intersection
     /// information for this pair.
-    fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool {
+    fn filter_intersection_pair(&self, _context: &PairFilterContext<Bodies, Colliders>) -> bool {
         false
     }
 
@@ -207,21 +215,22 @@ pub trait PhysicsHooks: Send + Sync {
     /// as 0 and can be modified in `context.user_data`.
     ///
     /// The world-space contact normal can be modified in `context.normal`.
-    fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {}
+    fn modify_solver_contacts(&self, _context: &mut ContactModificationContext<Bodies, Colliders>) {
+    }
 }
 
-impl PhysicsHooks for () {
+impl<Bodies, Colliders> PhysicsHooks<Bodies, Colliders> for () {
     fn active_hooks(&self) -> PhysicsHooksFlags {
         PhysicsHooksFlags::empty()
     }
 
-    fn filter_contact_pair(&self, _: &PairFilterContext) -> Option<SolverFlags> {
+    fn filter_contact_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> Option<SolverFlags> {
         None
     }
 
-    fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool {
+    fn filter_intersection_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> bool {
         false
     }
 
-    fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {}
+    fn modify_solver_contacts(&self, _: &mut ContactModificationContext<Bodies, Colliders>) {}
 }
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index f4ac531..5a7a827 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -1,17 +1,28 @@
 //! Physics pipeline structures.
 
 use crate::counters::Counters;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption};
 #[cfg(not(feature = "parallel"))]
 use crate::dynamics::IslandSolver;
-use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
+use crate::dynamics::{
+    CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyActivation, RigidBodyCcd,
+    RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces,
+    RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+    RigidBodyVelocity,
+};
 #[cfg(feature = "parallel")]
 use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
 use crate::geometry::{
-    BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
+    BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups,
+    ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition,
+    ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase,
 };
 use crate::math::{Real, Vector};
 use crate::pipeline::{EventHandler, PhysicsHooks};
 
+#[cfg(feature = "default-sets")]
+use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
+
 /// The physics pipeline, responsible for stepping the whole physics simulation.
 ///
 /// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh
@@ -58,17 +69,43 @@ impl PhysicsPipeline {
         }
     }
 
-    fn detect_collisions(
+    fn clear_modified_colliders(
+        &mut self,
+        colliders: &mut impl ComponentSetMut<ColliderChanges>,
+        modified_colliders: &mut Vec<ColliderHandle>,
+    ) {
+        for handle in modified_colliders.drain(..) {
+            colliders.set_internal(handle.0, ColliderChanges::empty())
+        }
+    }
+
+    fn detect_collisions<Bodies, Colliders>(
         &mut self,
         integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
-        hooks: &dyn PhysicsHooks,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        hooks: &dyn PhysicsHooks<Bodies, Colliders>,
         events: &dyn EventHandler,
         handle_user_changes: bool,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyIds>
+            + ComponentSet<RigidBodyDominance>,
+        Colliders: ComponentSetMut<ColliderBroadPhaseData>
+            + ComponentSet<ColliderChanges>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderType>
+            + ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderMaterial>,
+    {
         self.counters.stages.collision_detection_time.resume();
         self.counters.cd.broad_phase_time.resume();
 
@@ -78,6 +115,8 @@ impl PhysicsPipeline {
         broad_phase.update(
             integration_parameters.prediction_distance,
             colliders,
+            modified_colliders,
+            removed_colliders,
             &mut self.broad_phase_events,
         );
 
@@ -86,37 +125,46 @@ impl PhysicsPipeline {
 
         // Update narrow-phase.
         if handle_user_changes {
-            narrow_phase.handle_user_changes(colliders, bodies, events);
+            narrow_phase.handle_user_changes(
+                islands,
+                modified_colliders,
+                removed_colliders,
+                colliders,
+                bodies,
+                events,
+            );
         }
-        narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
+        narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events);
         narrow_phase.compute_contacts(
             integration_parameters.prediction_distance,
             bodies,
             colliders,
+            modified_colliders,
             hooks,
             events,
         );
-        narrow_phase.compute_intersections(bodies, colliders, hooks, events);
-
-        // Clear colliders modification flags.
-        colliders.clear_modified_colliders();
+        narrow_phase.compute_intersections(bodies, colliders, modified_colliders, hooks, events);
 
         self.counters.cd.narrow_phase_time.pause();
         self.counters.stages.collision_detection_time.pause();
     }
 
-    fn solve_position_constraints(
+    fn solve_position_constraints<Bodies>(
         &mut self,
         integration_parameters: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
+    {
         #[cfg(not(feature = "parallel"))]
         {
             enable_flush_to_zero!();
 
-            for island_id in 0..bodies.num_islands() {
+            for island_id in 0..islands.num_islands() {
                 self.solvers[island_id].solve_position_constraints(
                     island_id,
+                    islands,
                     &mut self.counters,
                     integration_parameters,
                     bodies,
@@ -129,7 +177,7 @@ impl PhysicsPipeline {
             use rayon::prelude::*;
             use std::sync::atomic::Ordering;
 
-            let num_islands = bodies.num_islands();
+            let num_islands = ilands.num_islands();
             let solvers = &mut self.solvers[..num_islands];
             let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _);
 
@@ -140,7 +188,7 @@ impl PhysicsPipeline {
                     .par_iter_mut()
                     .enumerate()
                     .for_each(|(island_id, solver)| {
-                        let bodies: &mut RigidBodySet =
+                        let bodies: &mut Bodies =
                             unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
 
                         solver.solve_position_constraints(
@@ -154,17 +202,30 @@ impl PhysicsPipeline {
         }
     }
 
-    fn build_islands_and_solve_velocity_constraints(
+    fn build_islands_and_solve_velocity_constraints<Bodies, Colliders>(
         &mut self,
         gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
         narrow_phase: &mut NarrowPhase,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
         joints: &mut JointSet,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyForces>
+            + ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetOption<ColliderParent>,
+    {
         self.counters.stages.island_construction_time.resume();
-        bodies.update_active_set_with_contacts(
+        islands.update_active_set_with_contacts(
+            bodies,
             colliders,
             narrow_phase,
             joints.joint_graph(),
@@ -172,42 +233,58 @@ impl PhysicsPipeline {
         );
         self.counters.stages.island_construction_time.pause();
 
-        if self.manifold_indices.len() < bodies.num_islands() {
+        if self.manifold_indices.len() < islands.num_islands() {
             self.manifold_indices
-                .resize(bodies.num_islands(), Vec::new());
+                .resize(islands.num_islands(), Vec::new());
         }
 
-        if self.joint_constraint_indices.len() < bodies.num_islands() {
+        if self.joint_constraint_indices.len() < islands.num_islands() {
             self.joint_constraint_indices
-                .resize(bodies.num_islands(), Vec::new());
+                .resize(islands.num_islands(), Vec::new());
         }
 
         let mut manifolds = Vec::new();
-        narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices);
-        joints.select_active_interactions(bodies, &mut self.joint_constraint_indices);
+        narrow_phase.select_active_contacts(
+            islands,
+            bodies,
+            &mut manifolds,
+            &mut self.manifold_indices,
+        );
+        joints.select_active_interactions(islands, bodies, &mut self.joint_constraint_indices);
 
         self.counters.stages.update_time.resume();
-        bodies.foreach_active_dynamic_body_mut_internal(|_, b| {
-            b.update_world_mass_properties();
-            b.add_gravity(*gravity)
-        });
+        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.add_linear_acceleration(&gravity, effective_inv_mass)
+            });
+        }
         self.counters.stages.update_time.pause();
 
         self.counters.stages.solver_time.resume();
-        if self.solvers.len() < bodies.num_islands() {
+        if self.solvers.len() < islands.num_islands() {
             self.solvers
-                .resize_with(bodies.num_islands(), IslandSolver::new);
+                .resize_with(islands.num_islands(), IslandSolver::new);
         }
 
         #[cfg(not(feature = "parallel"))]
         {
             enable_flush_to_zero!();
 
-            for island_id in 0..bodies.num_islands() {
+            for island_id in 0..islands.num_islands() {
                 self.solvers[island_id].init_constraints_and_solve_velocity_constraints(
                     island_id,
                     &mut self.counters,
                     integration_parameters,
+                    islands,
                     bodies,
                     &mut manifolds[..],
                     &self.manifold_indices[island_id],
@@ -238,7 +315,7 @@ impl PhysicsPipeline {
                     .par_iter_mut()
                     .enumerate()
                     .for_each(|(island_id, solver)| {
-                        let bodies: &mut RigidBodySet =
+                        let bodies: &mut Bodies =
                             unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
                         let manifolds: &mut Vec<&mut ContactManifold> =
                             unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -261,19 +338,32 @@ impl PhysicsPipeline {
         self.counters.stages.solver_time.pause();
     }
 
-    fn run_ccd_motion_clamping(
+    fn run_ccd_motion_clamping<Bodies, Colliders>(
         &mut self,
         integration_parameters: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
         narrow_phase: &NarrowPhase,
         ccd_solver: &mut CCDSolver,
         events: &dyn EventHandler,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyCcd>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyMassProps>,
+        Colliders: ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSet<ColliderType>,
+    {
         self.counters.ccd.toi_computation_time.start();
         // Handle CCD
         let impacts = ccd_solver.predict_impacts_at_next_positions(
             integration_parameters.dt,
+            islands,
             bodies,
             colliders,
             narrow_phase,
@@ -283,74 +373,176 @@ impl PhysicsPipeline {
         self.counters.ccd.toi_computation_time.pause();
     }
 
-    fn advance_to_final_positions(
+    fn advance_to_final_positions<Bodies, Colliders>(
         &mut self,
-        bodies: &mut RigidBodySet,
-        colliders: &mut ColliderSet,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
+        modified_colliders: &mut Vec<ColliderHandle>,
         clear_forces: bool,
-    ) {
+    ) where
+        Bodies: ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyForces>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyType>
+            + ComponentSet<RigidBodyColliders>,
+        Colliders: ComponentSetMut<ColliderPosition>
+            + ComponentSetMut<ColliderChanges>
+            + ComponentSetOption<ColliderParent>,
+    {
         // Set the rigid-bodies and kinematic bodies to their final position.
-        bodies.foreach_active_body_mut_internal(|_, rb| {
-            if rb.is_kinematic() {
-                rb.linvel = na::zero();
-                rb.angvel = na::zero();
+        for handle in islands.iter_active_bodies() {
+            let status: &RigidBodyType = bodies.index(handle.0);
+            if status.is_kinematic() {
+                bodies.set_internal(handle.0, RigidBodyVelocity::zero());
             }
 
             if clear_forces {
-                rb.force = na::zero();
-                rb.torque = na::zero();
+                bodies.map_mut_internal(handle.0, |f: &mut RigidBodyForces| {
+                    f.torque = na::zero();
+                    f.force = na::zero();
+                });
             }
 
-            rb.position = rb.next_position;
-            rb.update_colliders_positions(colliders);
-        });
+            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);
+        }
     }
 
-    fn interpolate_kinematic_velocities(
+    fn interpolate_kinematic_velocities<Bodies>(
         &mut self,
         integration_parameters: &IntegrationParameters,
-        bodies: &mut RigidBodySet,
-    ) {
+        islands: &IslandManager,
+        bodies: &mut Bodies,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyPosition>,
+    {
         // Update kinematic bodies velocities.
         // TODO: what is the best place for this? It should at least be
         // located before the island computation because we test the velocity
         // there to determine if this kinematic body should wake-up dynamic
         // bodies it is touching.
-        bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
-            body.compute_velocity_from_next_position(integration_parameters.inv_dt());
-        });
+        for handle in islands.active_kinematic_bodies() {
+            let ppos: &RigidBodyPosition = bodies.index(handle.0);
+            let new_vel = ppos.interpolate_velocity(integration_parameters.inv_dt());
+            bodies.set_internal(handle.0, new_vel);
+        }
     }
 
-    /// Executes one timestep of the physics simulation.
+    #[cfg(feature = "default-sets")]
     pub fn step(
         &mut self,
         gravity: &Vector<Real>,
         integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
         broad_phase: &mut BroadPhase,
         narrow_phase: &mut NarrowPhase,
         bodies: &mut RigidBodySet,
         colliders: &mut ColliderSet,
         joints: &mut JointSet,
         ccd_solver: &mut CCDSolver,
-        hooks: &dyn PhysicsHooks,
+        hooks: &dyn PhysicsHooks<RigidBodySet, ColliderSet>,
         events: &dyn EventHandler,
     ) {
+        let mut modified_bodies = bodies.take_modified();
+        let mut modified_colliders = colliders.take_modified();
+        let mut removed_colliders = colliders.take_removed();
+
+        self.step_generic(
+            gravity,
+            integration_parameters,
+            islands,
+            broad_phase,
+            narrow_phase,
+            bodies,
+            colliders,
+            &mut modified_bodies,
+            &mut modified_colliders,
+            &mut removed_colliders,
+            joints,
+            ccd_solver,
+            hooks,
+            events,
+        );
+    }
+
+    /// Executes one timestep of the physics simulation.
+    pub fn step_generic<Bodies, Colliders>(
+        &mut self,
+        gravity: &Vector<Real>,
+        integration_parameters: &IntegrationParameters,
+        islands: &mut IslandManager,
+        broad_phase: &mut BroadPhase,
+        narrow_phase: &mut NarrowPhase,
+        bodies: &mut Bodies,
+        colliders: &mut Colliders,
+        modified_bodies: &mut Vec<RigidBodyHandle>,
+        modified_colliders: &mut Vec<ColliderHandle>,
+        removed_colliders: &mut Vec<ColliderHandle>,
+        joints: &mut JointSet,
+        ccd_solver: &mut CCDSolver,
+        hooks: &dyn PhysicsHooks<Bodies, Colliders>,
+        events: &dyn EventHandler,
+    ) where
+        Bodies: ComponentSetMut<RigidBodyPosition>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyIds>
+            + ComponentSetMut<RigidBodyForces>
+            + ComponentSetMut<RigidBodyActivation>
+            + ComponentSetMut<RigidBodyChanges>
+            + ComponentSetMut<RigidBodyCcd>
+            + ComponentSet<RigidBodyColliders>
+            + ComponentSet<RigidBodyDamping>
+            + ComponentSet<RigidBodyDominance>
+            + ComponentSet<RigidBodyType>,
+        Colliders: ComponentSetMut<ColliderBroadPhaseData>
+            + ComponentSetMut<ColliderChanges>
+            + ComponentSetMut<ColliderPosition>
+            + ComponentSet<ColliderShape>
+            + ComponentSetOption<ColliderParent>
+            + ComponentSet<ColliderType>
+            + ComponentSet<ColliderGroups>
+            + ComponentSet<ColliderMaterial>,
+    {
         self.counters.reset();
         self.counters.step_started();
-        colliders.handle_user_changes(bodies);
-        bodies.handle_user_changes(colliders);
+
+        super::user_changes::handle_user_changes_to_colliders(
+            bodies,
+            colliders,
+            &modified_colliders[..],
+        );
+        super::user_changes::handle_user_changes_to_rigid_bodies(
+            islands,
+            bodies,
+            colliders,
+            &modified_bodies,
+            modified_colliders,
+        );
 
         self.detect_collisions(
             integration_parameters,
+            islands,
             broad_phase,
             narrow_phase,
             bodies,
             colliders,
+            &modified_colliders[..],
+            removed_colliders,
             hooks,
             events,
             true,
         );
 
+        self.clear_modified_colliders(colliders, modified_colliders);
+        removed_colliders.clear();
+
         let mut remaining_time = integration_parameters.dt;
         let mut integration_parameters = *integration_parameters;
 
@@ -375,9 +567,16 @@ impl PhysicsPipeline {
             if ccd_is_enabled && remaining_substeps > 1 {
                 // NOTE: Take forces into account when updating the bodies CCD activation flags
                 //       these forces have not been integrated to the body's velocity yet.
-                let ccd_active = ccd_solver.update_ccd_active_flags(bodies, remaining_time, true);
+                let ccd_active =
+                    ccd_solver.update_ccd_active_flags(islands, bodies, remaining_time, true);
                 let first_impact = if ccd_active {
-                    ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase)
+                    ccd_solver.find_first_impact(
+                        remaining_time,
+                        islands,
+                        bodies,
+                        colliders,
+                        narrow_phase,
+                    )
                 } else {
                     None
                 };
@@ -414,10 +613,11 @@ impl PhysicsPipeline {
 
             self.counters.ccd.num_substeps += 1;
 
-            self.interpolate_kinematic_velocities(&integration_parameters, bodies);
+            self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies);
             self.build_islands_and_solve_velocity_constraints(
                 gravity,
                 &integration_parameters,
+                islands,
                 narrow_phase,
                 bodies,
                 colliders,
@@ -428,11 +628,16 @@ impl PhysicsPipeline {
             if ccd_is_enabled {
                 // NOTE: don't the forces into account when updating the CCD active flags because
                 //       they have already been integrated into the velocities by the solver.
-                let ccd_active =
-                    ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt, false);
+                let ccd_active = ccd_solver.update_ccd_active_flags(
+                    islands,
+                    bodies,
+                    integration_parameters.dt,
+                    false,
+                );
                 if ccd_active {
                     self.run_ccd_motion_clamping(
                         &integration_parameters,
+                        islands,
                         bodies,
                         colliders,
                         narrow_phase,
@@ -449,22 +654,31 @@ impl PhysicsPipeline {
             //       This happens because our CCD use the real rigid-body
             //       velocities instead of just interpolating between
             //       isometries.
-            self.solve_position_constraints(&integration_parameters, bodies);
+            self.solve_position_constraints(&integration_parameters, islands, bodies);
 
             let clear_forces = remaining_substeps == 0;
-            self.advance_to_final_positions(bodies, colliders, clear_forces);
+            self.advance_to_final_positions(
+                islands,
+                bodies,
+                colliders,
+                modified_colliders,
+                clear_forces,
+            );
             self.detect_collisions(
                 &integration_parameters,
+                islands,
                 broad_phase,
                 narrow_phase,
                 bodies,
                 colliders,
+                modified_colliders,
+                removed_colliders,
                 hooks,
                 events,
                 false,
             );
 
-            bodies.modified_inactive_set.clear();
+            self.clear_modified_colliders(colliders, modified_colliders);
         }
 
         self.counters.step_completed();
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index cb56141..3139a88 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,9 +1,14 @@
-use crate::dynamics::RigidBodySet;
+use crate::data::{BundleSet, ComponentSet, ComponentSetOption};
+use crate::dynamics::{
+    IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition,
+    RigidBodyVelocity,
+};
 use crate::geometry::{
-    Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
-    RayIntersection, SimdQ