Merge branch 'master' into infinite_fall_memory

12 files changed, 1026 insertions, 405 deletions

diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs
deleted file mode 100644
index a43b7af..0000000
--- a/src/geometry/broad_phase.rs
+++ /dev/null
@@ -1,255 +0,0 @@
-use crate::geometry::ColliderHandle;
-use ncollide::bounding_volume::AABB;
-#[cfg(feature = "simd-is-enabled")]
-use {
-    crate::geometry::WAABB,
-    crate::math::{Point, SIMD_WIDTH},
-    crate::utils::WVec,
-    simba::simd::SimdBool as _,
-};
-
-#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-pub struct ColliderPair {
-    pub collider1: ColliderHandle,
-    pub collider2: ColliderHandle,
-}
-
-impl ColliderPair {
-    pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
-        ColliderPair {
-            collider1,
-            collider2,
-        }
-    }
-
-    pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
-        if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
-            Self::new(collider1, collider2)
-        } else {
-            Self::new(collider2, collider1)
-        }
-    }
-
-    pub fn swap(self) -> Self {
-        Self::new(self.collider2, self.collider1)
-    }
-
-    pub fn zero() -> Self {
-        Self {
-            collider1: ColliderHandle::from_raw_parts(0, 0),
-            collider2: ColliderHandle::from_raw_parts(0, 0),
-        }
-    }
-}
-
-pub struct WAABBHierarchyIntersections {
-    curr_level_interferences: Vec<usize>,
-    next_level_interferences: Vec<usize>,
-}
-
-impl WAABBHierarchyIntersections {
-    pub fn new() -> Self {
-        Self {
-            curr_level_interferences: Vec::new(),
-            next_level_interferences: Vec::new(),
-        }
-    }
-
-    pub fn computed_interferences(&self) -> &[usize] {
-        &self.curr_level_interferences[..]
-    }
-
-    pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec<usize> {
-        &mut self.curr_level_interferences
-    }
-}
-
-#[cfg(feature = "simd-is-enabled")]
-#[derive(Clone)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-pub struct WAABBHierarchy {
-    levels: Vec<Vec<WAABB>>,
-}
-
-#[cfg(feature = "simd-is-enabled")]
-impl WAABBHierarchy {
-    pub fn new(aabbs: &[AABB<f32>]) -> Self {
-        let mut waabbs: Vec<_> = aabbs
-            .chunks_exact(SIMD_WIDTH)
-            .map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH]))
-            .collect();
-
-        if aabbs.len() % SIMD_WIDTH != 0 {
-            let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH;
-            let last_i = aabbs.len() - 1;
-            let last_waabb =
-                WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]);
-            waabbs.push(last_waabb);
-        }
-
-        let mut levels = vec![waabbs];
-
-        loop {
-            let last_level = levels.last().unwrap();
-            let mut next_level = Vec::new();
-
-            for chunk in last_level.chunks_exact(SIMD_WIDTH) {
-                let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]);
-                let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]);
-                next_level.push(WAABB::new(mins, maxs));
-            }
-
-            // Deal with the last non-exact chunk.
-            if last_level.len() % SIMD_WIDTH != 0 {
-                let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH;
-                let last_id = last_level.len() - 1;
-                let mins = array![|ii| last_level[(first_id + ii).min(last_id)]
-                    .mins
-                    .horizontal_inf(); SIMD_WIDTH];
-                let maxs = array![|ii| last_level[(first_id + ii).min(last_id)]
-                    .maxs
-                    .horizontal_sup(); SIMD_WIDTH];
-
-                let mins = Point::from(mins);
-                let maxs = Point::from(maxs);
-                next_level.push(WAABB::new(mins, maxs));
-            }
-
-            if next_level.len() == 1 {
-                levels.push(next_level);
-                break;
-            }
-
-            levels.push(next_level);
-        }
-
-        Self { levels }
-    }
-
-    pub fn compute_interferences_with(
-        &self,
-        aabb: AABB<f32>,
-        workspace: &mut WAABBHierarchyIntersections,
-    ) {
-        let waabb1 = WAABB::splat(aabb);
-        workspace.next_level_interferences.clear();
-        workspace.curr_level_interferences.clear();
-        workspace.curr_level_interferences.push(0);
-
-        for level in self.levels.iter().rev() {
-            for i in &workspace.curr_level_interferences {
-                // This `if let` handle the case when `*i` is out of bounds because
-                // the initial number of aabbs was not a power of SIMD_WIDTH.
-                if let Some(waabb2) = level.get(*i) {
-                    // NOTE: using `intersect.bitmask()` and performing bit comparisons
-                    // is much more efficient than testing if each intersect.extract(i) is true.
-                    let intersect = waabb1.intersects_lanewise(waabb2);
-                    let bitmask = intersect.bitmask();
-
-                    for j in 0..SIMD_WIDTH {
-                        if (bitmask & (1 << j)) != 0 {
-                            workspace.next_level_interferences.push(i * SIMD_WIDTH + j)
-                        }
-                    }
-                }
-            }
-
-            std::mem::swap(
-                &mut workspace.curr_level_interferences,
-                &mut workspace.next_level_interferences,
-            );
-            workspace.next_level_interferences.clear();
-        }
-    }
-}
-
-#[cfg(not(feature = "simd-is-enabled"))]
-#[derive(Clone)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-pub struct WAABBHierarchy {
-    levels: Vec<Vec<AABB<f32>>>,
-}
-
-#[cfg(not(feature = "simd-is-enabled"))]
-impl WAABBHierarchy {
-    const GROUP_SIZE: usize = 4;
-
-    pub fn new(aabbs: &[AABB<f32>]) -> Self {
-        use ncollide::bounding_volume::BoundingVolume;
-
-        let mut levels = vec![aabbs.to_vec()];
-
-        loop {
-            let last_level = levels.last().unwrap();
-            let mut next_level = Vec::new();
-
-            for chunk in last_level.chunks(Self::GROUP_SIZE) {
-                let mut merged = chunk[0];
-                for aabb in &chunk[1..] {
-                    merged.merge(aabb)
-                }
-
-                next_level.push(merged);
-            }
-
-            if next_level.len() == 1 {
-                levels.push(next_level);
-                break;
-            }
-
-            levels.push(next_level);
-        }
-
-        Self { levels }
-    }
-
-    pub fn compute_interferences_with(
-        &self,
-        aabb1: AABB<f32>,
-        workspace: &mut WAABBHierarchyIntersections,
-    ) {
-        use ncollide::bounding_volume::BoundingVolume;
-
-        workspace.next_level_interferences.clear();
-        workspace.curr_level_interferences.clear();
-        workspace.curr_level_interferences.push(0);
-
-        for level in self.levels[1..].iter().rev() {
-            for i in &workspace.curr_level_interferences {
-                for j in 0..Self::GROUP_SIZE {
-                    if let Some(aabb2) = level.get(*i + j) {
-                        if aabb1.intersects(aabb2) {
-                            workspace
-                                .next_level_interferences
-                                .push((i + j) * Self::GROUP_SIZE)
-                        }
-                    }
-                }
-            }
-
-            std::mem::swap(
-                &mut workspace.curr_level_interferences,
-                &mut workspace.next_level_interferences,
-            );
-            workspace.next_level_interferences.clear();
-        }
-
-        // Last level.
-        for i in &workspace.curr_level_interferences {
-            for j in 0..Self::GROUP_SIZE {
-                if let Some(aabb2) = self.levels[0].get(*i + j) {
-                    if aabb1.intersects(aabb2) {
-                        workspace.next_level_interferences.push(i + j)
-                    }
-                }
-            }
-        }
-
-        std::mem::swap(
-            &mut workspace.curr_level_interferences,
-            &mut workspace.next_level_interferences,
-        );
-        workspace.next_level_interferences.clear();
-    }
-}
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index 10c0c8b..db83fa3 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -1,5 +1,6 @@
+use crate::data::pubsub::Subscription;
 use crate::dynamics::RigidBodySet;
-use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet};
+use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
 use crate::math::{Point, Vector, DIM};
 #[cfg(feature = "enhanced-determinism")]
 use crate::utils::FxHashMap32 as HashMap;
@@ -15,6 +16,41 @@ const NEXT_FREE_SENTINEL: u32 = u32::MAX;
 const SENTINEL_VALUE: f32 = f32::MAX;
 const CELL_WIDTH: f32 = 20.0;
 
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderPair {
+    pub collider1: ColliderHandle,
+    pub collider2: ColliderHandle,
+}
+
+impl ColliderPair {
+    pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
+        ColliderPair {
+            collider1,
+            collider2,
+        }
+    }
+
+    pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
+        if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
+            Self::new(collider1, collider2)
+        } else {
+            Self::new(collider2, collider1)
+        }
+    }
+
+    pub fn swap(self) -> Self {
+        Self::new(self.collider2, self.collider1)
+    }
+
+    pub fn zero() -> Self {
+        Self {
+            collider1: ColliderHandle::from_raw_parts(0, 0),
+            collider2: ColliderHandle::from_raw_parts(0, 0),
+        }
+    }
+}
+
 pub enum BroadPhasePairEvent {
     AddPair(ColliderPair),
     DeletePair(ColliderPair),
@@ -392,6 +428,7 @@ impl SAPRegion {
 pub struct BroadPhase {
     proxies: Proxies,
     regions: HashMap<Point<i32>, SAPRegion>,
+    removed_colliders: Option<Subscription<RemovedCollider>>,
     deleted_any: bool,
     // We could think serializing this workspace is useless.
     // It turns out is is important to serialize at least its capacity
@@ -480,6 +517,7 @@ impl BroadPhase {
     /// Create a new empty broad-phase.
     pub fn new() -> Self {
         BroadPhase {
+            removed_colliders: None,
             proxies: Proxies::new(),
             regions: HashMap::default(),
             reporting: HashMap::default(),
@@ -487,46 +525,60 @@ impl BroadPhase {
         }
     }
 
-    pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) {
-        for collider in handles.iter().filter_map(|h| colliders.get(*h)) {
-            if collider.proxy_index == crate::INVALID_USIZE {
-                // This collider has not been added to the broad-phase yet.
-                continue;
-            }
+    /// Maintain the broad-phase internal state by taking collider removal into account.
+    pub fn maintain(&mut self, colliders: &mut ColliderSet) {
+        // Ensure we already subscribed.
+        if self.removed_colliders.is_none() {
+            self.removed_colliders = Some(colliders.removed_colliders.subscribe());
+        }
 
-            let proxy = &mut self.proxies[collider.proxy_index];
+        let mut cursor = self.removed_colliders.take().unwrap();
+        for collider in colliders.removed_colliders.read(&cursor) {
+            self.remove_collider(collider.proxy_index);
+        }
 
-            // Push the proxy to infinity, but not beyond the sentinels.
-            proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
-            proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
-            // Discretize the AABB to find the regions that need to be invalidated.
-            let start = point_key(proxy.aabb.mins);
-            let end = point_key(proxy.aabb.maxs);
+        colliders.removed_colliders.ack(&mut cursor);
+        self.removed_colliders = Some(cursor);
+    }
 
-            #[cfg(feature = "dim2")]
-            for i in start.x..=end.x {
-                for j in start.y..=end.y {
-                    if let Some(region) = self.regions.get_mut(&Point::new(i, j)) {
-                        region.predelete_proxy(collider.proxy_index);
-                        self.deleted_any = true;
-                    }
+    fn remove_collider<'a>(&mut self, proxy_index: usize) {
+        if proxy_index == crate::INVALID_USIZE {
+            // This collider has not been added to the broad-phase yet.
+            return;
+        }
+
+        let proxy = &mut self.proxies[proxy_index];
+
+        // Push the proxy to infinity, but not beyond the sentinels.
+        proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0);
+        proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0);
+        // Discretize the AABB to find the regions that need to be invalidated.
+        let start = point_key(proxy.aabb.mins);
+        let end = point_key(proxy.aabb.maxs);
+
+        #[cfg(feature = "dim2")]
+        for i in start.x..=end.x {
+            for j in start.y..=end.y {
+                if let Some(region) = self.regions.get_mut(&Point::new(i, j)) {
+                    region.predelete_proxy(proxy_index);
+                    self.deleted_any = true;
                 }
             }
+        }
 
-            #[cfg(feature = "dim3")]
-            for i in start.x..=end.x {
-                for j in start.y..=end.y {
-                    for k in start.z..=end.z {
-                        if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) {
-                            region.predelete_proxy(collider.proxy_index);
-                            self.deleted_any = true;
-                        }
+        #[cfg(feature = "dim3")]
+        for i in start.x..=end.x {
+            for j in start.y..=end.y {
+                for k in start.z..=end.z {
+                    if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) {
+                        region.predelete_proxy(proxy_index);
+                        self.deleted_any = true;
                     }
                 }
             }
-
-            self.proxies.remove(collider.proxy_index);
         }
+
+        self.proxies.remove(proxy_index);
     }
 
     pub(crate) fn update_aabbs(
@@ -664,16 +716,13 @@ impl BroadPhase {
 mod test {
     use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
     use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
-    use crate::pipeline::PhysicsPipeline;
 
     #[test]
     fn test_add_update_remove() {
         let mut broad_phase = BroadPhase::new();
-        let mut narrow_phase = NarrowPhase::new();
         let mut bodies = RigidBodySet::new();
         let mut colliders = ColliderSet::new();
         let mut joints = JointSet::new();
-        let mut pipeline = PhysicsPipeline::new();
 
         let rb = RigidBodyBuilder::new_dynamic().build();
         let co = ColliderBuilder::ball(0.5).build();
@@ -682,15 +731,8 @@ mod test {
 
         broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
 
-        pipeline.remove_rigid_body(
-            hrb,
-            &mut broad_phase,
-            &mut narrow_phase,
-            &mut bodies,
-            &mut colliders,
-            &mut joints,
-        );
-
+        bodies.remove(hrb, &mut colliders, &mut joints);
+        broad_phase.maintain(&mut colliders);
         broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
 
         // Create another body.
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 2d55857..7c293b6 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,11 +1,12 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
     Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
-    Proximity, Triangle, Trimesh,
+    Proximity, Ray, RayIntersection, Triangle, Trimesh,
 };
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use na::Point3;
 use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+use ncollide::query::RayCast;
 use num::Zero;
 
 #[derive(Clone)]
@@ -97,6 +98,49 @@ impl Shape {
             Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
         }
     }
+
+    /// Computes the first intersection point between a ray in this collider.
+    ///
+    /// Some shapes are not supported yet and will always return `None`.
+    ///
+    /// # Parameters
+    /// - `position`: the position of this shape.
+    /// - `ray`: the ray to cast.
+    /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
+    ///   limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
+    pub fn cast_ray(
+        &self,
+        position: &Isometry<f32>,
+        ray: &Ray,
+        max_toi: f32,
+    ) -> Option<RayIntersection> {
+        match self {
+            Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true),
+            Shape::Polygon(_poly) => None,
+            Shape::Capsule(caps) => {
+                let pos = position * caps.transform_wrt_y();
+                let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius);
+                caps.toi_and_normal_with_ray(&pos, ray, max_toi, true)
+            }
+            Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true),
+            #[cfg(feature = "dim2")]
+            Shape::Triangle(_) | Shape::Trimesh(_) => {
+                // This is not implemented yet in 2D.
+                None
+            }
+            #[cfg(feature = "dim3")]
+            Shape::Triangle(triangle) => {
+                triangle.toi_and_normal_with_ray(position, ray, max_toi, true)
+            }
+            #[cfg(feature = "dim3")]
+            Shape::Trimesh(trimesh) => {
+                trimesh.toi_and_normal_with_ray(position, ray, max_toi, true)
+            }
+            Shape::HeightField(heightfield) => {
+                heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
+            }
+        }
+    }
 }
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -353,6 +397,12 @@ impl ColliderBuilder {
         self
     }
 
+    /// Sets the restitution coefficient of the collider this builder will build.
+    pub fn restitution(mut self, restitution: f32) -> Self {
+        self.restitution = restitution;
+        self
+    }
+
     /// Sets the density of the collider this builder will build.
     pub fn density(mut self, density: f32) -> Self {
         self.density = Some(density);
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index 22bba1b..5ac9658 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -1,14 +1,25 @@
 use crate::data::arena::Arena;
+use crate::data::pubsub::PubSub;
 use crate::dynamics::{RigidBodyHandle, RigidBodySet};
-use crate::geometry::Collider;
+use crate::geometry::{Collider, ColliderGraphIndex};
 use std::ops::{Index, IndexMut};
 
 /// The unique identifier of a collider added to a collider set.
 pub type ColliderHandle = crate::data::arena::Index;
 
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub(crate) struct RemovedCollider {
+    pub handle: ColliderHandle,
+    pub(crate) contact_graph_index: ColliderGraphIndex,
+    pub(crate) proximity_graph_index: ColliderGraphIndex,
+    pub(crate) proxy_index: usize,
+}
+
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 /// A set of colliders that can be handled by a physics `World`.
 pub struct ColliderSet {
+    pub(crate) removed_colliders: PubSub<RemovedCollider>,
     pub(crate) colliders: Arena<Collider>,
 }
 
@@ -16,6 +27,7 @@ impl ColliderSet {
     /// Create a new empty set of colliders.
     pub fn new() -> Self {
         ColliderSet {
+            removed_colliders: PubSub::new(),
             colliders: Arena::new(),
         }
     }
@@ -26,7 +38,7 @@ impl ColliderSet {
     }
 
     /// Iterate through all the colliders on this set.
-    pub fn iter(&self) -> impl Iterator<Item = (ColliderHandle, &Collider)> {
+    pub fn iter(&self) -> impl ExactSizeIterator<Item = (ColliderHandle, &Collider)> {
         self.colliders.iter()
     }
 
@@ -60,8 +72,35 @@ impl ColliderSet {
         handle
     }
 
-    pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option<Collider> {
-        self.colliders.remove(handle)
+    /// Remove a collider from this set and update its parent accordingly.
+    pub fn remove(
+        &mut self,
+        handle: ColliderHandle,
+        bodies: &mut RigidBodySet,
+    ) -> Option<Collider> {
+        let collider = self.colliders.remove(handle)?;
+
+        /*
+         * Delete the collider from its parent body.
+         */
+        if let Some(parent) = bodies.get_mut_internal(collider.parent) {
+            parent.remove_collider_internal(handle, &collider);
+            bodies.wake_up(collider.parent, true);
+        }
+
+        /*
+         * Publish removal.
+         */
+        let message = RemovedCollider {
+            handle,
+            contact_graph_index: collider.contact_graph_index,
+            proximity_graph_index: collider.proximity_graph_index,
+            proxy_index: collider.proxy_index,
+        };
+
+        self.removed_colliders.publish(message);
+
+        Some(collider)
     }
 
     /// Gets the collider with the given handle without a known generation.
diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
index f59a94b..04afc65 100644
--- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
@@ -71,10 +71,10 @@ fn do_generate_contacts(
             } else {
                 manifold.subshape_index_pair.1
             };
-            println!(
-                "Restoring for {} [chosen with {:?}]",
-                subshape_id, manifold.subshape_index_pair
-            );
+            // println!(
+            //     "Restoring for {} [chosen with {:?}]",
+            //     subshape_id, manifold.subshape_index_pair
+            // );
 
             // Use dummy shapes for the dispatch.
             #[cfg(feature = "dim2")]
diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
index 78f26bb..52ba9b7 100644
--- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
@@ -1,11 +1,11 @@
 use crate::geometry::contact_generator::{
     ContactGenerationContext, PrimitiveContactGenerationContext,
 };
-use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyIntersections};
+use crate::geometry::{Collider, ContactManifold, Shape, Trimesh};
 use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
 
 pub struct TrimeshShapeContactGeneratorWorkspace {
-    interferences: WAABBHierarchyIntersections,
+    interferences: Vec<usize>,
     local_aabb2: AABB<f32>,
     old_interferences: Vec<usize>,
     old_manifolds: Vec<ContactManifold>,
@@ -14,7 +14,7 @@ pub struct TrimeshShapeContactGeneratorWorkspace {
 impl TrimeshShapeContactGeneratorWorkspace {
     pub fn new() -> Self {
         Self {
-            interferences: WAABBHierarchyIntersections::new(),
+            interferences: Vec::new(),
             local_aabb2: AABB::new_invalid(),
             old_interferences: Vec::new(),
             old_manifolds: Vec::new(),
@@ -74,7 +74,7 @@ fn do_generate_contacts(
         let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value?
         std::mem::swap(
             &mut workspace.old_interferences,
-            workspace.interferences.computed_interferences_mut(),
+            &mut workspace.interferences,
         );
         std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds);
         ctxt.pair.manifolds.clear();
@@ -108,16 +108,17 @@ fn do_generate_contacts(
         //     workspace.old_manifolds.len()
         // );
 
+        workspace.interferences.clear();
         trimesh1
             .waabbs()
-            .compute_interferences_with(local_aabb2, &mut workspace.interferences);
+            .intersect_aabb(&local_aabb2, &mut workspace.interferences);
         workspace.local_aabb2 = local_aabb2;
     }
 
     /*
      * Dispatch to the specific solver by keeping the previous manifold if we already had one.
      */
-    let new_interferences = workspace.interferences.computed_interferences();
+    let new_interferences = &workspace.interferences;
     let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
     let mut old_manifolds_it = workspace.old_manifolds.drain(..);
 
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 4f72778..562f962 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -36,11 +36,15 @@ pub type AABB = ncollide::bounding_volume::AABB<f32>;
 pub type ContactEvent = ncollide::pipeline::ContactEvent<ColliderHandle>;
 /// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not).
 pub type ProximityEvent = ncollide::pipeline::ProximityEvent<ColliderHandle>;
+/// A ray that can be cast against colliders.
+pub type Ray = ncollide::query::Ray<f32>;
+/// The intersection between a ray and a  collider.
+pub type RayIntersection = ncollide::query::RayIntersection<f32>;
 
 #[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::ball::WBall;
-pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections};
-pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent;
+pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
+pub(crate) use self::collider_set::RemovedCollider;
 #[cfg(feature = "simd-is-enabled")]
 pub(crate) use self::contact::WContact;
 #[cfg(feature = "dim2")]
@@ -48,12 +52,11 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 #[cfg(feature = "dim3")]
 pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
-#[cfg(feature = "simd-is-enabled")]
-pub(crate) use self::waabb::WAABB;
+pub(crate) use self::waabb::{WRay, WAABB};
+pub(crate) use self::wquadtree::WQuadtree;
 //pub(crate) use self::z_order::z_cmp_floats;
 
 mod ball;
-mod broad_phase;
 mod broad_phase_multi_sap;
 mod capsule;
 mod collider;
@@ -75,6 +78,6 @@ mod proximity_detector;
 pub(crate) mod sat;
 pub(crate) mod triangle;
 mod trimesh;
-#[cfg(feature = "simd-is-enabled")]
 mod waabb;
+mod wquadtree;
 //mod z_order;
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs
index 1a36511..ebe0a79 100644
--- a/src/geometry/narrow_phase.rs
+++ b/src/geometry/narrow_phase.rs
@@ -14,13 +14,16 @@ use crate::geometry::proximity_detector::{
 //    proximity_detector::ProximityDetectionContextSimd, WBall,
 //};
 use crate::geometry::{
-    BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair,
+    BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent,
+    ProximityPair, RemovedCollider,
 };
 use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
 //#[cfg(feature = "simd-is-enabled")]
 //use crate::math::{SimdFloat, SIMD_WIDTH};
+use crate::data::pubsub::Subscription;
 use crate::ncollide::query::Proximity;
 use crate::pipeline::EventHandler;
+use std::collections::HashMap;
 //use simba::simd::SimdValue;
 
 /// The narrow-phase responsible for computing precise contact information between colliders.
@@ -28,6 +31,7 @@ use crate::pipeline::EventHandler;
 pub struct NarrowPhase {
     contact_graph: InteractionGraph<ContactPair>,
     proximity_graph: InteractionGraph<ProximityPair>,
+    removed_colliders: Option<Subscription<RemovedCollider>>,
     //    ball_ball: Vec<usize>,        // Workspace: Vec<*mut ContactPair>,
     //    shape_shape: Vec<usize>,      // Workspace: Vec<*mut ContactPair>,
     //    ball_ball_prox: Vec<usize>,   // Workspace: Vec<*mut ProximityPair>,
@@ -42,6 +46,7 @@ impl NarrowPhase {
         Self {
             contact_graph: InteractionGraph::new(),
             proximity_graph: InteractionGraph::new(),
+            removed_colliders: None,
             //            ball_ball: Vec::new(),
             //            shape_shape: Vec::new(),
             //            ball_ball_prox: Vec::new(),
@@ -73,45 +78,84 @@ impl NarrowPhase {
     //     &mut self.contact_graph.interactions
     // }
 
-    pub(crate) fn remove_colliders(
+    /// Maintain the narrow-phase internal state by taking collider removal into account.
+    pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) {
+        // Ensure we already subscribed.
+        if self.removed_colliders.is_none() {
+            self.removed_colliders = Some(colliders.removed_colliders.subscribe());
+        }
+
+        let mut cursor = self.removed_colliders.take().unwrap();
+
+        // TODO: avoid these hash-maps.
+        // They are necessary to handle the swap-remove done internally
+        // by the contact/proximity graphs when a node is removed.
+        let mut prox_id_remap = HashMap::new();
+        let mut contact_id_remap = HashMap::new();
+
+        for i in 0.. {
+            if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
+                let proximity_graph_id = prox_id_remap
+                    .get(&collider.handle)
+                    .copied()
+                    .unwrap_or(collider.proximity_graph_index);
+                let contact_graph_id = contact_id_remap
+                    .get(&collider.handle)
+                    .copied()
+                    .unwrap_or(collider.contact_graph_index);
+
+                self.remove_collider(
+                    proximity_graph_id,
+                    contact_graph_id,
+                    colliders,
+                    bodies,
+                    &mut prox_id_remap,
+                    &mut contact_id_remap,
+                );
+            } else {
+                break;
+            }
+        }
+
+        colliders.removed_colliders.ack(&mut cursor);
+        self.removed_colliders = Some(cursor);
+    }
+
+    pub(crate) fn remove_collider<'a>(
         &mut self,
-        handles: &[ColliderHandle],
+        proximity_graph_id: ColliderGraphIndex,
+        contact_graph_id: ColliderGraphIndex,
         colliders: &mut ColliderSet,
         bodies: &mut RigidBodySet,
+        prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
+        contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
     ) {
-        for handle in handles {
-            if let Some(collider) = colliders.get(*handle) {
-                let proximity_graph_id = collider.proximity_graph_index;
-                let contact_graph_id = collider.contact_graph_index;
-
-                // Wake up every body in contact with the deleted collider.
-                for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
-                    if let Some(pa