Merge branch 'master' into collider-builder-debug

13 files changed, 452 insertions, 198 deletions

diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs
new file mode 100644
index 0000000..06164a1
--- /dev/null
+++ b/src/geometry/broad_phase.rs
@@ -0,0 +1,50 @@
+use crate::dynamics::RigidBodySet;
+use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderSet};
+use parry::math::Real;
+
+/// An internal index stored in colliders by some broad-phase algorithms.
+pub type BroadPhaseProxyIndex = u32;
+
+/// Trait implemented by broad-phase algorithms supported by Rapier.
+///
+/// The task of a broad-phase algorithm is to detect potential collision pairs, usually based on
+/// bounding volumes. The pairs must be conservative: it is OK to create a collision pair if
+/// two objects don’t actually touch, but it is incorrect to remove a pair between two objects
+/// that are still touching. In other words, it can have false-positive (though these induce
+/// some computational overhead on the narrow-phase), but cannot have false-negative.
+pub trait BroadPhase: Send + Sync + 'static {
+    /// Updates the broad-phase.
+    ///
+    /// The results must be output through the `events` struct. The broad-phase algorithm is only
+    /// required to generate new events (i.e. no need to re-send an `AddPair` event if it was already
+    /// sent previously and no `RemovePair` happened since then). Sending redundant events is allowed
+    /// but can result in a slight computational overhead.
+    ///
+    /// The `colliders` set is mutable only to provide access to
+    /// [`collider.set_internal_broad_phase_proxy_index`]. Other properties of the collider should
+    /// **not** be modified during the broad-phase update.
+    ///
+    /// # Parameters
+    /// - `prediction_distance`: colliders that are not exactly touching, but closer to this
+    ///   distance must form a collision pair.
+    /// - `colliders`: the set of colliders. Change detection with `collider.needs_broad_phase_update()`
+    ///   can be relied on at this stage.
+    /// - `modified_colliders`: colliders that are know to be modified since the last update.
+    /// - `removed_colliders`: colliders that got removed since the last update. Any associated data
+    ///   in the broad-phase should be removed by this call to `update`.
+    /// - `events`: the broad-phase’s output. They indicate what collision pairs need to be created
+    ///   and what pairs need to be removed. It is OK to create pairs for colliders that don’t
+    ///   actually collide (though this can increase computational overhead in the narrow-phase)
+    ///   but it is important not to indicate removal of a collision pair if the underlying colliders
+    ///   are still touching or closer than `prediction_distance`.
+    fn update(
+        &mut self,
+        dt: Real,
+        prediction_distance: Real,
+        colliders: &mut ColliderSet,
+        bodies: &RigidBodySet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    );
+}
diff --git a/src/geometry/broad_phase_multi_sap/broad_phase.rs b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
index 9e1bc06..d382258 100644
--- a/src/geometry/broad_phase_multi_sap/broad_phase.rs
+++ b/src/geometry/broad_phase_multi_sap/broad_phase_multi_sap.rs
@@ -1,12 +1,12 @@
 use super::{
     BroadPhasePairEvent, ColliderPair, SAPLayer, SAPProxies, SAPProxy, SAPProxyData, SAPRegionPool,
 };
-use crate::geometry::broad_phase_multi_sap::SAPProxyIndex;
 use crate::geometry::{
-    ColliderBroadPhaseData, ColliderChanges, ColliderHandle, ColliderPosition, ColliderSet,
-    ColliderShape,
+    BroadPhaseProxyIndex, Collider, ColliderBroadPhaseData, ColliderChanges, ColliderHandle,
+    ColliderSet,
 };
-use crate::math::Real;
+use crate::math::{Isometry, Real};
+use crate::prelude::{BroadPhase, RigidBodySet};
 use crate::utils::IndexMut2;
 use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::HashMap;
@@ -74,7 +74,7 @@ use parry::utils::hashmap::HashMap;
 ///   broad-phase, as well as the Aabbs of all the regions part of this broad-phase.
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
-pub struct BroadPhase {
+pub struct BroadPhaseMultiSap {
     proxies: SAPProxies,
     layers: Vec<SAPLayer>,
     smallest_layer: u8,
@@ -90,7 +90,7 @@ pub struct BroadPhase {
     //       Another alternative would be to remove ColliderProxyId and
     //       just use a Coarena. But this seems like it could use too
     //       much memory.
-    colliders_proxy_ids: HashMap<ColliderHandle, SAPProxyIndex>,
+    colliders_proxy_ids: HashMap<ColliderHandle, BroadPhaseProxyIndex>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     region_pool: SAPRegionPool, // To avoid repeated allocations.
     // We could think serializing this workspace is useless.
@@ -114,16 +114,16 @@ pub struct BroadPhase {
     reporting: HashMap<(u32, u32), bool>, // Workspace
 }
 
-impl Default for BroadPhase {
+impl Default for BroadPhaseMultiSap {
     fn default() -> Self {
         Self::new()
     }
 }
 
-impl BroadPhase {
+impl BroadPhaseMultiSap {
     /// Create a new empty broad-phase.
     pub fn new() -> Self {
-        BroadPhase {
+        BroadPhaseMultiSap {
             proxies: SAPProxies::new(),
             layers: Vec::new(),
             smallest_layer: 0,
@@ -138,7 +138,7 @@ impl BroadPhase {
     ///
     /// For each colliders marked as removed, we make their containing layer mark
     /// its proxy as pre-deleted. The actual proxy removal will happen at the end
-    /// of the `BroadPhase::update`.
+    /// of the `BroadPhaseMultiSap::update`.
     fn handle_removed_colliders(&mut self, removed_colliders: &[ColliderHandle]) {
         // For each removed collider, remove the corresponding proxy.
         for removed in removed_colliders {
@@ -156,7 +156,7 @@ impl BroadPhase {
     /// remove, the `complete_removal` method MUST be called to
     /// complete the removal of these proxies, by actually removing them
     /// from all the relevant layers/regions/axes.
-    fn predelete_proxy(&mut self, proxy_index: SAPProxyIndex) {
+    fn predelete_proxy(&mut self, proxy_index: BroadPhaseProxyIndex) {
         if proxy_index == crate::INVALID_U32 {
             // This collider has not been added to the broad-phase yet.
             return;
@@ -353,13 +353,18 @@ impl BroadPhase {
         prediction_distance: Real,
         handle: ColliderHandle,
         proxy_index: &mut u32,
-        collider: (&ColliderPosition, &ColliderShape, &ColliderChanges),
+        collider: &Collider,
+        next_position: Option<&Isometry<Real>>,
     ) -> bool {
-        let (co_pos, co_shape, co_changes) = collider;
-
-        let mut aabb = co_shape
-            .compute_aabb(co_pos)
-            .loosened(prediction_distance / 2.0);
+        let mut aabb = collider.compute_collision_aabb(prediction_distance / 2.0);
+
+        if let Some(next_position) = next_position {
+            let next_aabb = collider
+                .shape
+                .compute_aabb(next_position)
+                .loosened(collider.contact_skin() + prediction_distance / 2.0);
+            aabb.merge(&next_aabb);
+        }
 
         if aabb.mins.coords.iter().any(|e| !e.is_finite())
             || aabb.maxs.coords.iter().any(|e| !e.is_finite())
@@ -378,7 +383,7 @@ impl BroadPhase {
             prev_aabb = proxy.aabb;
             proxy.aabb = aabb;
 
-            if co_changes.contains(ColliderChanges::SHAPE) {
+            if collider.changes.contains(ColliderChanges::SHAPE) {
                 // If the shape was changed, then we need to see if this proxy should be
                 // migrated to a larger layer. Indeed, if the shape was replaced by
                 // a much larger shape, we need to promote the proxy to a bigger layer
@@ -449,65 +454,6 @@ impl BroadPhase {
         !layer.created_regions.is_empty()
     }
 
-    /// Updates the broad-phase, taking into account the new collider positions.
-    pub fn update(
-        &mut self,
-        prediction_distance: Real,
-        colliders: &mut ColliderSet,
-        modified_colliders: &[ColliderHandle],
-        removed_colliders: &[ColliderHandle],
-        events: &mut Vec<BroadPhasePairEvent>,
-    ) {
-        // Phase 1: pre-delete the collisions that have been deleted.
-        self.handle_removed_colliders(removed_colliders);
-
-        let mut need_region_propagation = false;
-
-        // Phase 2: pre-delete the collisions that have been deleted.
-        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_mut_internal(*handle) {
-                if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
-                    continue;
-                }
-
-                let mut new_proxy_id = co.bf_data.proxy_index;
-
-                if self.handle_modified_collider(
-                    prediction_distance,
-                    *handle,
-                    &mut new_proxy_id,
-                    (&co.pos, &co.shape, &co.changes),
-                ) {
-                    need_region_propagation = true;
-                }
-
-                if co.bf_data.proxy_index != new_proxy_id {
-                    self.colliders_proxy_ids.insert(*handle, new_proxy_id);
-
-                    // Make sure we have the new proxy index in case
-                    // the collider was added for the first time.
-                    co.bf_data = ColliderBroadPhaseData {
-                        proxy_index: new_proxy_id,
-                    };
-                }
-            }
-        }
-
-        // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
-        if need_region_propagation {
-            self.propagate_created_regions();
-        }
-
-        // Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
-        self.update_layers_and_find_pairs(events);
-
-        // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
-        // layers to possible remove regions from larger layers that would become empty that way.
-        self.complete_removals(colliders, removed_colliders);
-    }
-
     /// Propagate regions from the smallest layers up to the larger layers.
     ///
     /// Whenever a region is created on a layer `n`, then its Aabb must be
@@ -618,16 +564,90 @@ impl BroadPhase {
     }
 }
 
+impl BroadPhase for BroadPhaseMultiSap {
+    /// Updates the broad-phase, taking into account the new collider positions.
+    fn update(
+        &mut self,
+        dt: Real,
+        prediction_distance: Real,
+        colliders: &mut ColliderSet,
+        bodies: &RigidBodySet,
+        modified_colliders: &[ColliderHandle],
+        removed_colliders: &[ColliderHandle],
+        events: &mut Vec<BroadPhasePairEvent>,
+    ) {
+        // Phase 1: pre-delete the collisions that have been deleted.
+        self.handle_removed_colliders(removed_colliders);
+
+        let mut need_region_propagation = false;
+
+        // Phase 2: pre-delete the collisions that have been deleted.
+        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_mut_internal(*handle) {
+                if !co.is_enabled() || !co.changes.needs_broad_phase_update() {
+                    continue;
+                }
+
+                let mut new_proxy_id = co.bf_data.proxy_index;
+
+                let next_pos = co.parent.and_then(|p| {
+                    let parent = bodies.get(p.handle)?;
+                    (parent.soft_ccd_prediction() > 0.0).then(|| {
+                        parent.predict_position_using_velocity_and_forces_with_max_dist(
+                            dt,
+                            parent.soft_ccd_prediction(),
+                        ) * p.pos_wrt_parent
+                    })
+                });
+
+                if self.handle_modified_collider(
+                    prediction_distance,
+                    *handle,
+                    &mut new_proxy_id,
+                    co,
+                    next_pos.as_ref(),
+                ) {
+                    need_region_propagation = true;
+                }
+
+                if co.bf_data.proxy_index != new_proxy_id {
+                    self.colliders_proxy_ids.insert(*handle, new_proxy_id);
+
+                    // Make sure we have the new proxy index in case
+                    // the collider was added for the first time.
+                    co.bf_data = ColliderBroadPhaseData {
+                        proxy_index: new_proxy_id,
+                    };
+                }
+            }
+        }
+
+        // Phase 3: bottom-up pass to propagate new regions from smaller layers to larger layers.
+        if need_region_propagation {
+            self.propagate_created_regions();
+        }
+
+        // Phase 4: top-down pass to propagate proxies from larger layers to smaller layers.
+        self.update_layers_and_find_pairs(events);
+
+        // Phase 5: bottom-up pass to remove proxies, and propagate region removed from smaller
+        // layers to possible remove regions from larger layers that would become empty that way.
+        self.complete_removals(colliders, removed_colliders);
+    }
+}
+
 #[cfg(test)]
 mod test {
     use crate::dynamics::{
         ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBodyBuilder, RigidBodySet,
     };
-    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
+    use crate::geometry::{BroadPhase, BroadPhaseMultiSap, ColliderBuilder, ColliderSet};
 
     #[test]
     fn test_add_update_remove() {
-        let mut broad_phase = BroadPhase::new();
+        let mut broad_phase = BroadPhaseMultiSap::new();
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
         let mut impulse_joints = ImpulseJointSet::new();
@@ -640,7 +660,7 @@ mod test {
         let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
 
         let mut events = Vec::new();
-        broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
+        broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
 
         bodies.remove(
             hrb,
@@ -650,7 +670,7 @@ mod test {
             &mut multibody_joints,
             true,
         );
-        broad_phase.update(0.0, &mut colliders, &[], &[coh], &mut events);
+        broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[], &[coh], &mut events);
 
         // Create another body.
         let rb = RigidBodyBuilder::dynamic().build();
@@ -659,6 +679,6 @@ mod test {
         let coh = colliders.insert_with_parent(co, hrb, &mut bodies);
 
         // Make sure the proxy handles is recycled properly.
-        broad_phase.update(0.0, &mut colliders, &[coh], &[], &mut events);
+        broad_phase.update(0.0, 0.0, &mut colliders, &bodies, &[coh], &[], &mut events);
     }
 }
diff --git a/src/geometry/broad_phase_multi_sap/mod.rs b/src/geometry/broad_phase_multi_sap/mod.rs
index 0f85e96..b9b3097 100644
--- a/src/geometry/broad_phase_multi_sap/mod.rs
+++ b/src/geometry/broad_phase_multi_sap/mod.rs
@@ -1,6 +1,5 @@
-pub use self::broad_phase::BroadPhase;
+pub use self::broad_phase_multi_sap::BroadPhaseMultiSap;
 pub use self::broad_phase_pair_event::{BroadPhasePairEvent, ColliderPair};
-pub use self::sap_proxy::SAPProxyIndex;
 
 use self::sap_axis::*;
 use self::sap_endpoint::*;
@@ -9,7 +8,7 @@ use self::sap_proxy::*;
 use self::sap_region::*;
 use self::sap_utils::*;
 
-mod broad_phase;
+mod broad_phase_multi_sap;
 mod broad_phase_pair_event;
 mod sap_axis;
 mod sap_endpoint;
diff --git a/src/geometry/broad_phase_multi_sap/sap_axis.rs b/src/geometry/broad_phase_multi_sap/sap_axis.rs
index 2452148..f1afdee 100644
--- a/src/geometry/broad_phase_multi_sap/sap_axis.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_axis.rs
@@ -1,6 +1,6 @@
 use super::{SAPEndpoint, SAPProxies, NUM_SENTINELS};
 use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
-use crate::geometry::SAPProxyIndex;
+use crate::geometry::BroadPhaseProxyIndex;
 use crate::math::Real;
 use bit_vec::BitVec;
 use parry::bounding_volume::BoundingVolume;
@@ -39,7 +39,7 @@ impl SAPAxis {
     pub fn batch_insert(
         &mut self,
         dim: usize,
-        new_proxies: &[SAPProxyIndex],
+        new_proxies: &[BroadPhaseProxyIndex],
         proxies: &SAPProxies,
         reporting: Option<&mut HashMap<(u32, u32), bool>>,
     ) {
diff --git a/src/geometry/broad_phase_multi_sap/sap_layer.rs b/src/geometry/broad_phase_multi_sap/sap_layer.rs
index 2266d56..f1a3a31 100644
--- a/src/geometry/broad_phase_multi_sap/sap_layer.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_layer.rs
@@ -1,6 +1,6 @@
 use super::{SAPProxies, SAPProxy, SAPRegion, SAPRegionPool};
 use crate::geometry::broad_phase_multi_sap::DELETED_AABB_VALUE;
-use crate::geometry::{Aabb, SAPProxyIndex};
+use crate::geometry::{Aabb, BroadPhaseProxyIndex};
 use crate::math::{Point, Real};
 use parry::bounding_volume::BoundingVolume;
 use parry::utils::hashmap::{Entry, HashMap};
@@ -13,11 +13,11 @@ pub(crate) struct SAPLayer {
     pub smaller_layer: Option<u8>,
     pub larger_layer: Option<u8>,
     region_width: Real,
-    pub regions: HashMap<Point<i32>, SAPProxyIndex>,
+    pub regions: HashMap<Point<i32>, BroadPhaseProxyIndex>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     regions_to_potentially_remove: Vec<Point<i32>>, // Workspace
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub created_regions: Vec<SAPProxyIndex>,
+    pub created_regions: Vec<BroadPhaseProxyIndex>,
 }
 
 impl SAPLayer {
@@ -71,7 +71,7 @@ impl SAPLayer {
     ///
     /// This method must be called in a bottom-up loop, propagating new regions from the
     /// smallest layer, up to the largest layer. That loop is done by the Phase 3 of the
-    /// BroadPhase::update.
+    /// BroadPhaseMultiSap::update.
     pub fn propagate_created_regions(
         &mut self,
         larger_layer: &mut Self,
@@ -103,7 +103,7 @@ impl SAPLayer {
     /// one region on its parent "larger" layer.
     fn register_subregion(
         &mut self,
-        proxy_id: SAPProxyIndex,
+        proxy_id: BroadPhaseProxyIndex,
         proxies: &mut SAPProxies,
         pool: &mut SAPRegionPool,
     ) {
@@ -140,7 +140,7 @@ impl SAPLayer {
 
     fn unregister_subregion(
         &mut self,
-        proxy_id: SAPProxyIndex,
+        proxy_id: BroadPhaseProxyIndex,
         proxy_region: &SAPRegion,
         proxies: &mut SAPProxies,
     ) {
@@ -182,7 +182,7 @@ impl SAPLayer {
     /// If the region with the given region key does not exist yet, it is created.
     /// When a region is created, it creates a new proxy for that region, and its
     /// proxy ID is added to `self.created_region` so it can be propagated during
-    /// the Phase 3 of `BroadPhase::update`.
+    /// the Phase 3 of `BroadPhaseMultiSap::update`.
     ///
     /// This returns the proxy ID of the already existing region if it existed, or
     /// of the new region if it did not exist and has been created by this method.
@@ -191,7 +191,7 @@ impl SAPLayer {
         region_key: Point<i32>,
         proxies: &mut SAPProxies,
         pool: &mut SAPRegionPool,
-    ) -> SAPProxyIndex {
+    ) -> BroadPhaseProxyIndex {
         match self.regions.entry(region_key) {
             // Yay, the region already exists!
             Entry::Occupied(occupied) => *occupied.get(),
@@ -266,7 +266,7 @@ impl SAPLayer {
         }
     }
 
-    pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: SAPProxyIndex) {
+    pub fn predelete_proxy(&mut self, proxies: &mut SAPProxies, proxy_index: BroadPhaseProxyIndex) {
         // Discretize the Aabb to find the regions that need to be invalidated.
         let proxy_aabb = &mut proxies[proxy_index].aabb;
         let start = super::point_key(proxy_aabb.mins, self.region_width);
@@ -379,7 +379,7 @@ impl SAPLayer {
     pub fn proper_proxy_moved_to_bigger_layer(
         &mut self,
         proxies: &mut SAPProxies,
-        proxy_id: SAPProxyIndex,
+        proxy_id: BroadPhaseProxyIndex,
     ) {
         for (point, region_id) in &self.regions {
             let region = &mut proxies[*region_id].data.as_region_mut();
diff --git a/src/geometry/broad_phase_multi_sap/sap_proxy.rs b/src/geometry/broad_phase_multi_sap/sap_proxy.rs
index 4d5d79e..ccc172f 100644
--- a/src/geometry/broad_phase_multi_sap/sap_proxy.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_proxy.rs
@@ -1,11 +1,9 @@
 use super::NEXT_FREE_SENTINEL;
 use crate::geometry::broad_phase_multi_sap::SAPRegion;
-use crate::geometry::ColliderHandle;
+use crate::geometry::{BroadPhaseProxyIndex, ColliderHandle};
 use parry::bounding_volume::Aabb;
 use std::ops::{Index, IndexMut};
 
-pub type SAPProxyIndex = u32;
-
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 pub enum SAPProxyData {
@@ -49,7 +47,7 @@ impl SAPProxyData {
 pub struct SAPProxy {
     pub data: SAPProxyData,
     pub aabb: Aabb,
-    pub next_free: SAPProxyIndex,
+    pub next_free: BroadPhaseProxyIndex,
     // TODO: pack the layer_id and layer_depth into a single u16?
     pub layer_id: u8,
     pub layer_depth: i8,
@@ -81,7 +79,7 @@ impl SAPProxy {
 #[derive(Clone)]
 pub struct SAPProxies {
     pub elements: Vec<SAPProxy>,
-    pub first_free: SAPProxyIndex,
+    pub first_free: BroadPhaseProxyIndex,
 }
 
 impl Default for SAPProxies {
@@ -98,7 +96,7 @@ impl SAPProxies {
         }
     }
 
-    pub fn insert(&mut self, proxy: SAPProxy) -> SAPProxyIndex {
+    pub fn insert(&mut self, proxy: SAPProxy) -> BroadPhaseProxyIndex {
         if self.first_free != NEXT_FREE_SENTINEL {
             let proxy_id = self.first_free;
             self.first_free = self.elements[proxy_id as usize].next_free;
@@ -110,31 +108,31 @@ impl SAPProxies {
         }
     }
 
-    pub fn remove(&mut self, proxy_id: SAPProxyIndex) {
+    pub fn remove(&mut self, proxy_id: BroadPhaseProxyIndex) {
         let proxy = &mut self.elements[proxy_id as usize];
         proxy.next_free = self.first_free;
         self.first_free = proxy_id;
     }
 
     // NOTE: this must not take holes into account.
-    pub fn get_mut(&mut self, i: SAPProxyIndex) -> Option<&mut SAPProxy> {
+    pub fn get_mut(&mut self, i: BroadPhaseProxyIndex) -> Option<&mut SAPProxy> {
         self.elements.get_mut(i as usize)
     }
     // NOTE: this must not take holes into account.
-    pub fn get(&self, i: SAPProxyIndex) -> Option<&SAPProxy> {
+    pub fn get(&self, i: BroadPhaseProxyIndex) -> Option<&SAPProxy> {
         self.elements.get(i as usize)
     }
 }
 
-impl Index<SAPProxyIndex> for SAPProxies {
+impl Index<BroadPhaseProxyIndex> for SAPProxies {
     type Output = SAPProxy;
-    fn index(&self, i: SAPProxyIndex) -> &SAPProxy {
+    fn index(&self, i: BroadPhaseProxyIndex) -> &SAPProxy {
         self.elements.index(i as usize)
     }
 }
 
-impl IndexMut<SAPProxyIndex> for SAPProxies {
-    fn index_mut(&mut self, i: SAPProxyIndex) -> &mut SAPProxy {
+impl IndexMut<BroadPhaseProxyIndex> for SAPProxies {
+    fn index_mut(&mut self, i: BroadPhaseProxyIndex) -> &mut SAPProxy {
         self.elements.index_mut(i as usize)
     }
 }
diff --git a/src/geometry/broad_phase_multi_sap/sap_region.rs b/src/geometry/broad_phase_multi_sap/sap_region.rs
index 21ebca5..7e38eaa 100644
--- a/src/geometry/broad_phase_multi_sap/sap_region.rs
+++ b/src/geometry/broad_phase_multi_sap/sap_region.rs
@@ -1,5 +1,5 @@
 use super::{SAPAxis, SAPProxies};
-use crate::geometry::SAPProxyIndex;
+use crate::geometry::BroadPhaseProxyIndex;
 use crate::math::DIM;
 use bit_vec::BitVec;
 use parry::bounding_volume::Aabb;
@@ -13,8 +13,8 @@ pub struct SAPRegion {
     pub axes: [SAPAxis; DIM],
     pub existing_proxies: BitVec,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub to_insert: Vec<SAPProxyIndex>, // Workspace
-    pub subregions: Vec<SAPProxyIndex>,
+    pub to_insert: Vec<BroadPhaseProxyIndex>, // Workspace
+    pub subregions: Vec<BroadPhaseProxyIndex>,
     pub id_in_parent_subregion: u32,
     pub update_count: u8,
     pub needs_update_after_subregion_removal: bool,
@@ -90,7 +90,7 @@ impl SAPRegion {
     /// If this region contains the given proxy, this will decrement this region's proxy count.
     ///
     /// Returns `true` if this region contained the proxy. Returns `false` otherwise.
-    pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: SAPProxyIndex) -> bool {
+    pub fn proper_proxy_moved_to_a_bigger_layer(&mut self, proxy_id: BroadPhaseProxyIndex) -> bool {
         if self.existing_proxies.get(proxy_id as usize) == Some(true) {
             // NOTE: we are just registering the fact that that proxy isn't a
             //       subproper proxy anymore. But it is still part of this region
@@ -142,7 +142,7 @@ impl SAPRegion {
         self.subproper_proxy_count -= num_deleted_subregion_endpoints[0] / 2;
     }
 
-    pub fn predelete_proxy(&mut self, _proxy_id: SAPProxyIndex) {
+    pub fn predelete_proxy(&mut self, _proxy_id: BroadPhaseProxyIndex) {
         // We keep the proxy_id as argument for uniformity with the "preupdate"
         // method. However we don't actually need it because the deletion will be
         // handled transparently during the next update.
@@ -153,14 +153,18 @@ impl SAPRegion {
         self.update_count = self.update_count.max(1);
     }
 
-    pub fn register_subregion(&mut self, proxy_id: SAPProxyIndex) -> usize {
+    pub fn register_subregion(&mut self, proxy_id: BroadPhaseProxyIndex) -> usize {
         let subregion_index = self.subregions.len();
         self.subregions.push(proxy_id);
         self.preupdate_proxy(proxy_id, true);
         subregion_index
     }
 
-    pub fn preupdate_proxy(&mut self, proxy_id: SAPProxyIndex, is_subproper_proxy: bool) -> bool {
+    pub fn preupdate_proxy(
+        &mut self,
+        proxy_id: BroadPhaseProxyIndex,
+        is_subproper_proxy: bool,
+    ) -> bool {
         let mask_len = self.existing_proxies.len();
         if proxy_id as usize >= mask_len {
             self.existing_proxies
diff --git a/src/geometry/broad_phase_qbvh.rs b/src/geometry/broad_phase_qbvh.rs
index 22ca562..5fd6bcf 100644
--- a/src/geometry/broad_phase_qbvh.rs
+++ b/src/geometry/broad_phase_qbvh.rs
@@ -1,5 +1,4 @@
 use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
-use parry::bounding_volume::BoundingVolume;
 use parry::math::Real;
 use parry::partitioning::Qbvh;
 use parry::partitioning::QbvhUpdateWorkspace;
@@ -7,20 +6,20 @@ use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
-pub struct BroadPhase {
+pub struct BroadPhaseQbvh {
     qbvh: Qbvh<ColliderHandle>,
     stack: Vec<(u32, u32)>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     workspace: QbvhUpdateWorkspace,
 }
 
-impl Default for BroadPhase {
+impl Default for BroadPhaseQbvh {
     fn default() -> Self {
         Self::new()
     }
 }
 
-impl BroadPhase {
+impl BroadPhaseQbvh {
     pub fn new() -> Self {
         Self {
             qbvh: Qbvh::new(),
@@ -59,7 +58,7 @@ impl BroadPhase {
                 colliders.iter().map(|(handle, collider)| {
                     (
                         handle,
-                        collider.compute_aabb().loosened(prediction_distance / 2.0),
+                        collider.compute_collision_aabb(prediction_distance / 2.0),
                     )
                 }),
                 margin,
@@ -76,9 +75,7 @@ impl BroadPhase {
             }
 
             let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
-                colliders[*handle]
-                    .compute_aabb()
-                    .loosened(prediction_distance / 2.0)
+                colliders[*handle].compute_collision_aabb(prediction_distance / 2.0)
             });
             self.qbvh
                 .traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 261de46..565d6e2 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,17 +1,20 @@
 use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
 use crate::geometry::{
-    ActiveCollisionTypes, ColliderBroadPhaseData, ColliderChanges, ColliderFlags,
-    ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition, ColliderShape,
-    ColliderType, InteractionGroups, SharedShape,
+    ActiveCollisionTypes, BroadPhaseProxyIndex, ColliderBroadPhaseData, ColliderChanges,
+    ColliderFlags, ColliderMassProps, ColliderMaterial, ColliderParent, ColliderPosition,
+    ColliderShape, ColliderType, InteractionGroups, SharedShape,
 };
 use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
 use crate::parry::transformation::vhacd::VHACDParameters;
 use crate::pipeline::{ActiveEvents, ActiveHooks};
 use crate::prelude::ColliderEnabled;
 use na::Unit;
-use parry::bounding_volume::Aabb;
+use parry::bounding_volume::{Aabb, BoundingVolume};
 use parry::shape::{Shape, TriMeshFlags};
 
+#[cfg(feature = "dim3")]
+use crate::geometry::HeightFieldFlags;
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 /// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
@@ -27,6 +30,7 @@ pub struct Collider {
     pub(crate) material: ColliderMaterial,
     pub(crate) flags: ColliderFlags,
     pub(crate) bf_data: ColliderBroadPhaseData,
+    contact_skin: Real,
     contact_force_event_threshold: Real,
     /// User-defined data associated to this collider.
     pub user_data: u128,
@@ -50,6 +54,21 @@ impl Collider {
         }
     }
 
+    /// An internal index associated to this collider by the broad-phase algorithm.
+    pub fn internal_broad_phase_proxy_index(&self) -> BroadPhaseProxyIndex {
+        self.bf_data.proxy_index
+    }
+
+    /// Sets the internal index associated to this collider by the broad-phase algorithm.
+    ///
+    /// This must **not** be called, unless you are implementing your own custom broad-phase
+    /// that require storing an index in the collider struct.
+    /// Modifying that index outside of a custom broad-phase code will most certainly break
+    /// the physics engine.
+    pub fn set_internal_broad_phase_proxy_index(&mut self, id: BroadPhaseProxyIndex) {
+        self.bf_data.proxy_index = id;
+    }
+
     /// The rigid body this collider is attached to.
     pub fn parent(&self) -> Option<RigidBodyHandle> {
         self.parent.map(|parent| parent.handle)
@@ -60,6 +79,55 @@ impl Collider {
         self.coll_type.is_sensor()
     }
 
+    /// Copy all the characteristics from `other` to `self`.
+    ///
+    /// If you have a mutable reference to a collider `collider: &mut Collider`, attempting to
+    /// assign it a whole new collider instance, e.g., `*collider = ColliderBuilder::ball(0.5).build()`,
+    /// will crash due to some internal indices being overwritten. Instead, use
+    /// `collider.copy_from(&ColliderBuilder::ball(0.5).build())`.
+    ///
+    /// This method will allow you to set most characteristics of this collider from another
+    /// collider instance without causing any breakage.
+    ///
+    /// This method **cannot** be used for reparenting a collider. Therefore, the parent of the
+    /// `other` (if any), as well as its relative position to that parent will not be copied into
+    /// `self`.
+    ///
+    /// The pose of `other` will only copied into `self` if `self` doesn’t have a parent (if it has
+    /// a parent, its position is directly controlled by the parent rigid-body).
+    pub fn copy_from(&mut self, other: &Collider) {
+        // NOTE: we deconstruct the collider struct to be sure we don’t forget to
+        //       add some copies here if we add more field to Collider in the future.
+        let Collider {
+            coll_type,
+            shape,
+            mprops,
+            changes: _changes, // Will be set to ALL.
+            parent: _parent,   // This function cannot be used to reparent the collider.
+            pos,
+            material,
+            flags,
+            bf_data: _bf_data, // Internal ids must not be overwritten.