Merge branch 'master' into infinite_fall_memory

24 files changed, 1607 insertions, 585 deletions

diff --git a/src/data/mod.rs b/src/data/mod.rs
index 7c00442..5d3efa6 100644
--- a/src/data/mod.rs
+++ b/src/data/mod.rs
@@ -2,3 +2,4 @@
 
 pub mod arena;
 pub(crate) mod graph;
+pub mod pubsub;
diff --git a/src/data/pubsub.rs b/src/data/pubsub.rs
new file mode 100644
index 0000000..b2c9e27
--- /dev/null
+++ b/src/data/pubsub.rs
@@ -0,0 +1,175 @@
+//! Publish-subscribe mechanism for internal events.
+
+use std::collections::VecDeque;
+use std::marker::PhantomData;
+
+/// A permanent subscription to a pub-sub queue.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Subscription<T> {
+    // Position on the cursor array.
+    id: u32,
+    _phantom: PhantomData<T>,
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+struct PubSubCursor {
+    // Position on the offset array.
+    id: u32,
+    // Index of the next message to read.
+    // NOTE: Having this here is not actually necessary because
+    // this value is supposed to be equal to `offsets[self.id]`.
+    // However, we keep it because it lets us avoid one lookup
+    // on the `offsets` array inside of message-polling loops
+    // based on `read_ith`.
+    next: u32,
+}
+
+impl PubSubCursor {
+    fn id(&self, num_deleted: u32) -> usize {
+        (self.id - num_deleted) as usize
+    }
+
+    fn next(&self, num_deleted: u32) -> usize {
+        (self.next - num_deleted) as usize
+    }
+}
+
+/// A pub-sub queue.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct PubSub<T> {
+    deleted_messages: u32,
+    deleted_offsets: u32,
+    messages: VecDeque<T>,
+    offsets: VecDeque<u32>,
+    cursors: Vec<PubSubCursor>,
+}
+
+impl<T> PubSub<T> {
+    /// Create a new empty pub-sub queue.
+    pub fn new() -> Self {
+        Self {
+            deleted_offsets: 0,
+            deleted_messages: 0,
+            messages: VecDeque::new(),
+            offsets: VecDeque::new(),
+            cursors: Vec::new(),
+        }
+    }
+
+    fn reset_shifts(&mut self) {
+        for offset in &mut self.offsets {
+            *offset -= self.deleted_messages;
+        }
+
+        for cursor in &mut self.cursors {
+            cursor.id -= self.deleted_offsets;
+            cursor.next -= self.deleted_messages;
+        }
+
+        self.deleted_offsets = 0;
+        self.deleted_messages = 0;
+    }
+
+    /// Publish a new message.
+    pub fn publish(&mut self, message: T) {
+        if self.offsets.is_empty() {
+            // No subscribers, drop the message.
+            return;
+        }
+
+        self.messages.push_back(message);
+    }
+
+    /// Subscribe to the queue.
+    ///
+    /// A subscription cannot be cancelled.
+    #[must_use]
+    pub fn subscribe(&mut self) -> Subscription<T> {
+        let cursor = PubSubCursor {
+            next: self.messages.len() as u32 + self.deleted_messages,
+            id: self.offsets.len() as u32 + self.deleted_offsets,
+        };
+
+        let subscription = Subscription {
+            id: self.cursors.len() as u32,
+            _phantom: PhantomData,
+        };
+
+        self.offsets.push_back(cursor.next);
+        self.cursors.push(cursor);
+        subscription
+    }
+
+    /// Read the i-th message not yet read by the given subsciber.
+    pub fn read_ith(&self, sub: &Subscription<T>, i: usize) -> Option<&T> {
+        let cursor = &self.cursors[sub.id as usize];
+        self.messages
+            .get(cursor.next(self.deleted_messages) as usize + i)
+    }
+
+    /// Get the messages not yet read by the given subscriber.
+    pub fn read(&self, sub: &Subscription<T>) -> impl Iterator<Item = &T> {
+        let cursor = &self.cursors[sub.id as usize];
+        let next = cursor.next(self.deleted_messages);
+
+        // TODO: use self.queue.range(next..) once it is stabilised.
+        MessageRange {
+            queue: &self.messages,
+            next,
+        }
+    }
+
+    /// Makes the given subscribe acknowledge all the messages in the queue.
+    ///
+    /// A subscriber cannot read acknowledged messages any more.
+    pub fn ack(&mut self, sub: &Subscription<T>) {
+        // Update the cursor.
+        let cursor = &mut self.cursors[sub.id as usize];
+
+        self.offsets[cursor.id(self.deleted_offsets)] = u32::MAX;
+        cursor.id = self.offsets.len() as u32 + self.deleted_offsets;
+
+        cursor.next = self.messages.len() as u32 + self.deleted_messages;
+        self.offsets.push_back(cursor.next);
+
+        // Now clear the messages we don't need to
+        // maintain in memory anymore.
+        while self.offsets.front() == Some(&u32::MAX) {
+            self.offsets.pop_front();
+            self.deleted_offsets += 1;
+        }
+
+        // There must be at least one offset otherwise
+        // that would mean we have no subscribers.
+        let next = self.offsets.front().unwrap();
+        let num_to_delete = *next - self.deleted_messages;
+
+        for _ in 0..num_to_delete {
+            self.messages.pop_front();
+        }
+
+        self.deleted_messages += num_to_delete;
+
+        if self.deleted_messages > u32::MAX / 2 || self.deleted_offsets > u32::MAX / 2 {
+            // Don't let the deleted_* shifts grow indefinitely otherwise
+            // they will end up overflowing, breaking everything.
+            self.reset_shifts();
+        }
+    }
+}
+
+struct MessageRange<'a, T> {
+    queue: &'a VecDeque<T>,
+    next: usize,
+}
+
+impl<'a, T> Iterator for MessageRange<'a, T> {
+    type Item = &'a T;
+
+    #[inline(always)]
+    fn next(&mut self) -> Option<&'a T> {
+        let result = self.queue.get(self.next);
+        self.next += 1;
+        result
+    }
+}
diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index faf22e1..b31c3f6 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -2,7 +2,7 @@
 #[derive(Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct IntegrationParameters {
-    /// The timestep (default: `1.0 / 60.0`)
+    /// The timestep length (default: `1.0 / 60.0`)
     dt: f32,
     /// The inverse of `dt`.
     inv_dt: f32,
diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs
index 51d7432..2b1895f 100644
--- a/src/dynamics/joint/joint_set.rs
+++ b/src/dynamics/joint/joint_set.rs
@@ -66,17 +66,21 @@ impl JointSet {
     }
 
     /// Iterates through all the joint on this set.
-    pub fn iter(&self) -> impl Iterator<Item = &Joint> {
-        self.joint_graph.graph.edges.iter().map(|e| &e.weight)
+    pub fn iter(&self) -> impl Iterator<Item = (JointHandle, &Joint)> {
+        self.joint_graph
+            .graph
+            .edges
+            .iter()
+            .map(|e| (e.weight.handle, &e.weight))
     }
 
     /// Iterates mutably through all the joint on this set.
-    pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Joint> {
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (JointHandle, &mut Joint)> {
         self.joint_graph
             .graph
             .edges
             .iter_mut()
-            .map(|e| &mut e.weight)
+            .map(|e| (e.weight.handle, &mut e.weight))
     }
 
     // /// The set of joints as an array.
@@ -202,8 +206,8 @@ impl JointSet {
                 }
 
                 // Wake up the attached bodies.
-                bodies.wake_up(h1);
-                bodies.wake_up(h2);
+                bodies.wake_up(h1, true);
+                bodies.wake_up(h2, true);
             }
 
             if let Some(other) = self.joint_graph.remove_node(deleted_id) {
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index a2fcacc..af1fb4a 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -137,6 +137,15 @@ impl RigidBody {
         crate::utils::inv(self.mass_properties.inv_mass)
     }
 
+    /// The predicted position of this rigid-body.
+    ///
+    /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position`
+    /// method and is used for estimating the kinematic body velocity at the next timestep.
+    /// For non-kinematic bodies, this value is currently unspecified.
+    pub fn predicted_position(&self) -> &Isometry<f32> {
+        &self.predicted_position
+    }
+
     /// Adds a collider to this rigid-body.
     pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
         let mass_properties = coll
@@ -172,10 +181,13 @@ impl RigidBody {
     }
 
     /// Wakes up this rigid body if it is sleeping.
-    pub fn wake_up(&mut self) {
+    ///
+    /// If `strong` is `true` then it is assured that the rigid-body will
+    /// remain awake during multiple subsequent timesteps.
+    pub fn wake_up(&mut self, strong: bool) {
         self.activation.sleeping = false;
 
-        if self.activation.energy == 0.0 && self.is_dynamic() {
+        if (strong || self.activation.energy == 0.0) && self.is_dynamic() {
             self.activation.energy = self.activation.threshold.abs() * 2.0;
         }
     }
@@ -189,9 +201,18 @@ impl RigidBody {
 
     /// Is this rigid body sleeping?
     pub fn is_sleeping(&self) -> bool {
+        // TODO: should we:
+        // - return false for static bodies.
+        // - return true for non-sleeping dynamic bodies.
+        // - return true only for kinematic bodies with non-zero velocity?
         self.activation.sleeping
     }
 
+    /// Is the velocity of this body not zero?
+    pub fn is_moving(&self) -> bool {
+        !self.linvel.is_zero() || !self.angvel.is_zero()
+    }
+
     fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
         let com = &self.position * self.mass_properties.local_com;
         let shift = Translation::from(com.coords);
@@ -201,12 +222,17 @@ impl RigidBody {
         self.position = self.integrate_velocity(dt) * self.position;
     }
 
-    /// Sets the position of this rigid body.
+    /// Sets the position and `next_kinematic_position` of this rigid body.
+    ///
+    /// This will teleport the rigid-body to the specified position/orientation,
+    /// completely ignoring any physics rule. If this body is kinematic, this will
+    /// also set the next kinematic position to the same value, effectively
+    /// resetting to zero the next interpolated velocity of the kinematic body.
     pub fn set_position(&mut self, pos: Isometry<f32>) {
         self.position = pos;
 
         // TODO: update the predicted position for dynamic bodies too?
-        if self.is_static() {
+        if self.is_static() || self.is_kinematic() {
             self.predicted_position = pos;
         }
     }
diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs
index 99d6f10..83f1c51 100644
--- a/src/dynamics/rigid_body_set.rs
+++ b/src/dynamics/rigid_body_set.rs
@@ -2,8 +2,8 @@
 use rayon::prelude::*;
 
 use crate::data::arena::Arena;
-use crate::dynamics::{Joint, RigidBody};
-use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
+use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
+use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
 use crossbeam::channel::{Receiver, Sender};
 use std::ops::{Deref, DerefMut, Index, IndexMut};
 
@@ -128,9 +128,23 @@ impl RigidBodySet {
 
     pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
         let mut rb = &mut self.bodies[handle];
-        if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
-            rb.active_set_id = self.active_dynamic_set.len();
-            self.active_dynamic_set.push(handle);
+        match rb.body_status {
+            // XXX: this case should only concern the dynamic bodies.
+            // For static bodies we should use the modified_inactive_set, or something
+            // similar. Right now we do this for static bodies as well so the broad-phase
+            // takes them into account the first time they are inserted.
+            BodyStatus::Dynamic | BodyStatus::Static => {
+                if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
+                    rb.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
+            }
+            BodyStatus::Kinematic => {
+                if self.active_kinematic_set.get(rb.active_set_id) != Some(&handle) {
+                    rb.active_set_id = self.active_kinematic_set.len();
+                    self.active_kinematic_set.push(handle);
+                }
+            }
         }
     }
 
@@ -143,13 +157,14 @@ impl RigidBodySet {
     pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
         let handle = self.bodies.insert(rb);
         let rb = &mut self.bodies[handle];
-        rb.active_set_id = self.active_dynamic_set.len();
 
         if !rb.is_sleeping() && rb.is_dynamic() {
+            rb.active_set_id = self.active_dynamic_set.len();
             self.active_dynamic_set.push(handle);
         }
 
         if rb.is_kinematic() {
+            rb.active_set_id = self.active_kinematic_set.len();
             self.active_kinematic_set.push(handle);
         }
 
@@ -160,29 +175,57 @@ impl RigidBodySet {
         handle
     }
 
-    pub(crate) fn num_islands(&self) -> usize {
-        self.active_islands.len() - 1
-    }
-
-    pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
+    /// Removes a rigid-body, and all its attached colliders and joints, from these sets.
+    pub fn remove(
+        &mut self,
+        handle: RigidBodyHandle,
+        colliders: &mut ColliderSet,
+        joints: &mut JointSet,
+    ) -> Option<RigidBody> {
         let rb = self.bodies.remove(handle)?;
-        // Remove this body from the active dynamic set.
-        if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
-            self.active_dynamic_set.swap_remove(rb.active_set_id);
+        /*
+         * Update active sets.
+         */
+        let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
+
+        for active_set in &mut active_sets {
+            if active_set.get(rb.active_set_id) == Some(&handle) {
+                active_set.swap_remove(rb.active_set_id);
 
-            if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) {
-                self.bodies[*replacement].active_set_id = rb.active_set_id;
+                if let Some(replacement) = active_set.get(rb.active_set_id) {
+                    self.bodies[*replacement].active_set_id = rb.active_set_id;
+                }
             }
         }
 
+        /*
+         * Remove colliders attached to this rigid-body.
+         */
+        for collider in &rb.colliders {
+            colliders.remove(*collider, self);
+        }
+
+        /*
+         * Remove joints attached to this rigid-body.
+         */
+        joints.remove_rigid_body(rb.joint_graph_index, self);
+
         Some(rb)
     }
 
+    pub(crate) fn num_islands(&self) -> usize {
+        self.active_islands.len() - 1
+    }
+
     /// Forces the specified rigid-body to wake up if it is dynamic.
-    pub fn wake_up(&mut self, handle: RigidBodyHandle) {
+    ///
+    /// If `strong` is `true` then it is assured that the rigid-body will
+    /// remain awake during multiple subsequent timesteps.
+    pub fn wake_up(&mut self, handle: RigidBodyHandle, strong: bool) {
         if let Some(rb) = self.bodies.get_mut(handle) {
+            // TODO: what about kinematic bodies?
             if rb.is_dynamic() {
-                rb.wake_up();
+                rb.wake_up(strong);
 
                 if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
                     rb.active_set_id = self.active_dynamic_set.len();
@@ -214,8 +257,11 @@ impl RigidBodySet {
     ///
     /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
     /// suffer form the ABA problem.
-    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
-        self.bodies.get_unknown_gen_mut(i)
+    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(RigidBodyMut, RigidBodyHandle)> {
+        let sender = &self.activation_channel.0;
+        self.bodies
+            .get_unknown_gen_mut(i)
+            .map(|(rb, handle)| (RigidBodyMut::new(handle, rb, sender), handle))
     }
 
     /// Gets the rigid-body with the given handle.
@@ -256,6 +302,16 @@ impl RigidBodySet {
             .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
     }
 
+    /// Iter through all the active kinematic rigid-bodies on this set.
+    pub fn iter_active_kinematic<'a>(
+        &'a self,
+    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
+        let bodies: &'a _ = &self.bodies;
+        self.active_kinematic_set
+            .iter()
+            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+    }
+
     /// Iter through all the active dynamic rigid-bodies on this set.
     pub fn iter_active_dynamic<'a>(
         &'a self,
@@ -425,6 +481,45 @@ impl RigidBodySet {
             }
         }
 
+        // Read all the contacts and push objects touching touching this rigid-body.
+        #[inline(always)]
+        fn push_contacting_colliders(
+            rb: &RigidBody,
+            colliders: &ColliderSet,
+            contact_graph: &InteractionGraph<ContactPair>,
+            stack: &mut Vec<ColliderHandle>,
+        ) {
+            for collider_handle in &rb.colliders {
+                let collider = &colliders[*collider_handle];
+
+                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
+                    for manifold in &inter.2.manifolds {
+                        if manifold.num_active_contacts() > 0 {
+                            let other =
+                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
+                            let other_body = colliders[other].parent;
+                            stack.push(other_body);
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+
+        // Now iterate on all active kinematic bodies and push all the bodies
+        // touching them to the stack so they can be woken up.
+        for h in self.active_kinematic_set.iter() {
+            let rb = &self.bodies[*h];
+
+            if !rb.is_moving() {
+                // If the kinematic body does not move, it does not have
+                // to wake up any dynamic body.
+                continue;
+            }
+
+            push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
+        }
+
         //        println!("Selection: {}", instant::now() - t);
 
         //        let t = instant::now();
@@ -443,7 +538,9 @@ impl RigidBodySet {
                 // We already visited this body and its neighbors.
                 // Also, we don't propagate awake state through static bodies.
                 continue;
-            } else if self.stack.len() < island_marker {
+            }
+
+            if self.stack.len() < island_marker {
                 if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
                     >= min_island_size
                 {
@@ -454,29 +551,16 @@ impl RigidBodySet {
                 island_marker = self.stack.len();
             }
 
-            rb.wake_up();
+            rb.wake_up(false);
             rb.active_island_id = self.active_islands.len() - 1;
             rb.active_set_id = self.active_dynamic_set.len();
             rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
             rb.active_set_timestamp = self.active_set_timestamp;
             self.active_dynamic_set.push(handle);
 
-            // Read all the contacts and push objects touching this one.
-            for collider_handle in &rb.colliders {
-                let collider = &colliders[*collider_handle];
-
-                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
-                    for manifold in &inter.2.manifolds {
-                        if manifold.num_active_contacts() > 0 {
-                            let other =
-                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
-                            let other_body = colliders[other].parent;
-                            self.stack.push(other_body);
-                            break;
-                        }
-                    }
-                }
-            }
+            // Transmit the active state to all the rigid-bodies with colliders
+            // in contact or joined with this collider.
+            push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
 
             for inter in joint_graph.interactions_with(rb.joint_graph_index) {
                 let other = crate::utils::other_handle((inter.0, inter.1), handle);
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