First public release of Rapier.

src/geometry/ball.rs

@@ -0,0 +1,16 @@
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::{Point, SimdFloat};
+
+#[cfg(feature = "simd-is-enabled")]
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WBall {
+    pub center: Point<SimdFloat>,
+    pub radius: SimdFloat,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+impl WBall {
+    pub fn new(center: Point<SimdFloat>, radius: SimdFloat) -> Self {
+        WBall { center, radius }
+    }
+}

src/geometry/broad_phase.rs

@@ -0,0 +1,255 @@
+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();
+    }
+}

src/geometry/broad_phase_multi_sap.rs

@@ -0,0 +1,645 @@
+use crate::dynamics::RigidBodySet;
+use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet};
+use crate::math::{Point, Vector, DIM};
+#[cfg(feature = "enhanced-determinism")]
+use crate::utils::FxHashMap32 as HashMap;
+use bit_vec::BitVec;
+use ncollide::bounding_volume::{BoundingVolume, AABB};
+#[cfg(not(feature = "enhanced-determinism"))]
+use rustc_hash::FxHashMap as HashMap;
+use std::cmp::Ordering;
+use std::ops::{Index, IndexMut};
+
+const NUM_SENTINELS: usize = 1;
+const NEXT_FREE_SENTINEL: u32 = u32::MAX;
+const SENTINEL_VALUE: f32 = f32::MAX;
+const CELL_WIDTH: f32 = 20.0;
+
+pub enum BroadPhasePairEvent {
+    AddPair(ColliderPair),
+    DeletePair(ColliderPair),
+}
+
+fn sort2(a: u32, b: u32) -> (u32, u32) {
+    assert_ne!(a, b);
+
+    if a < b {
+        (a, b)
+    } else {
+        (b, a)
+    }
+}
+
+fn point_key(point: Point<f32>) -> Point<i32> {
+    (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into()
+}
+
+fn region_aabb(index: Point<i32>) -> AABB<f32> {
+    let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into();
+    let maxs = mins + Vector::repeat(CELL_WIDTH);
+    AABB::new(mins, maxs)
+}
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+struct Endpoint {
+    value: f32,
+    packed_flag_proxy: u32,
+}
+
+const START_FLAG_MASK: u32 = 0b1 << 31;
+const PROXY_MASK: u32 = u32::MAX ^ START_FLAG_MASK;
+const START_SENTINEL_TAG: u32 = u32::MAX;
+const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK;
+
+impl Endpoint {
+    pub fn start_endpoint(value: f32, proxy: u32) -> Self {
+        Self {
+            value,
+            packed_flag_proxy: proxy | START_FLAG_MASK,
+        }
+    }
+
+    pub fn end_endpoint(value: f32, proxy: u32) -> Self {
+        Self {
+            value,
+            packed_flag_proxy: proxy & PROXY_MASK,
+        }
+    }
+
+    pub fn start_sentinel() -> Self {
+        Self {
+            value: -SENTINEL_VALUE,
+            packed_flag_proxy: START_SENTINEL_TAG,
+        }
+    }
+
+    pub fn end_sentinel() -> Self {
+        Self {
+            value: SENTINEL_VALUE,
+            packed_flag_proxy: END_SENTINEL_TAG,
+        }
+    }
+
+    pub fn is_sentinel(self) -> bool {
+        self.packed_flag_proxy & PROXY_MASK == PROXY_MASK
+    }
+
+    pub fn proxy(self) -> u32 {
+        self.packed_flag_proxy & PROXY_MASK
+    }
+
+    pub fn is_start(self) -> bool {
+        (self.packed_flag_proxy & START_FLAG_MASK) != 0
+    }
+
+    pub fn is_end(self) -> bool {
+        (self.packed_flag_proxy & START_FLAG_MASK) == 0
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+struct SAPAxis {
+    min_bound: f32,
+    max_bound: f32,
+    endpoints: Vec<Endpoint>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    new_endpoints: Vec<(Endpoint, usize)>, // Workspace
+}
+
+impl SAPAxis {
+    fn new(min_bound: f32, max_bound: f32) -> Self {
+        assert!(min_bound <= max_bound);
+
+        Self {
+            min_bound,
+            max_bound,
+            endpoints: vec![Endpoint::start_sentinel(), Endpoint::end_sentinel()],
+            new_endpoints: Vec::new(),
+        }
+    }
+
+    fn batch_insert(
+        &mut self,
+        dim: usize,
+        new_proxies: &[usize],
+        proxies: &Proxies,
+        reporting: Option<&mut HashMap<(u32, u32), bool>>,
+    ) {
+        if new_proxies.is_empty() {
+            return;
+        }
+
+        self.new_endpoints.clear();
+
+        for proxy_id in new_proxies {
+            let proxy = &proxies[*proxy_id];
+            assert!(proxy.aabb.mins[dim] <= self.max_bound);
+            assert!(proxy.aabb.maxs[dim] >= self.min_bound);
+            let start_endpoint = Endpoint::start_endpoint(proxy.aabb.mins[dim], *proxy_id as u32);
+            let end_endpoint = Endpoint::end_endpoint(proxy.aabb.maxs[dim], *proxy_id as u32);
+
+            self.new_endpoints.push((start_endpoint, 0));
+            self.new_endpoints.push((end_endpoint, 0));
+        }
+
+        self.new_endpoints
+            .sort_by(|a, b| a.0.value.partial_cmp(&b.0.value).unwrap_or(Ordering::Equal));
+
+        let mut curr_existing_index = self.endpoints.len() - NUM_SENTINELS - 1;
+        let new_num_endpoints = self.endpoints.len() + self.new_endpoints.len();
+        self.endpoints
+            .resize(new_num_endpoints, Endpoint::end_sentinel());
+        let mut curr_shift_index = new_num_endpoints - NUM_SENTINELS - 1;
+
+        // Sort the endpoints.
+        // TODO: specialize for the case where this is the
+        // first time we insert endpoints to this axis?
+        for new_endpoint in self.new_endpoints.iter_mut().rev() {
+            loop {
+                let existing_endpoint = self.endpoints[curr_existing_index];
+                if existing_endpoint.value <= new_endpoint.0.value {
+                    break;
+                }
+
+                self.endpoints[curr_shift_index] = existing_endpoint;
+
+                curr_shift_index -= 1;
+                curr_existing_index -= 1;
+            }
+
+            self.endpoints[curr_shift_index] = new_endpoint.0;
+            new_endpoint.1 = curr_shift_index;
+            curr_shift_index -= 1;
+        }
+
+        // Report pairs using a single mbp pass on each new endpoint.
+        let endpoints_wo_last_sentinel = &self.endpoints[..self.endpoints.len() - 1];
+        if let Some(reporting) = reporting {
+            for (endpoint, endpoint_id) in self.new_endpoints.drain(..).filter(|e| e.0.is_start()) {
+                let proxy1 = &proxies[endpoint.proxy() as usize];
+                let min = endpoint.value;
+                let max = proxy1.aabb.maxs[dim];
+
+                for endpoint2 in &endpoints_wo_last_sentinel[endpoint_id + 1..] {
+                    if endpoint2.proxy() == endpoint.proxy() {
+                        continue;
+                    }
+
+                    let proxy2 = &proxies[endpoint2.proxy() as usize];
+
+                    // NOTE: some pairs with equal aabb.mins[dim] may end up being reported twice.
+                    if (endpoint2.is_start() && endpoint2.value < max)
+                        || (endpoint2.is_end() && proxy2.aabb.mins[dim] <= min)
+                    {
+                        // Report pair.
+                        if proxy1.aabb.intersects(&proxy2.aabb) {
+                            // Report pair.
+                            let pair = sort2(endpoint.proxy(), endpoint2.proxy());
+                            reporting.insert(pair, true);
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> bool {
+        let mut deleted_any = false;
+        for endpoint in &self.endpoints {
+            if endpoint.value < self.min_bound {
+                if endpoint.is_end() {
+                    existing_proxies.set(endpoint.proxy() as usize, false);
+                    deleted_any = true;
+                }
+            } else {
+                break;
+            }
+        }
+
+        for endpoint in self.endpoints.iter().rev() {
+            if endpoint.value > self.max_bound {
+                if endpoint.is_start() {
+                    existing_proxies.set(endpoint.proxy() as usize, false);
+                    deleted_any = true;
+                }
+            } else {
+                break;
+            }
+        }
+
+        deleted_any
+    }
+
+    fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) {
+        self.endpoints
+            .retain(|endpt| endpt.is_sentinel() || existing_proxies[endpt.proxy() as usize])
+    }
+
+    fn update_endpoints(
+        &mut self,
+        dim: usize,
+        proxies: &Proxies,
+        reporting: &mut HashMap<(u32, u32), bool>,
+    ) {
+        let last_endpoint = self.endpoints.len() - NUM_SENTINELS;
+        for i in NUM_SENTINELS..last_endpoint {
+            let mut endpoint_i = self.endpoints[i];
+            let aabb_i = proxies[endpoint_i.proxy() as usize].aabb;
+
+            if endpoint_i.is_start() {
+                endpoint_i.value = aabb_i.mins[dim];
+            } else {
+                endpoint_i.value = aabb_i.maxs[dim];
+            }
+
+            let mut j = i;
+
+            if endpoint_i.is_start() {
+                while endpoint_i.value < self.endpoints[j - 1].value {
+                    let endpoint_j = self.endpoints[j - 1];
+                    self.endpoints[j] = endpoint_j;
+
+                    if endpoint_j.is_end() {
+                        // Report start collision.
+                        if aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].aabb) {
+                            let pair = sort2(endpoint_i.proxy(), endpoint_j.proxy());
+                            reporting.insert(pair, true);
+                        }
+                    }
+
+                    j -= 1;
+                }
+            } else {
+                while endpoint_i.value < self.endpoints[j - 1].value {
+                    let endpoint_j = self.endpoints[j - 1];
+                    self.endpoints[j] = endpoint_j;
+
+                    if endpoint_j.is_start() {
+                        // Report end collision.
+                        if !aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].aabb) {
+                            let pair = sort2(endpoint_i.proxy(), endpoint_j.proxy());
+                            reporting.insert(pair, false);
+                        }
+                    }
+
+                    j -= 1;
+                }
+            }
+
+            self.endpoints[j] = endpoint_i;
+        }
+
+        // println!(
+        //     "Num start swaps: {}, end swaps: {}, dim: {}",
+        //     num_start_swaps, num_end_swaps, dim
+        // );
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+struct SAPRegion {
+    axii: [SAPAxis; DIM],
+    existing_proxies: BitVec,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    to_insert: Vec<usize>, // Workspace
+    need_update: bool,
+}
+
+impl SAPRegion {
+    pub fn new(bounds: AABB<f32>) -> Self {
+        let axii = [
+            SAPAxis::new(bounds.mins.x, bounds.maxs.x),
+            SAPAxis::new(bounds.mins.y, bounds.maxs.y),
+            #[cfg(feature = "dim3")]
+            SAPAxis::new(bounds.mins.z, bounds.maxs.z),
+        ];
+        SAPRegion {
+            axii,
+            existing_proxies: BitVec::new(),
+            to_insert: Vec::new(),
+            need_update: false,
+        }
+    }
+
+    pub fn predelete_proxy(&mut self, _proxy_id: usize) {
+        // 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.
+        self.need_update = true;
+    }
+
+    pub fn preupdate_proxy(&mut self, proxy_id: usize) -> bool {
+        let mask_len = self.existing_proxies.len();
+        if proxy_id >= mask_len {
+            self.existing_proxies.grow(proxy_id + 1 - mask_len, false);
+        }
+
+        if !self.existing_proxies[proxy_id] {
+            self.to_insert.push(proxy_id);
+            self.existing_proxies.set(proxy_id, true);
+            false
+        } else {
+            self.need_update = true;
+            true
+        }
+    }
+
+    pub fn update(&mut self, proxies: &Proxies, reporting: &mut HashMap<(u32, u32), bool>) {
+        if self.need_update {
+            // Update endpoints.
+            let mut deleted_any = false;
+            for dim in 0..DIM {
+                self.axii[dim].update_endpoints(dim, proxies, reporting);
+                deleted_any = self.axii[dim]
+                    .delete_out_of_bounds_proxies(&mut self.existing_proxies)
+                    || deleted_any;
+            }
+
+            if deleted_any {
+                for dim in 0..DIM {
+                    self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
+                }
+            }
+
+            self.need_update = false;
+        }
+
+        if !self.to_insert.is_empty() {
+            // Insert new proxies.
+            for dim in 1..DIM {
+                self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None);
+            }
+            self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
+            self.to_insert.clear();
+        }
+    }
+}
+
+/// A broad-phase based on multiple Sweep-and-Prune instances running of disjoint region of the 3D world.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct BroadPhase {
+    proxies: Proxies,
+    regions: HashMap<Point<i32>, SAPRegion>,
+    deleted_any: bool,
+    // We could think serializing this workspace is useless.
+    // It turns out is is important to serialize at least its capacity
+    // and restore this capacity when deserializing the hashmap.
+    // This is because the order of future elements inserted into the
+    // hashmap depends on its capacity (because the internal bucket indices
+    // depend on this capacity). So not restoring this capacity may alter
+    // the order at which future elements are reported. This will in turn
+    // alter the order at which the pairs are registered in the narrow-phase,
+    // thus altering the order of the contact manifold. In the end, this
+    // alters the order of the resolution of contacts, resulting in
+    // diverging simulation after restoration of a snapshot.
+    #[cfg_attr(
+        feature = "serde-serialize",
+        serde(
+            serialize_with = "crate::utils::serialize_hashmap_capacity",
+            deserialize_with = "crate::utils::deserialize_hashmap_capacity"
+        )
+    )]
+    reporting: HashMap<(u32, u32), bool>, // Workspace
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub(crate) struct BroadPhaseProxy {
+    handle: ColliderHandle,
+    aabb: AABB<f32>,
+    next_free: u32,
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+struct Proxies {
+    elements: Vec<BroadPhaseProxy>,
+    first_free: u32,
+}
+
+impl Proxies {
+    pub fn new() -> Self {
+        Self {
+            elements: Vec::new(),
+            first_free: NEXT_FREE_SENTINEL,
+        }
+    }
+
+    pub fn insert(&mut self, proxy: BroadPhaseProxy) -> usize {
+        if self.first_free != NEXT_FREE_SENTINEL {
+            let proxy_id = self.first_free;
+            self.first_free = self.elements[self.first_free as usize].next_free;
+            self.elements[self.first_free as usize] = proxy;
+            proxy_id as usize
+        } else {
+            self.elements.push(proxy);
+            self.elements.len() - 1
+        }
+    }
+
+    pub fn remove(&mut self, proxy_id: usize) {
+        self.elements[proxy_id].next_free = self.first_free;
+        self.first_free = proxy_id as u32;
+    }
+
+    // // FIXME: take holes into account?
+    // pub fn get(&self, i: usize) -> Option<&BroadPhaseProxy> {
+    //     self.elements.get(i)
+    // }
+
+    // FIXME: take holes into account?
+    pub fn get_mut(&mut self, i: usize) -> Option<&mut BroadPhaseProxy> {
+        self.elements.get_mut(i)
+    }
+}
+
+impl Index<usize> for Proxies {
+    type Output = BroadPhaseProxy;
+    fn index(&self, i: usize) -> &BroadPhaseProxy {
+        self.elements.index(i)
+    }
+}
+
+impl IndexMut<usize> for Proxies {
+    fn index_mut(&mut self, i: usize) -> &mut BroadPhaseProxy {
+        self.elements.index_mut(i)
+    }
+}
+
+impl BroadPhase {
+    /// Create a new empty broad-phase.
+    pub fn new() -> Self {
+        BroadPhase {
+            proxies: Proxies::new(),
+            regions: HashMap::default(),
+            reporting: HashMap::default(),
+            deleted_any: false,
+        }
+    }
+
+    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;
+            }
+
+            let proxy = &mut self.proxies[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);
+
+            #[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;
+                    }
+                }
+            }
+
+            #[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;
+                        }
+                    }
+                }
+            }
+
+            self.proxies.remove(collider.proxy_index);
+        }
+    }
+
+    pub(crate) fn update_aabbs(
+        &mut self,
+        prediction_distance: f32,
+        bodies: &RigidBodySet,
+        colliders: &mut ColliderSet,
+    ) {
+        // First, if we have any pending removals we have
+        // to deal with them now because otherwise we will
+        // end up with an ABA problems when reusing proxy
+        // ids.
+        self.complete_removals();
+
+        for body_handle in bodies
+            .active_dynamic_set
+            .iter()
+            .chain(bodies.active_kinematic_set.iter())
+        {
+            for handle in &bodies[*body_handle].colliders {
+                let collider = &mut colliders[*handle];
+                let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0);
+
+                if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) {
+                    proxy.aabb = aabb;
+                } else {
+                    let proxy = BroadPhaseProxy {
+                        handle: *handle,
+                        aabb,
+                        next_free: NEXT_FREE_SENTINEL,
+                    };
+                    collider.proxy_index = self.proxies.insert(proxy);
+                }
+
+                // Discretize the aabb.
+                let proxy_id = collider.proxy_index;
+                // let start = Point::origin();
+                // let end = Point::origin();
+                let start = point_key(aabb.mins);
+                let end = point_key(aabb.maxs);
+
+                #[cfg(feature = "dim2")]
+                for i in start.x..=end.x {
+                    for j in start.y..=end.y {
+                        let region_key = Point::new(i, j);
+                        let region_bounds = region_aabb(region_key);
+                        let region = self
+                            .regions
+                            .entry(region_key)
+                            .or_insert_with(|| SAPRegion::new(region_bounds));
+                        let _ = region.preupdate_proxy(proxy_id);
+                    }
+                }
+
+                #[cfg(feature = "dim3")]
+                for i in start.x..=end.x {
+                    for j in start.y..=end.y {
+                        for k in start.z..=end.z {
+                            let region_key = Point::new(i, j, k);
+                            let region_bounds = region_aabb(region_key);
+                            let region = self
+                                .regions
+                                .entry(region_key)
+                                .or_insert_with(|| SAPRegion::new(region_bounds));
+                            let _ = region.preupdate_proxy(proxy_id);
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    pub(crate) fn complete_removals(&mut self) {
+        if self.deleted_any {
+            for (_, region) in &mut self.regions {
+                region.update(&self.proxies, &mut self.reporting);
+            }
+
+            // NOTE: we don't care about reporting pairs.
+            self.reporting.clear();
+            self.deleted_any = false;
+        }
+    }
+
+    pub(crate) fn find_pairs(&mut self, out_events: &mut Vec<BroadPhasePairEvent>) {
+        // println!("num regions: {}", self.regions.len());
+
+        self.reporting.clear();
+        for (_, region) in &mut self.regions {
+            region.update(&self.proxies, &mut self.reporting)
+        }
+
+        // Convert reports to broad phase events.
+        // let t = instant::now();
+        // let mut num_add_events = 0;
+        // let mut num_delete_events = 0;
+
+        for ((proxy1, proxy2), colliding) in &self.reporting {
+            let proxy1 = &self.proxies[*proxy1 as usize];
+            let proxy2 = &self.proxies[*proxy2 as usize];
+
+            let handle1 = proxy1.handle;
+            let handle2 = proxy2.handle;
+
+            if *colliding {
+                out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(
+                    handle1, handle2,
+                )));
+            // num_add_events += 1;
+            } else {
+                out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new(
+                    handle1, handle2,
+                )));
+                // num_delete_events += 1;
+            }
+        }
+
+        // println!(
+        //     "Event conversion time: {}, add: {}/{}, delete: {}/{}",
+        //     instant::now() - t,
+        //     num_add_events,
+        //     out_events.len(),
+        //     num_delete_events,
+        //     out_events.len()
+        // );
+    }
+}

src/geometry/capsule.rs

@@ -0,0 +1,168 @@
+use crate::geometry::AABB;
+use crate::math::{Isometry, Point, Rotation, Vector};
+use approx::AbsDiffEq;
+use na::Unit;
+use ncollide::query::{PointProjection, PointQuery};
+use ncollide::shape::{FeatureId, Segment};
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A capsule shape defined as a segment with a radius.
+pub struct Capsule {
+    /// The first endpoint of the capsule.
+    pub a: Point<f32>,
+    /// The second enpdoint of the capsule.
+    pub b: Point<f32>,
+    /// The radius of the capsule.
+    pub radius: f32,
+}
+
+impl Capsule {
+    /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius.
+    pub fn new_x(half_height: f32, radius: f32) -> Self {
+        let b = Point::from(Vector::x() * half_height);
+        Self::new(-b, b, radius)
+    }
+
+    /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius.
+    pub fn new_y(half_height: f32, radius: f32) -> Self {
+        let b = Point::from(Vector::y() * half_height);
+        Self::new(-b, b, radius)
+    }
+
+    /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius.
+    #[cfg(feature = "dim3")]
+    pub fn new_z(half_height: f32, radius: f32) -> Self {
+        let b = Point::from(Vector::z() * half_height);
+        Self::new(-b, b, radius)
+    }
+
+    /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`.
+    pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
+        Self { a, b, radius }
+    }
+
+    /// The axis-aligned bounding box of this capsule.
+    pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
+        let a = pos * self.a;
+        let b = pos * self.b;
+        let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius);
+        let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius);
+        AABB::new(mins.into(), maxs.into())
+    }
+
+    /// The height of this capsule.
+    pub fn height(&self) -> f32 {
+        (self.b - self.a).norm()
+    }
+
+    /// The half-height of this capsule.
+    pub fn half_height(&self) -> f32 {
+        self.height() / 2.0
+    }
+
+    /// The center of this capsule.
+    pub fn center(&self) -> Point<f32> {
+        na::center(&self.a, &self.b)
+    }
+
+    /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`.
+    pub fn transform_by(&self, pos: &Isometry<f32>) -> Self {
+        Self::new(pos * self.a, pos * self.b, self.radius)
+    }
+
+    /// The rotation `r` such that `r * Y` is collinear with `b - a`.
+    pub fn rotation_wrt_y(&self) -> Rotation<f32> {
+        let mut dir = self.b - self.a;
+        if dir.y < 0.0 {
+            dir = -dir;
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            Rotation::rotation_between(&Vector::y(), &dir)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity())
+        }
+    }
+
+    /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`.
+    pub fn transform_wrt_y(&self) -> Isometry<f32> {
+        let rot = self.rotation_wrt_y();
+        Isometry::from_parts(self.center().coords.into(), rot)
+    }
+}
+
+// impl SupportMap<f32> for Capsule {
+//     fn local_support_point(&self, dir: &Vector) -> Point {
+//         let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis());
+//         self.local_support_point_toward(&dir)
+//     }
+//
+//     fn local_support_point_toward(&self, dir: &Unit<Vector>) -> Point {
+//         if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) {
+//             self.a + **dir * self.radius
+//         } else {
+//             self.b + **dir * self.radius
+//         }
+//     }
+// }
+
+// TODO: this code has been extracted from ncollide and added here
+// so we can modify it to fit with our new definition of capsule.
+// Wa should find a way to avoid this code duplication.
+impl PointQuery<f32> for Capsule {
+    #[inline]
+    fn project_point(
+        &self,
+        m: &Isometry<f32>,
+        pt: &Point<f32>,
+        solid: bool,
+    ) -> PointProjection<f32> {
+        let seg = Segment::new(self.a, self.b);
+        let proj = seg.project_point(m, pt, solid);
+        let dproj = *pt - proj.point;
+
+        if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) {
+            let inside = dist <= self.radius;
+            if solid && inside {
+                return PointProjection::new(true, *pt);
+            } else {
+                return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius);
+            }
+        } else if solid {
+            return PointProjection::new(true, *pt);
+        }
+
+        #[cfg(feature = "dim2")]
+        if let Some(dir) = seg.normal() {
+            let dir = m * *dir;
+            PointProjection::new(true, proj.point + dir * self.radius)
+        } else {
+            // The segment has no normal, likely because it degenerates to a point.
+            PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
+        }
+
+        #[cfg(feature = "dim3")]
+        if let Some(dir) = seg.direction() {
+            use crate::utils::WBasis;
+            let dir = m * dir.orthonormal_basis()[0];
+            PointProjection::new(true, proj.point + dir * self.radius)
+        } else {
+            // The segment has no normal, likely because it degenerates to a point.
+            PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
+        }
+    }
+
+    #[inline]
+    fn project_point_with_feature(
+        &self,
+        m: &Isometry<f32>,
+        pt: &Point<f32>,
+    ) -> (PointProjection<f32>, FeatureId) {
+        (self.project_point(m, pt, false), FeatureId::Face(0))
+    }
+}

src/geometry/collider.rs

@@ -0,0 +1,373 @@
+use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
+use crate::geometry::{
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
+    Proximity, Triangle, Trimesh,
+};
+use crate::math::{Isometry, Point, Vector};
+use na::Point3;
+use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+use num::Zero;
+
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// An enum grouping all the possible shape of a collider.
+pub enum Shape {
+    /// A ball shape.
+    Ball(Ball),
+    /// A convex polygon shape.
+    Polygon(Polygon),
+    /// A cuboid shape.
+    Cuboid(Cuboid),
+    /// A capsule shape.
+    Capsule(Capsule),
+    /// A triangle shape.
+    Triangle(Triangle),
+    /// A triangle mesh shape.
+    Trimesh(Trimesh),
+    /// A heightfield shape.
+    HeightField(HeightField),
+}
+
+impl Shape {
+    /// Gets a reference to the underlying ball shape, if `self` is one.
+    pub fn as_ball(&self) -> Option<&Ball> {
+        match self {
+            Shape::Ball(b) => Some(b),
+            _ => None,
+        }
+    }
+
+    /// Gets a reference to the underlying polygon shape, if `self` is one.
+    pub fn as_polygon(&self) -> Option<&Polygon> {
+        match self {
+            Shape::Polygon(p) => Some(p),
+            _ => None,
+        }
+    }
+
+    /// Gets a reference to the underlying cuboid shape, if `self` is one.
+    pub fn as_cuboid(&self) -> Option<&Cuboid> {
+        match self {
+            Shape::Cuboid(c) => Some(c),
+            _ => None,
+        }
+    }
+
+    /// Gets a reference to the underlying capsule shape, if `self` is one.
+    pub fn as_capsule(&self) -> Option<&Capsule> {
+        match self {
+            Shape::Capsule(c) => Some(c),
+            _ => None,
+        }
+    }
+
+    /// Gets a reference to the underlying triangle mesh shape, if `self` is one.
+    pub fn as_trimesh(&self) -> Option<&Trimesh> {
+        match self {
+            Shape::Trimesh(c) => Some(c),
+            _ => None,
+        }
+    }
+
+    /// Gets a reference to the underlying heightfield shape, if `self` is one.
+    pub fn as_heightfield(&self) -> Option<&HeightField> {
+        match self {
+            Shape::HeightField(h) => Some(h),
+            _ => None,
+        }
+    }
+
+    /// Gets a reference to the underlying triangle shape, if `self` is one.
+    pub fn as_triangle(&self) -> Option<&Triangle> {
+        match self {
+            Shape::Triangle(c) => Some(c),
+            _ => None,
+        }
+    }
+
+    /// Computes the axis-aligned bounding box of this shape.
+    pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+        match self {
+            Shape::Ball(ball) => ball.bounding_volume(position),
+            Shape::Polygon(poly) => poly.aabb(position),
+            Shape::Capsule(caps) => caps.aabb(position),
+            Shape::Cuboid(cuboid) => cuboid.bounding_volume(position),
+            Shape::Triangle(triangle) => triangle.bounding_volume(position),
+            Shape::Trimesh(trimesh) => trimesh.aabb(position),
+            Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
+        }
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries.
+///
+/// To build a new collider, use the `ColliderBuilder` structure.
+pub struct Collider {
+    shape: Shape,
+    density: f32,
+    is_sensor: bool,
+    pub(crate) parent: RigidBodyHandle,
+    pub(crate) delta: Isometry<f32>,
+    pub(crate) position: Isometry<f32>,
+    pub(crate) predicted_position: Isometry<f32>,
+    /// The friction coefficient of this collider.
+    pub friction: f32,
+    /// The restitution coefficient of this collider.
+    pub restitution: f32,
+    pub(crate) contact_graph_index: ColliderGraphIndex,
+    pub(crate) proximity_graph_index: ColliderGraphIndex,
+    pub(crate) proxy_index: usize,
+}
+
+impl Clone for Collider {
+    fn clone(&self) -> Self {
+        Self {
+            shape: self.shape.clone(),
+            parent: RigidBodySet::invalid_handle(),
+            contact_graph_index: ColliderGraphIndex::new(crate::INVALID_U32),
+            proximity_graph_index: ColliderGraphIndex::new(crate::INVALID_U32),
+            proxy_index: crate::INVALID_USIZE,
+            ..*self
+        }
+    }
+}
+
+impl Collider {
+    /// The rigid body this collider is attached to.
+    pub fn parent(&self) -> RigidBodyHandle {
+        self.parent
+    }
+
+    /// Is this collider a sensor?
+    pub fn is_sensor(&self) -> bool {
+        self.is_sensor
+    }
+
+    #[doc(hidden)]
+    pub fn set_position_debug(&mut self, position: Isometry<f32>) {
+        self.position = position;
+    }
+
+    /// The position of this collider expressed in the local-space of the rigid-body it is attached to.
+    pub fn delta(&self) -> &Isometry<f32> {
+        &self.delta
+    }
+
+    /// The world-space position of this collider.
+    pub fn position(&self) -> &Isometry<f32> {
+        &self.position
+    }
+
+    /// The density of this collider.
+    pub fn density(&self) -> f32 {
+        self.density
+    }
+
+    /// The geometric shape of this collider.
+    pub fn shape(&self) -> &Shape {
+        &self.shape
+    }
+
+    /// Compute the axis-aligned bounding box of this collider.
+    pub fn compute_aabb(&self) -> AABB<f32> {
+        self.shape.compute_aabb(&self.position)
+    }
+
+    // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB<f32> {
+    //     let aabb1 = self.shape.compute_aabb(&self.position);
+    //     let aabb2 = self.shape.compute_aabb(&self.predicted_position);
+    //     aabb1.merged(&aabb2)
+    // }
+
+    /// Compute the local-space mass properties of this collider.
+    pub fn mass_properties(&self) -> MassProperties {
+        match &self.shape {
+            Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius),
+            #[cfg(feature = "dim2")]
+            Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()),
+            #[cfg(feature = "dim3")]
+            Shape::Polygon(_p) => unimplemented!(),
+            Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents),
+            Shape::Capsule(caps) => {
+                MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius)
+            }
+            Shape::Triangle(_) => MassProperties::zero(),
+            Shape::Trimesh(_) => MassProperties::zero(),
+            Shape::HeightField(_) => MassProperties::zero(),
+        }
+    }
+}
+
+/// A structure responsible for building a new collider.
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderBuilder {
+    /// The shape of the collider to be built.
+    pub shape: Shape,
+    /// The density of the collider to be built.
+    pub density: f32,
+    /// The friction coefficient of the collider to be built.
+    pub friction: f32,
+    /// The restitution coefficient of the collider to be built.
+    pub restitution: f32,
+    /// The position of this collider relative to the local frame of the rigid-body it is attached to.
+    pub delta: Isometry<f32>,
+    /// Is this collider a sensor?
+    pub is_sensor: bool,
+}
+
+impl ColliderBuilder {
+    /// Initialize a new collider builder with the given shape.
+    pub fn new(shape: Shape) -> Self {
+        Self {
+            shape,
+            density: 1.0,
+            friction: Self::default_friction(),
+            restitution: 0.0,
+            delta: Isometry::identity(),
+            is_sensor: false,
+        }
+    }
+
+    /// Initialize a new collider builder with a ball shape defined by its radius.
+    pub fn ball(radius: f32) -> Self {
+        Self::new(Shape::Ball(Ball::new(radius)))
+    }
+
+    /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
+    #[cfg(feature = "dim2")]
+    pub fn cuboid(hx: f32, hy: f32) -> Self {
+        let cuboid = Cuboid {
+            half_extents: Vector::new(hx, hy),
+        };
+
+        Self::new(Shape::Cuboid(cuboid))
+
+        /*
+        use crate::math::Point;
+        let vertices = vec![
+            Point::new(hx, -hy),
+            Point::new(hx, hy),
+            Point::new(-hx, hy),
+            Point::new(-hx, -hy),
+        ];
+        let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()];
+        let polygon = Polygon::new(vertices, normals);
+
+        Self::new(Shape::Polygon(polygon))
+        */
+    }
+
+    /// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
+    pub fn capsule_x(half_height: f32, radius: f32) -> Self {
+        let capsule = Capsule::new_x(half_height, radius);
+        Self::new(Shape::Capsule(capsule))
+    }
+
+    /// Initialize a new collider builder with a capsule shape aligned with the `y` axis.
+    pub fn capsule_y(half_height: f32, radius: f32) -> Self {
+        let capsule = Capsule::new_y(half_height, radius);
+        Self::new(Shape::Capsule(capsule))
+    }
+
+    /// Initialize a new collider builder with a capsule shape aligned with the `z` axis.
+    #[cfg(feature = "dim3")]
+    pub fn capsule_z(half_height: f32, radius: f32) -> Self {
+        let capsule = Capsule::new_z(half_height, radius);
+        Self::new(Shape::Capsule(capsule))
+    }
+
+    /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
+    #[cfg(feature = "dim3")]
+    pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self {
+        let cuboid = Cuboid {
+            half_extents: Vector::new(hx, hy, hz),
+        };
+
+        Self::new(Shape::Cuboid(cuboid))
+    }
+
+    /// Initializes a collider builder with a segment shape.
+    ///
+    /// A segment shape is modeled by a capsule with a 0 radius.
+    pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
+        let capsule = Capsule::new(a, b, 0.0);
+        Self::new(Shape::Capsule(capsule))
+    }
+
+    /// Initializes a collider builder with a triangle shape.
+    pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
+        let triangle = Triangle::new(a, b, c);
+        Self::new(Shape::Triangle(triangle))
+    }
+
+    /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers.
+    pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
+        let trimesh = Trimesh::new(vertices, indices);
+        Self::new(Shape::Trimesh(trimesh))
+    }
+
+    /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
+    /// factor along each coordinate axis.
+    #[cfg(feature = "dim2")]
+    pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
+        let heightfield = HeightField::new(heights, scale);
+        Self::new(Shape::HeightField(heightfield))
+    }
+
+    /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
+    /// factor along each coordinate axis.
+    #[cfg(feature = "dim3")]
+    pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
+        let heightfield = HeightField::new(heights, scale);
+        Self::new(Shape::HeightField(heightfield))
+    }
+
+    /// The default friction coefficient used by the collider builder.
+    pub fn default_friction() -> f32 {
+        0.5
+    }
+
+    /// Sets whether or not the collider built by this builder is a sensor.
+    pub fn sensor(mut self, is_sensor: bool) -> Self {
+        self.is_sensor = is_sensor;
+        self
+    }
+
+    /// Sets the friction coefficient of the collider this builder will build.
+    pub fn friction(mut self, friction: f32) -> Self {
+        self.friction = friction;
+        self
+    }
+
+    /// Sets the density of the collider this builder will build.
+    pub fn density(mut self, density: f32) -> Self {
+        self.density = density;
+        self
+    }
+
+    /// Set the position of this collider in the local-space of the rigid-body it is attached to.
+    pub fn delta(mut self, delta: Isometry<f32>) -> Self {
+        self.delta = delta;
+        self
+    }
+
+    /// Buildes a new collider attached to the given rigid-body.
+    pub fn build(&self) -> Collider {
+        Collider {
+            shape: self.shape.clone(),
+            density: self.density,
+            friction: self.friction,
+            restitution: self.restitution,
+            delta: self.delta,
+            is_sensor: self.is_sensor,
+            parent: RigidBodySet::invalid_handle(),
+            position: Isometry::identity(),
+            predicted_position: Isometry::identity(),
+            contact_graph_index: InteractionGraph::<Contact>::invalid_graph_index(),
+            proximity_graph_index: InteractionGraph::<Proximity>::invalid_graph_index(),
+            proxy_index: crate::INVALID_USIZE,
+        }
+    }
+}

src/geometry/collider_set.rs

@@ -0,0 +1,139 @@
+use crate::data::arena::Arena;
+use crate::dynamics::{RigidBodyHandle, RigidBodySet};
+use crate::geometry::Collider;
+use std::ops::{Index, IndexMut};
+
+/// The unique identifier of a collider added to a collider set.
+pub type ColliderHandle = crate::data::arena::Index;
+
+#[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) colliders: Arena<Collider>,
+}
+
+impl ColliderSet {
+    /// Create a new empty set of colliders.
+    pub fn new() -> Self {
+        ColliderSet {
+            colliders: Arena::new(),
+        }
+    }
+
+    /// An always-invalid collider handle.
+    pub fn invalid_handle() -> ColliderHandle {
+        ColliderHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+    }
+
+    /// Iterate through all the colliders on this set.
+    pub fn iter(&self) -> impl Iterator<Item = (ColliderHandle, &Collider)> {
+        self.colliders.iter()
+    }
+
+    /// The number of colliders on this set.
+    pub fn len(&self) -> usize {
+        self.colliders.len()
+    }
+
+    /// Is this collider handle valid?
+    pub fn contains(&self, handle: ColliderHandle) -> bool {
+        self.colliders.contains(handle)
+    }
+
+    /// Inserts a new collider to this set and retrieve its handle.
+    pub fn insert(
+        &mut self,
+        mut coll: Collider,
+        parent_handle: RigidBodyHandle,
+        bodies: &mut RigidBodySet,
+    ) -> ColliderHandle {
+        let mass_properties = coll.mass_properties();
+        coll.parent = parent_handle;
+        let parent = bodies
+            .get_mut_internal(parent_handle)
+            .expect("Parent rigid body not found.");
+        coll.position = parent.position * coll.delta;
+        coll.predicted_position = parent.predicted_position * coll.delta;
+        let handle = self.colliders.insert(coll);
+        parent.colliders.push(handle);
+        parent.mass_properties += mass_properties;
+        parent.update_world_mass_properties();
+        bodies.activate(parent_handle);
+        handle
+    }
+
+    pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option<Collider> {
+        self.colliders.remove(handle)
+    }
+
+    /// Gets the collider with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the collider at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the collider position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// Using this is discouraged in favor of `self.get(handle)` which does not
+    /// suffer form the ABA problem.
+    pub fn get_unknown_gen(&self, i: usize) -> Option<(&Collider, ColliderHandle)> {
+        self.colliders.get_unknown_gen(i)
+    }
+
+    /// Gets a mutable reference to the collider with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the collider at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the collider position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// 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 Collider, ColliderHandle)> {
+        self.colliders.get_unknown_gen_mut(i)
+    }
+
+    /// Get the collider with the given handle.
+    pub fn get(&self, handle: ColliderHandle) -> Option<&Collider> {
+        self.colliders.get(handle)
+    }
+
+    /// Gets a mutable reference to the collider with the given handle.
+    pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> {
+        self.colliders.get_mut(handle)
+    }
+
+    pub(crate) fn get2_mut_internal(
+        &mut self,
+        h1: ColliderHandle,
+        h2: ColliderHandle,
+    ) -> (Option<&mut Collider>, Option<&mut Collider>) {
+        self.colliders.get2_mut(h1, h2)
+    }
+
+    // pub fn iter_mut(&mut self) -> impl Iterator<Item = (ColliderHandle, ColliderMut)> {
+    //     //        let sender = &self.activation_channel_sender;
+    //     self.colliders.iter_mut().map(move |(h, rb)| {
+    //         (h, ColliderMut::new(h, rb /*sender.clone()*/))
+    //     })
+    // }
+
+    //    pub(crate) fn iter_mut_internal(
+    //        &mut self,
+    //    ) -> impl Iterator<Item = (ColliderHandle, &mut Collider)> {
+    //        self.colliders.iter_mut()
+    //    }
+}
+
+impl Index<ColliderHandle> for ColliderSet {
+    type Output = Collider;
+
+    fn index(&self, index: ColliderHandle) -> &Collider {
+        &self.colliders[index]
+    }
+}
+
+impl IndexMut<ColliderHandle> for ColliderSet {
+    fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider {
+        &mut self.colliders[index]
+    }
+}

src/geometry/contact.rs

@@ -0,0 +1,485 @@
+use crate::dynamics::BodyPair;
+use crate::geometry::contact_generator::ContactPhase;
+use crate::geometry::{Collider, ColliderPair, ColliderSet};
+use crate::math::{Isometry, Point, Vector};
+use std::any::Any;
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SimdFloat, SIMD_WIDTH},
+    simba::simd::SimdValue,
+};
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The type local linear approximation of the neighborhood of a pair contact points on two shapes
+pub enum KinematicsCategory {
+    /// Both neighborhoods are assimilated to a single point.
+    PointPoint,
+    /// The first shape's neighborhood at the contact point is assimilated to a plane while
+    /// the second is assimilated to a point.
+    PlanePoint,
+}
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// Local contact geometry at the neighborhood of a pair of contact points.
+pub struct ContactKinematics {
+    /// The local contact geometry.
+    pub category: KinematicsCategory,
+    /// The dilation applied to the first contact geometry.
+    pub radius1: f32,
+    /// The dilation applied to the second contact geometry.
+    pub radius2: f32,
+}
+
+impl Default for ContactKinematics {
+    fn default() -> Self {
+        ContactKinematics {
+            category: KinematicsCategory::PointPoint,
+            radius1: 0.0,
+            radius2: 0.0,
+        }
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub(crate) struct WContact {
+    pub local_p1: Point<SimdFloat>,
+    pub local_p2: Point<SimdFloat>,
+    pub local_n1: Vector<SimdFloat>,
+    pub local_n2: Vector<SimdFloat>,
+    pub dist: SimdFloat,
+    pub fid1: [u8; SIMD_WIDTH],
+    pub fid2: [u8; SIMD_WIDTH],
+}
+
+#[cfg(feature = "simd-is-enabled")]
+impl WContact {
+    pub fn extract(&self, i: usize) -> (Contact, Vector<f32>, Vector<f32>) {
+        let c = Contact {
+            local_p1: self.local_p1.extract(i),
+            local_p2: self.local_p2.extract(i),
+            dist: self.dist.extract(i),
+            impulse: 0.0,
+            tangent_impulse: Contact::zero_tangent_impulse(),
+            fid1: self.fid1[i],
+            fid2: self.fid2[i],
+        };
+
+        (c, self.local_n1.extract(i), self.local_n2.extract(i))
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A single contact between two collider.
+pub struct Contact {
+    /// The contact point in the local-space of the first collider.
+    pub local_p1: Point<f32>,
+    /// The contact point in the local-space of the second collider.
+    pub local_p2: Point<f32>,
+    /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body.
+    ///
+    /// The impulse applied to the second collider's rigid-body is given by `-impulse`.
+    pub impulse: f32,
+    /// The friction impulse along the vector orthonormal to the contact normal, applied to the first
+    /// collider's rigid-body.
+    #[cfg(feature = "dim2")]
+    pub tangent_impulse: f32,
+    /// The friction impulses along the basis orthonormal to the contact normal, applied to the first
+    /// collider's rigid-body.
+    #[cfg(feature = "dim3")]
+    pub tangent_impulse: [f32; 2],
+    /// The identifier of the subshape of the first collider involved in this contact.
+    ///
+    /// For primitive shapes like cuboid, ball, etc., this is 0.
+    /// For shapes like trimesh and heightfield this identifies the specific triangle
+    /// involved in the contact.
+    pub fid1: u8,
+    /// The identifier of the subshape of the second collider involved in this contact.
+    ///
+    /// For primitive shapes like cuboid, ball, etc., this is 0.
+    /// For shapes like trimesh and heightfield this identifies the specific triangle
+    /// involved in the contact.
+    pub fid2: u8,
+    /// The distance between the two colliders along the contact normal.
+    ///
+    /// If this is negative, the colliders are penetrating.
+    pub dist: f32,
+}
+
+impl Contact {
+    pub(crate) fn new(
+        local_p1: Point<f32>,
+        local_p2: Point<f32>,
+        fid1: u8,
+        fid2: u8,
+        dist: f32,
+    ) -> Self {
+        Self {
+            local_p1,
+            local_p2,
+            impulse: 0.0,
+            #[cfg(feature = "dim2")]
+            tangent_impulse: 0.0,
+            #[cfg(feature = "dim3")]
+            tangent_impulse: [0.0; 2],
+            fid1,
+            fid2,
+            dist,
+        }
+    }
+
+    #[cfg(feature = "dim2")]
+    pub(crate) fn zero_tangent_impulse() -> f32 {
+        0.0
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn zero_tangent_impulse() -> [f32; 2] {
+        [0.0, 0.0]
+    }
+
+    pub(crate) fn copy_geometry_from(&mut self, contact: Contact) {
+        self.local_p1 = contact.local_p1;
+        self.local_p2 = contact.local_p2;
+        self.fid1 = contact.fid1;
+        self.fid2 = contact.fid2;
+        self.dist = contact.dist;
+    }
+
+    // pub(crate) fn swap(self) -> Self {
+    //     Self {
+    //         local_p1: self.local_p2,
+    //         local_p2: self.local_p1,
+    //         impulse: self.impulse,
+    //         tangent_impulse: self.tangent_impulse,
+    //         fid1: self.fid2,
+    //         fid2: self.fid1,
+    //         dist: self.dist,
+    //     }
+    // }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The description of all the contacts between a pair of colliders.
+pub struct ContactPair {
+    /// The pair of colliders involved.
+    pub pair: ColliderPair,
+    /// The set of contact manifolds between the two colliders.
+    ///
+    /// All contact manifold contain themselves contact points between the colliders.
+    pub manifolds: Vec<ContactManifold>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pub(crate) generator: Option<ContactPhase>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pub(crate) generator_workspace: Option<Box<dyn Any + Send + Sync>>,
+}
+
+impl ContactPair {
+    pub(crate) fn new(
+        pair: ColliderPair,
+        generator: ContactPhase,
+        generator_workspace: Option<Box<dyn Any + Send + Sync>>,
+    ) -> Self {
+        Self {
+            pair,
+            manifolds: Vec::new(),
+            generator: Some(generator),
+            generator_workspace,
+        }
+    }
+
+    /// Does this contact pair have any active contact?
+    ///
+    /// An active contact is a contact that may result in a non-zero contact force.
+    pub fn has_any_active_contact(&self) -> bool {
+        for manifold in &self.manifolds {
+            if manifold.num_active_contacts != 0 {
+                return true;
+            }
+        }
+
+        false
+    }
+
+    pub(crate) fn single_manifold<'a, 'b>(
+        &'a mut self,
+        colliders: &'b ColliderSet,
+    ) -> (
+        &'b Collider,
+        &'b Collider,
+        &'a mut ContactManifold,
+        Option<&'a mut (dyn Any + Send + Sync)>,
+    ) {
+        let coll1 = &colliders[self.pair.collider1];
+        let coll2 = &colliders[self.pair.collider2];
+
+        if self.manifolds.len() == 0 {
+            let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2);
+            self.manifolds.push(manifold);
+        }
+
+        // We have to make sure the order of the returned collider
+        // match the order of the pair stored inside of the manifold.
+        // (This order can be modified by the contact determination algorithm).
+        let manifold = &mut self.manifolds[0];
+        if manifold.pair.collider1 == self.pair.collider1 {
+            (
+                coll1,
+                coll2,
+                manifold,
+                self.generator_workspace.as_mut().map(|w| &mut **w),
+            )
+        } else {
+            (
+                coll2,
+                coll1,
+                manifold,
+                self.generator_workspace.as_mut().map(|w| &mut **w),
+            )
+        }
+    }
+}
+
+#[derive(Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A contact manifold between two colliders.
+///
+/// A contact manifold describes a set of contacts between two colliders. All the contact
+/// part of the same contact manifold share the same contact normal and contact kinematics.
+pub struct ContactManifold {
+    // NOTE: use a SmallVec instead?
+    // And for 2D use an ArrayVec since there will never be more than 2 contacts anyways.
+    #[cfg(feature = "dim2")]
+    pub(super) points: arrayvec::ArrayVec<[Contact; 2]>,
+    #[cfg(feature = "dim3")]
+    pub(super) points: Vec<Contact>,
+    /// The number of active contacts on this contact manifold.
+    ///
+    /// Active contacts are these that may result in contact forces.
+    pub num_active_contacts: usize,
+    /// The contact normal of all the contacts of this manifold, expressed in the local space of the first collider.
+    pub local_n1: Vector<f32>,
+    /// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider.
+    pub local_n2: Vector<f32>,
+    /// The contact kinematics of all the contacts of this manifold.
+    pub kinematics: ContactKinematics,
+    // The following are set by the narrow-phase.
+    /// The pair of body involved in this contact manifold.
+    pub body_pair: BodyPair,
+    /// The pair of colliders involved in this contact manifold.
+    pub pair: ColliderPair,
+    /// The pair of subshapes involved in this contact manifold.
+    pub subshape_index_pair: (usize, usize),
+    pub(crate) warmstart_multiplier: f32,
+    // We put the friction and restitution here because
+    // this avoids reading the colliders inside of the
+    // contact preparation method.
+    /// The friction coefficient for of all the contacts on this contact manifold.
+    pub friction: f32,
+    /// The restitution coefficient for all the contacts on this contact manifold.
+    pub restitution: f32,
+    // The following are set by the constraints solver.
+    pub(crate) constraint_index: usize,
+    pub(crate) position_constraint_index: usize,
+}
+
+impl ContactManifold {
+    pub(crate) fn new(
+        pair: ColliderPair,
+        subshapes: (usize, usize),
+        body_pair: BodyPair,
+        friction: f32,
+        restitution: f32,
+    ) -> ContactManifold {
+        Self {
+            #[cfg(feature = "dim2")]
+            points: arrayvec::ArrayVec::new(),
+            #[cfg(feature = "dim3")]
+            points: Vec::new(),
+            num_active_contacts: 0,
+            local_n1: Vector::zeros(),
+            local_n2: Vector::zeros(),
+            pair,
+            subshape_index_pair: subshapes,
+            body_pair,
+            kinematics: ContactKinematics::default(),
+            warmstart_multiplier: Self::min_warmstart_multiplier(),
+            friction,
+            restitution,
+            constraint_index: 0,
+            position_constraint_index: 0,
+        }
+    }
+
+    pub(crate) fn take(&mut self) -> Self {
+        ContactManifold {
+            #[cfg(feature = "dim2")]
+            points: self.points.clone(),
+            #[cfg(feature = "dim3")]
+            points: std::mem::replace(&mut self.points, Vec::new()),
+            num_active_contacts: self.num_active_contacts,
+            local_n1: self.local_n1,
+            local_n2: self.local_n2,
+            kinematics: self.kinematics,
+            body_pair: self.body_pair,
+            pair: self.pair,
+            subshape_index_pair: self.subshape_index_pair,
+            warmstart_multiplier: self.warmstart_multiplier,
+            friction: self.friction,
+            restitution: self.restitution,
+            constraint_index: self.constraint_index,
+            position_constraint_index: self.position_constraint_index,
+        }
+    }
+
+    pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self {
+        Self::with_subshape_indices(pair, coll1, coll2, 0, 0)
+    }
+
+    pub(crate) fn with_subshape_indices(
+        pair: ColliderPair,
+        coll1: &Collider,
+        coll2: &Collider,
+        subshape1: usize,
+        subshape2: usize,
+    ) -> Self {
+        Self::new(
+            pair,
+            (subshape1, subshape2),
+            BodyPair::new(coll1.parent, coll2.parent),
+            (coll1.friction + coll2.friction) * 0.5,
+            (coll1.restitution + coll2.restitution) * 0.5,
+        )
+    }
+
+    pub(crate) fn min_warmstart_multiplier() -> f32 {
+        // Multiplier used to reduce the amount of warm-starting.
+        // This coefficient increases exponentially over time, until it reaches 1.0.
+        // This will reduce significant overshoot at the timesteps that
+        // follow a timestep involving high-velocity impacts.
+        0.01
+    }
+
+    /// Number of active contacts on this contact manifold.
+    #[inline]
+    pub fn num_active_contacts(&self) -> usize {
+        self.num_active_contacts
+    }
+
+    /// The slice of all the active contacts on this contact manifold.
+    ///
+    /// Active contacts are contacts that may end up generating contact forces.
+    #[inline]
+    pub fn active_contacts(&self) -> &[Contact] {
+        &self.points[..self.num_active_contacts]
+    }
+
+    #[inline]
+    pub(crate) fn active_contacts_mut(&mut self) -> &mut [Contact] {
+        &mut self.points[..self.num_active_contacts]
+    }
+
+    /// The slice of all the contacts, active or not, on this contact manifold.
+    #[inline]
+    pub fn all_contacts(&self) -> &[Contact] {
+        &self.points
+    }
+
+    pub(crate) fn swap_identifiers(&mut self) {
+        self.pair = self.pair.swap();
+        self.body_pair = self.body_pair.swap();
+        self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0);
+    }
+
+    pub(crate) fn update_warmstart_multiplier(&mut self) {
+        // In 2D, tall stacks will actually suffer from this
+        // because oscillation due to inaccuracies in 2D often
+        // cause contacts to break, which would result in
+        // a reset of the warmstart multiplier.
+        if cfg!(feature = "dim2") {
+            self.warmstart_multiplier = 1.0;
+            return;
+        }
+
+        for pt in &self.points {
+            if pt.impulse != 0.0 {
+                self.warmstart_multiplier = (self.warmstart_multiplier * 2.0).min(1.0);
+                return;
+            }
+        }
+
+        // Reset the multiplier.
+        self.warmstart_multiplier = Self::min_warmstart_multiplier()
+    }
+
+    #[inline]
+    pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
+        if self.points.len() == 0 {
+            return false;
+        }
+
+        //        const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
+        const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
+
+        let local_n2 = pos12 * self.local_n2;
+
+        if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
+            return false;
+        }
+
+        for pt in &mut self.points {
+            let local_p2 = pos12 * pt.local_p2;
+            let dpt = local_p2 - pt.local_p1;
+            let dist = dpt.dot(&self.local_n1);
+
+            if dist * pt.dist < 0.0 {
+                // We switched between penetrating/non-penetrating.
+                // The may result in other contacts to appear.
+                return false;
+            }
+            let new_local_p1 = local_p2 - self.local_n1 * dist;
+
+            let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
+            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
+                return false;
+            }
+
+            pt.dist = dist;
+            pt.local_p1 = new_local_p1;
+        }
+
+        true
+    }
+
+    /// Sort the contacts of this contact manifold such that the active contacts are in the first
+    /// positions of the array.
+    #[inline]
+    pub(crate) fn sort_contacts(&mut self, prediction_distance: f32) {
+        let num_contacts = self.points.len();
+        match num_contacts {
+            0 => {
+                self.num_active_contacts = 0;
+            }
+            1 => {
+                self.num_active_contacts = (self.points[0].dist < prediction_distance) as usize;
+            }
+            _ => {
+                let mut first_inactive_index = num_contacts;
+
+                self.num_active_contacts = 0;
+                while self.num_active_contacts != first_inactive_index {
+                    if self.points[self.num_active_contacts].dist >= prediction_distance {
+                        // Swap with the last contact.
+                        self.points
+                            .swap(self.num_active_contacts, first_inactive_index - 1);
+                        first_inactive_index -= 1;
+                    } else {
+                        self.num_active_contacts += 1;
+                    }
+                }
+            }
+        }
+    }
+}

src/geometry/contact_generator/ball_ball_contact_generator.rs

@@ -0,0 +1,98 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{Contact, KinematicsCategory};
+use crate::math::Point;
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd,
+    crate::geometry::{WBall, WContact},
+    crate::math::{Isometry, SimdFloat, SIMD_WIDTH},
+    simba::simd::SimdValue,
+};
+
+#[cfg(feature = "simd-is-enabled")]
+fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdFloat>) -> WContact {
+    let dcenter = ball2.center - ball1.center;
+    let center_dist = dcenter.magnitude();
+    let normal = dcenter / center_dist;
+
+    WContact {
+        local_p1: ball1.center + normal * ball1.radius,
+        local_p2: pos21 * (ball2.center - normal * ball2.radius),
+        local_n1: normal,
+        local_n2: pos21 * -normal,
+        fid1: [0; SIMD_WIDTH],
+        fid2: [0; SIMD_WIDTH],
+        dist: center_dist - ball1.radius - ball2.radius,
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) {
+    let pos_ba = ctxt.positions2.inverse() * ctxt.positions1;
+    let radii_a =
+        SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
+    let radii_b =
+        SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
+
+    let wball_a = WBall::new(Point::origin(), radii_a);
+    let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b);
+    let contacts = generate_contacts_simd(&wball_a, &wball_b, &pos_ba);
+
+    for (i, manifold) in ctxt.manifolds.iter_mut().enumerate() {
+        // FIXME: compare the dist before extracting the contact.
+        let (contact, local_n1, local_n2) = contacts.extract(i);
+        if contact.dist <= ctxt.prediction_distance {
+            if manifold.points.len() != 0 {
+                manifold.points[0].copy_geometry_from(contact);
+            } else {
+                manifold.points.push(contact);
+            }
+
+            manifold.local_n1 = local_n1;
+            manifold.local_n2 = local_n2;
+            manifold.kinematics.category = KinematicsCategory::PointPoint;
+            manifold.kinematics.radius1 = radii_a.extract(i);
+            manifold.kinematics.radius2 = radii_b.extract(i);
+            manifold.update_warmstart_multiplier();
+        } else {
+            manifold.points.clear();
+        }
+
+        manifold.sort_contacts(ctxt.prediction_distance);
+    }
+}
+
+pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) {
+    let pos_ba = ctxt.position2.inverse() * ctxt.position1;
+    let radius_a = ctxt.shape1.as_ball().unwrap().radius;
+    let radius_b = ctxt.shape2.as_ball().unwrap().radius;
+
+    let dcenter = pos_ba.inverse_transform_point(&Point::origin()).coords;
+    let center_dist = dcenter.magnitude();
+    let dist = center_dist - radius_a - radius_b;
+
+    if dist < ctxt.prediction_distance {
+        let local_n1 = dcenter / center_dist;
+        let local_n2 = pos_ba.inverse_transform_vector(&-local_n1);
+        let local_p1 = local_n1 * radius_a;
+        let local_p2 = local_n2 * radius_b;
+        let contact = Contact::new(local_p1.into(), local_p2.into(), 0, 0, dist);
+
+        if ctxt.manifold.points.len() != 0 {
+            ctxt.manifold.points[0].copy_geometry_from(contact);
+        } else {
+            ctxt.manifold.points.push(contact);
+        }
+
+        ctxt.manifold.local_n1 = local_n1;
+        ctxt.manifold.local_n2 = local_n2;
+        ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint;
+        ctxt.manifold.kinematics.radius1 = radius_a;
+        ctxt.manifold.kinematics.radius2 = radius_b;
+        ctxt.manifold.update_warmstart_multiplier();
+    } else {
+        ctxt.manifold.points.clear();
+    }
+
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}

src/geometry/contact_generator/ball_convex_contact_generator.rs

@@ -0,0 +1,85 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{Ball, Contact, KinematicsCategory, Shape};
+use crate::math::Isometry;
+use na::Unit;
+use ncollide::query::PointQuery;
+
+pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let Shape::Ball(ball1) = ctxt.shape1 {
+        ctxt.manifold.swap_identifiers();
+
+        match ctxt.shape2 {
+            Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true),
+            Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true),
+            Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true),
+            _ => unimplemented!(),
+        }
+    } else if let Shape::Ball(ball2) = ctxt.shape2 {
+        match ctxt.shape1 {
+            Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false),
+            Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false),
+            Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false),
+            _ => unimplemented!(),
+        }
+    }
+
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}
+
+fn do_generate_contacts<P: PointQuery<f32>>(
+    point_query1: &P,
+    ball2: &Ball,
+    ctxt: &mut PrimitiveContactGenerationContext,
+    swapped: bool,
+) {
+    let position1;
+    let position2;
+
+    if swapped {
+        position1 = ctxt.position2;
+        position2 = ctxt.position1;
+    } else {
+        position1 = ctxt.position1;
+        position2 = ctxt.position2;
+    }
+
+    let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into());
+
+    // TODO: add a `project_local_point` to the PointQuery trait to avoid
+    // the identity isometry.
+    let proj =
+        point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3"));
+    let dpos = local_p2_1 - proj.point;
+
+    #[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D.
+    if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) {
+        #[cfg(feature = "dim2")]
+        if proj.is_inside {
+            local_n1 = -local_n1;
+            dist = -dist;
+        }
+
+        if dist <= ball2.radius + ctxt.prediction_distance {
+            let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1));
+            let local_p2 = (local_n2 * ball2.radius).into();
+
+            let contact_point = Contact::new(proj.point, local_p2, 0, 0, dist - ball2.radius);
+            if ctxt.manifold.points.len() != 1 {
+                ctxt.manifold.points.clear();
+                ctxt.manifold.points.push(contact_point);
+            } else {
+                // Copy only the geometry so we keep the warmstart impulses.
+                ctxt.manifold.points[0].copy_geometry_from(contact_point);
+            }
+
+            ctxt.manifold.local_n1 = *local_n1;
+            ctxt.manifold.local_n2 = local_n2;
+            ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint;
+            ctxt.manifold.kinematics.radius1 = 0.0;
+            ctxt.manifold.kinematics.radius2 = ball2.radius;
+            ctxt.manifold.update_warmstart_multiplier();
+        } else {
+            ctxt.manifold.points.clear();
+        }
+    }
+}

src/geometry/contact_generator/capsule_capsule_contact_generator.rs

@@ -0,0 +1,200 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory, Shape};
+use crate::math::Isometry;
+use crate::math::Vector;
+use approx::AbsDiffEq;
+use na::Unit;
+#[cfg(feature = "dim2")]
+use ncollide::shape::{Segment, SegmentPointLocation};
+
+pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            ctxt.prediction_distance,
+            capsule1,
+            ctxt.position1,
+            capsule2,
+            ctxt.position2,
+            ctxt.manifold,
+        );
+    }
+
+    ctxt.manifold.update_warmstart_multiplier();
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}
+
+#[cfg(feature = "dim2")]
+pub fn generate_contacts<'a>(
+    prediction_distance: f32,
+    capsule1: &'a Capsule,
+    pos1: &'a Isometry<f32>,
+    capsule2: &'a Capsule,
+    pos2: &'a Isometry<f32>,
+    manifold: &mut ContactManifold,
+) {
+    // FIXME: the contact kinematics is not correctly set here.
+    // We use the common "Point-Plane" kinematics with zero radius everytime.
+    // Instead we should select point/point ore point-plane (with non-zero
+    // radius for the point) depending on the features involved in the contact.
+    let pos12 = pos1.inverse() * pos2;
+    let pos21 = pos12.inverse();
+
+    let capsule2_1 = capsule2.transform_by(&pos12);
+    let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
+        (&capsule1.a, &capsule1.b),
+        (&capsule2_1.a, &capsule2_1.b),
+    );
+
+    // We do this clone to perform contact tracking and transfer impulses.
+    // FIXME: find a more efficient way of doing this.
+    let old_manifold_points = manifold.points.clone();
+    manifold.points.clear();
+
+    let swapped = false;
+
+    let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 {
+        v1 as u8 * 2
+    } else {
+        1
+    };
+    let fid2 = if let SegmentPointLocation::OnVertex(v2) = loc2 {
+        v2 as u8 * 2
+    } else {
+        1
+    };
+
+    let bcoords1 = loc1.barycentric_coordinates();
+    let bcoords2 = loc2.barycentric_coordinates();
+    let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1];
+    let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
+
+    let local_n1 =
+        Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
+    let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius;
+
+    if dist <= prediction_distance {
+        let local_n2 = pos21 * -local_n1;
+        let contact = Contact::new(local_p1, pos21 * local_p2, fid1, fid2, dist);
+        manifold.points.push(contact);
+
+        manifold.local_n1 = *local_n1;
+        manifold.local_n2 = *local_n2;
+        manifold.kinematics.category = KinematicsCategory::PlanePoint;
+        manifold.kinematics.radius1 = 0.0;
+        manifold.kinematics.radius2 = 0.0;
+    } else {
+        // No contact within tolerance.
+        return;
+    }
+
+    let seg1 = Segment::new(capsule1.a, capsule1.b);
+    let seg2 = Segment::new(capsule2_1.a, capsule2_1.b);
+
+    if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) {
+        if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8
+            && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8
+        {
+            // Capsules axii are almost parallel and are almost perpendicular to the normal.
+            // Find a second contact point.
+            if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal(
+                (capsule1.a, capsule1.b),
+                (capsule2_1.a, capsule2_1.b),
+                *local_n1,
+            ) {
+                let contact =
+                    if (clip_a.0 - local_p1).norm_squared() > f32::default_epsilon() * 100.0 {
+                        // Use clip_a as the second contact.
+                        Contact::new(
+                            clip_a.0,
+                            pos21 * clip_a.1,
+                            clip_a.2 as u8,
+                            clip_a.3 as u8,
+                            (clip_a.1 - clip_a.0).dot(&local_n1),
+                        )
+                    } else {
+                        // Use clip_b as the second contact.
+                        Contact::new(
+                            clip_b.0,
+                            pos21 * clip_b.1,
+                            clip_b.2 as u8,
+                            clip_b.3 as u8,
+                            (clip_b.1 - clip_b.0).dot(&local_n1),
+                        )
+                    };
+
+                manifold.points.push(contact);
+            }
+        }
+    }
+
+    if swapped {
+        for point in &mut manifold.points {
+            point.local_p1 += manifold.local_n1 * capsule2.radius;
+            point.local_p2 += manifold.local_n2 * capsule1.radius;
+            point.dist -= capsule1.radius + capsule2.radius;
+        }
+    } else {
+        for point in &mut manifold.points {
+            point.local_p1 += manifold.local_n1 * capsule1.radius;
+            point.local_p2 += manifold.local_n2 * capsule2.radius;
+            point.dist -= capsule1.radius + capsule2.radius;
+        }
+    }
+
+    super::match_contacts(manifold, &old_manifold_points, swapped);
+}
+
+#[cfg(feature = "dim3")]
+pub fn generate_contacts<'a>(
+    prediction_distance: f32,
+    capsule1: &'a Capsule,
+    pos1: &'a Isometry<f32>,
+    capsule2: &'a Capsule,
+    pos2: &'a Isometry<f32>,
+    manifold: &mut ContactManifold,
+) {
+    let pos12 = pos1.inverse() * pos2;
+    let pos21 = pos12.inverse();
+
+    let capsule2_1 = capsule1.transform_by(&pos12);
+    let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
+        (&capsule1.a, &capsule1.b),
+        (&capsule2_1.a, &capsule2_1.b),
+    );
+
+    {
+        let bcoords1 = loc1.barycentric_coordinates();
+        let bcoords2 = loc2.barycentric_coordinates();
+        let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1];
+        let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
+
+        let local_n1 =
+            Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
+        let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius;
+
+        if dist <= prediction_distance {
+            let local_n2 = pos21 * -local_n1;
+            let contact = Contact::new(
+                local_p1 + *local_n1 * capsule1.radius,
+                pos21 * local_p2 + *local_n2 * capsule2.radius,
+                0,
+                0,
+                dist,
+            );
+
+            if manifold.points.len() != 0 {
+                manifold.points[0].copy_geometry_from(contact);
+            } else {
+                manifold.points.push(contact);
+            }
+
+            manifold.local_n1 = *local_n1;
+            manifold.local_n2 = *local_n2;
+            manifold.kinematics.category = KinematicsCategory::PlanePoint;
+            manifold.kinematics.radius1 = 0.0;
+            manifold.kinematics.radius2 = 0.0;
+        } else {
+            manifold.points.clear();
+        }
+    }
+}

src/geometry/contact_generator/contact_dispatcher.rs

@@ -0,0 +1,127 @@
+use crate::geometry::contact_generator::{
+    ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
+    PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
+};
+use crate::geometry::Shape;
+use std::any::Any;
+
+/// Trait implemented by structures responsible for selecting a collision-detection algorithm
+/// for a given pair of shapes.
+pub trait ContactDispatcher {
+    /// Select the collision-detection algorithm for the given pair of primitive shapes.
+    fn dispatch_primitives(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (
+        PrimitiveContactGenerator,
+        Option<Box<dyn Any + Send + Sync>>,
+    );
+    /// Select the collision-detection algorithm for the given pair of non-primitive shapes.
+    fn dispatch(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>);
+}
+
+/// The default contact dispatcher used by Rapier.
+pub struct DefaultContactDispatcher;
+
+impl ContactDispatcher for DefaultContactDispatcher {
+    fn dispatch_primitives(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (
+        PrimitiveContactGenerator,
+        Option<Box<dyn Any + Send + Sync>>,
+    ) {
+        match (shape1, shape2) {
+            (Shape::Ball(_), Shape::Ball(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_ball_ball,
+                    #[cfg(feature = "simd-is-enabled")]
+                    generate_contacts_simd: super::generate_contacts_ball_ball_simd,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            (Shape::Cuboid(_), Shape::Cuboid(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_cuboid_cuboid,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            (Shape::Polygon(_), Shape::Polygon(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_polygon_polygon,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            (Shape::Capsule(_), Shape::Capsule(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_capsule_capsule,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            (Shape::Cuboid(_), Shape::Ball(_))
+            | (Shape::Ball(_), Shape::Cuboid(_))
+            | (Shape::Triangle(_), Shape::Ball(_))
+            | (Shape::Ball(_), Shape::Triangle(_))
+            | (Shape::Capsule(_), Shape::Ball(_))
+            | (Shape::Ball(_), Shape::Capsule(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_ball_convex,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_cuboid_capsule,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_cuboid_triangle,
+                    ..PrimitiveContactGenerator::default()
+                },
+                None,
+            ),
+            _ => (PrimitiveContactGenerator::default(), None),
+        }
+    }
+
+    fn dispatch(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>) {
+        match (shape1, shape2) {
+            (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => (
+                ContactPhase::NearPhase(ContactGenerator {
+                    generate_contacts: super::generate_contacts_trimesh_shape,
+                    ..ContactGenerator::default()
+                }),
+                Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())),
+            ),
+            (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => (
+                ContactPhase::NearPhase(ContactGenerator {
+                    generate_contacts: super::generate_contacts_heightfield_shape,
+                    ..ContactGenerator::default()
+                }),
+                Some(Box::new(HeightFieldShapeContactGeneratorWorkspace::new())),
+            ),
+            _ => {
+                let (gen, workspace) = self.dispatch_primitives(shape1, shape2);
+                (ContactPhase::ExactPhase(gen), workspace)
+            }
+        }
+    }
+}

src/geometry/contact_generator/contact_generator.rs

@@ -0,0 +1,222 @@
+use crate::geometry::{
+    Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
+};
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::{SimdFloat, SIMD_WIDTH};
+use crate::pipeline::EventHandler;
+use std::any::Any;
+
+#[derive(Copy, Clone)]
+pub enum ContactPhase {
+    NearPhase(ContactGenerator),
+    ExactPhase(PrimitiveContactGenerator),
+}
+
+impl ContactPhase {
+    #[inline]
+    pub fn generate_contacts(
+        self,
+        mut context: ContactGenerationContext,
+        events: &dyn EventHandler,
+    ) {
+        let had_contacts_before = context.pair.has_any_active_contact();
+
+        match self {
+            Self::NearPhase(gen) => (gen.generate_contacts)(&mut context),
+            Self::ExactPhase(gen) => {
+                // Build the primitive context from the non-primitive context and dispatch.
+                let (collider1, collider2, manifold, workspace) =
+                    context.pair.single_manifold(context.colliders);
+                let mut context2 = PrimitiveContactGenerationContext {
+                    prediction_distance: context.prediction_distance,
+                    collider1,
+                    collider2,
+                    shape1: collider1.shape(),
+                    shape2: collider2.shape(),
+                    position1: collider1.position(),
+                    position2: collider2.position(),
+                    manifold,
+                    workspace,
+                };
+
+                (gen.generate_contacts)(&mut context2)
+            }
+        }
+
+        if had_contacts_before != context.pair.has_any_active_contact() {
+            if had_contacts_before {
+                events.handle_contact_event(ContactEvent::Stopped(
+                    context.pair.pair.collider1,
+                    context.pair.pair.collider2,
+                ));
+            } else {
+                events.handle_contact_event(ContactEvent::Started(
+                    context.pair.pair.collider1,
+                    context.pair.pair.collider2,
+                ))
+            }
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    #[inline]
+    pub fn generate_contacts_simd(
+        self,
+        mut context: ContactGenerationContextSimd,
+        events: &dyn EventHandler,
+    ) {
+        let mut had_contacts_before = [false; SIMD_WIDTH];
+
+        for (i, pair) in context.pairs.iter().enumerate() {
+            had_contacts_before[i] = pair.has_any_active_contact()
+        }
+
+        match self {
+            Self::NearPhase(gen) => (gen.generate_contacts_simd)(&mut context),
+            Self::ExactPhase(gen) => {
+                // Build the primitive context from the non-primitive context and dispatch.
+                use arrayvec::ArrayVec;
+                let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> =
+                    ArrayVec::new();
+                let mut manifold_arr: ArrayVec<[&mut ContactManifold; SIMD_WIDTH]> =
+                    ArrayVec::new();
+                let mut workspace_arr: ArrayVec<
+                    [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH],
+                > = ArrayVec::new();
+
+                for pair in context.pairs.iter_mut() {
+                    let (collider1, collider2, manifold, workspace) =
+                        pair.single_manifold(context.colliders);
+                    colliders_arr.push((collider1, collider2));
+                    manifold_arr.push(manifold);
+                    workspace_arr.push(workspace);
+                }
+
+                let max_index = colliders_arr.len() - 1;
+                let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH];
+                let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH];
+
+                let mut context2 = PrimitiveContactGenerationContextSimd {
+                    prediction_distance: context.prediction_distance,
+                    colliders1,
+                    colliders2,
+                    shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH],
+                    shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH],
+                    positions1: &Isometry::from(
+                        array![|ii| *colliders1[ii].position(); SIMD_WIDTH],
+                    ),
+                    positions2: &Isometry::from(
+                        array![|ii| *colliders2[ii].position(); SIMD_WIDTH],
+                    ),
+                    manifolds: manifold_arr.as_mut_slice(),
+                    workspaces: workspace_arr.as_mut_slice(),
+                };
+
+                (gen.generate_contacts_simd)(&mut context2)
+            }
+        }
+
+        for (i, pair) in context.pairs.iter().enumerate() {
+            if had_contacts_before[i] != pair.has_any_active_contact() {
+                if had_contacts_before[i] {
+                    events.handle_contact_event(ContactEvent::Stopped(
+                        pair.pair.collider1,
+                        pair.pair.collider2,
+                    ))
+                } else {
+                    events.handle_contact_event(ContactEvent::Started(
+                        pair.pair.collider1,
+                        pair.pair.collider2,
+                    ))
+                }
+            }
+        }
+    }
+}
+
+pub struct PrimitiveContactGenerationContext<'a> {
+    pub prediction_distance: f32,
+    pub collider1: &'a Collider,
+    pub collider2: &'a Collider,
+    pub shape1: &'a Shape,
+    pub shape2: &'a Shape,
+    pub position1: &'a Isometry<f32>,
+    pub position2: &'a Isometry<f32>,
+    pub manifold: &'a mut ContactManifold,
+    pub workspace: Option<&'a mut (dyn Any + Send + Sync)>,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub struct PrimitiveContactGenerationContextSimd<'a, 'b> {
+    pub prediction_distance: f32,
+    pub colliders1: [&'a Collider; SIMD_WIDTH],
+    pub colliders2: [&'a Collider; SIMD_WIDTH],
+    pub shapes1: [&'a Shape; SIMD_WIDTH],
+    pub shapes2: [&'a Shape; SIMD_WIDTH],
+    pub positions1: &'a Isometry<SimdFloat>,
+    pub positions2: &'a Isometry<SimdFloat>,
+    pub manifolds: &'a mut [&'b mut ContactManifold],
+    pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],
+}
+
+#[derive(Copy, Clone)]
+pub struct PrimitiveContactGenerator {
+    pub generate_contacts: fn(&mut PrimitiveContactGenerationContext),
+    #[cfg(feature = "simd-is-enabled")]
+    pub generate_contacts_simd: fn(&mut PrimitiveContactGenerationContextSimd),
+}
+
+impl PrimitiveContactGenerator {
+    fn unimplemented_fn(_ctxt: &mut PrimitiveContactGenerationContext) {}
+    #[cfg(feature = "simd-is-enabled")]
+    fn unimplemented_simd_fn(_ctxt: &mut PrimitiveContactGenerationContextSimd) {}
+}
+
+impl Default for PrimitiveContactGenerator {
+    fn default() -> Self {
+        Self {
+            generate_contacts: Self::unimplemented_fn,
+            #[cfg(feature = "simd-is-enabled")]
+            generate_contacts_simd: Self::unimplemented_simd_fn,
+        }
+    }
+}
+
+pub struct ContactGenerationContext<'a> {
+    pub dispatcher: &'a dyn ContactDispatcher,
+    pub prediction_distance: f32,
+    pub colliders: &'a ColliderSet,
+    pub pair: &'a mut ContactPair,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub struct ContactGenerationContextSimd<'a, 'b> {
+    pub dispatcher: &'a dyn ContactDispatcher,
+    pub prediction_distance: f32,
+    pub colliders: &'a ColliderSet,
+    pub pairs: &'a mut [&'b mut ContactPair],
+}
+
+#[derive(Copy, Clone)]
+pub struct ContactGenerator {
+    pub generate_contacts: fn(&mut ContactGenerationContext),
+    #[cfg(feature = "simd-is-enabled")]
+    pub generate_contacts_simd: fn(&mut ContactGenerationContextSimd),
+}
+
+impl ContactGenerator {
+    fn unimplemented_fn(_ctxt: &mut ContactGenerationContext) {}
+    #[cfg(feature = "simd-is-enabled")]
+    fn unimplemented_simd_fn(_ctxt: &mut ContactGenerationContextSimd) {}
+}
+
+impl Default for ContactGenerator {
+    fn default() -> Self {
+        Self {
+            generate_contacts: Self::unimplemented_fn,
+            #[cfg(feature = "simd-is-enabled")]
+            generate_contacts_simd: Self::unimplemented_simd_fn,
+        }
+    }
+}

src/geometry/contact_generator/cuboid_capsule_contact_generator.rs

@@ -0,0 +1,188 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+#[cfg(feature = "dim3")]
+use crate::geometry::PolyhedronFace;
+use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory, Shape};
+#[cfg(feature = "dim2")]
+use crate::geometry::{CuboidFeature, CuboidFeatureFace};
+use crate::math::Isometry;
+use crate::math::Vector;
+use ncollide::shape::Segment;
+
+pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            ctxt.prediction_distance,
+            cube1,
+            ctxt.position1,
+            capsule2,
+            ctxt.position2,
+            ctxt.manifold,
+            false,
+        );
+        ctxt.manifold.update_warmstart_multiplier();
+    } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            ctxt.prediction_distance,
+            cube2,
+            ctxt.position2,
+            capsule1,
+            ctxt.position1,
+            ctxt.manifold,
+            true,
+        );
+        ctxt.manifold.update_warmstart_multiplier();
+    }
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}
+
+pub fn generate_contacts<'a>(
+    prediction_distance: f32,
+    cube1: &'a Cuboid,
+    mut pos1: &'a Isometry<f32>,
+    capsule2: &'a Capsule,
+    mut pos2: &'a Isometry<f32>,
+    manifold: &mut ContactManifold,
+    swapped: bool,
+) {
+    let mut pos12 = pos1.inverse() * pos2;
+    let mut pos21 = pos12.inverse();
+
+    if (!swapped && manifold.try_update_contacts(&pos12))
+        || (swapped && manifold.try_update_contacts(&pos21))
+    {
+        return;
+    }
+
+    let segment2 = Segment::new(capsule2.a, capsule2.b);
+
+    /*
+     *
+     * Point-Face cases.
+     *
+     */
+    let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12);
+    if sep1.0 > capsule2.radius + prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    #[cfg(feature = "dim3")]
+    let sep2 = (-f32::MAX, Vector::x());
+    #[cfg(feature = "dim2")]
+    let sep2 = sat::segment_cuboid_find_local_separating_normal_oneway(&segment2, cube1, &pos21);
+    if sep2.0 > capsule2.radius + prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    /*
+     *
+     * Edge-Edge cases.
+     *
+     */
+    #[cfg(feature = "dim2")]
+    let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
+    #[cfg(feature = "dim3")]
+    let sep3 =
+        sat::cube_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12, &pos21);
+    if sep3.0 > capsule2.radius + prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    /*
+     *
+     * Select the best combination of features
+     * and get the polygons to clip.
+     *
+     */
+    let mut swapped_reference = false;
+    let mut best_sep = sep1;
+
+    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
+        // The reference shape will be the second shape.
+        // std::mem::swap(&mut cube1, &mut capsule2);
+        std::mem::swap(&mut pos1, &mut pos2);
+        std::mem::swap(&mut pos12, &mut pos21);
+        best_sep = sep2;
+        swapped_reference = true;
+    } else if sep3.0 > sep1.0 {
+        best_sep = sep3;
+    }
+
+    let feature1;
+    let mut feature2;
+
+    #[cfg(feature = "dim2")]
+    {
+        if swapped_reference {
+            feature1 = CuboidFeatureFace::from(segment2);
+            feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1);
+        } else {
+            feature1 = cuboid::support_face(cube1, best_sep.1);
+            feature2 = CuboidFeatureFace::from(segment2);
+        }
+    }
+    #[cfg(feature = "dim3")]
+    {
+        if swapped_reference {
+            feature1 = PolyhedronFace::from(segment2);
+            feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1);
+        } else {
+            feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1);
+            feature2 = PolyhedronFace::from(segment2);
+        }
+    }
+
+    feature2.transform_by(&pos12);
+
+    if swapped ^ swapped_reference {
+        manifold.swap_identifiers();
+    }
+
+    // We do this clone to perform contact tracking and transfer impulses.
+    // FIXME: find a more efficient way of doing this.
+    let old_manifold_points = manifold.points.clone();
+    manifold.points.clear();
+
+    #[cfg(feature = "dim2")]
+    CuboidFeature::face_face_contacts(
+        prediction_distance + capsule2.radius,
+        &feature1,
+        &best_sep.1,
+        &feature2,
+        &pos21,
+        manifold,
+    );
+    #[cfg(feature = "dim3")]
+    PolyhedronFace::contacts(
+        prediction_distance + capsule2.radius,
+        &feature1,
+        &best_sep.1,
+        &feature2,
+        &pos21,
+        manifold,
+    );
+
+    // Adjust points to take the radius into account.
+    manifold.local_n1 = best_sep.1;
+    manifold.local_n2 = pos21 * -best_sep.1;
+    manifold.kinematics.category = KinematicsCategory::PlanePoint;
+    manifold.kinematics.radius1 = 0.0;
+    manifold.kinematics.radius2 = 0.0;
+
+    if swapped_reference {
+        for point in &mut manifold.points {
+            point.local_p1 += manifold.local_n1 * capsule2.radius;
+            point.dist -= capsule2.radius;
+        }
+    } else {
+        for point in &mut manifold.points {
+            point.local_p2 += manifold.local_n2 * capsule2.radius;
+            point.dist -= capsule2.radius;
+        }
+    }
+
+    // Transfer impulses.
+    super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference);
+}

src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs

@@ -0,0 +1,155 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape};
+use crate::math::Isometry;
+#[cfg(feature = "dim2")]
+use crate::math::Vector;
+use ncollide::shape::Cuboid;
+
+pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            ctxt.prediction_distance,
+            cube1,
+            ctxt.position1,
+            cube2,
+            ctxt.position2,
+            ctxt.manifold,
+        );
+    } else {
+        unreachable!()
+    }
+
+    ctxt.manifold.update_warmstart_multiplier();
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}
+
+pub fn generate_contacts<'a>(
+    prediction_distance: f32,
+    mut cube1: &'a Cuboid<f32>,
+    mut pos1: &'a Isometry<f32>,
+    mut cube2: &'a Cuboid<f32>,
+    mut pos2: &'a Isometry<f32>,
+    manifold: &mut ContactManifold,
+) {
+    let mut pos12 = pos1.inverse() * pos2;
+    let mut pos21 = pos12.inverse();
+
+    if manifold.try_update_contacts(&pos12) {
+        return;
+    }
+
+    /*
+     *
+     * Point-Face
+     *
+     */
+    let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21);
+    if sep1.0 > prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12);
+    if sep2.0 > prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    /*
+     *
+     * Edge-Edge cases
+     *
+     */
+    #[cfg(feature = "dim2")]
+    let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
+    #[cfg(feature = "dim3")]
+    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21);
+    if sep3.0 > prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    /*
+     *
+     * Select the best combination of features
+     * and get the polygons to clip.
+     *
+     */
+    let mut swapped = false;
+    let mut best_sep = sep1;
+
+    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
+        // The reference shape will be the second shape.
+        std::mem::swap(&mut cube1, &mut cube2);
+        std::mem::swap(&mut pos1, &mut pos2);
+        std::mem::swap(&mut pos12, &mut pos21);
+        manifold.swap_identifiers();
+        best_sep = sep2;
+        swapped = true;
+    } else if sep3.0 > sep1.0 {
+        best_sep = sep3;
+    }
+
+    // We do this clone to perform contact tracking and transfer impulses.
+    // FIXME: find a more efficient way of doing this.
+    let old_manifold_points = manifold.points.clone();
+    manifold.points.clear();
+
+    // Now the reference feature is from `cube1` and the best separation is `best_sep`.
+    // Everything must be expressed in the local-space of `cube1` for contact clipping.
+    let feature1 = cuboid::support_feature(cube1, best_sep.1);
+    let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1);
+    feature2.transform_by(&pos12);
+
+    match (&feature1, &feature2) {
+        (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => {
+            CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold)
+        }
+        #[cfg(feature = "dim3")]
+        (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts(
+            prediction_distance,
+            f1,
+            &best_sep.1,
+            e2,
+            &pos21,
+            manifold,
+            false,
+        ),
+        (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts(
+            prediction_distance,
+            f1,
+            &best_sep.1,
+            f2,
+            &pos21,
+            manifold,
+        ),
+        #[cfg(feature = "dim3")]
+        (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => {
+            CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold)
+        }
+        #[cfg(feature = "dim3")]
+        (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => {
+            // Since f2 is also expressed in the local-space of the first
+            // feature, the position we provide here is pos21.
+            CuboidFeature::face_edge_contacts(
+                prediction_distance,
+                f2,
+                &-best_sep.1,
+                e1,
+                &pos21,
+                manifold,
+                true,
+            )
+        }
+        _ => unreachable!(), // The other cases are not possible.
+    }
+
+    manifold.local_n1 = best_sep.1;
+    manifold.local_n2 = pos21 * -best_sep.1;
+    manifold.kinematics.category = KinematicsCategory::PlanePoint;
+    manifold.kinematics.radius1 = 0.0;
+    manifold.kinematics.radius2 = 0.0;
+
+    // Transfer impulses.
+    super::match_contacts(manifold, &old_manifold_points, swapped);
+}

src/geometry/contact_generator/cuboid_triangle_contact_generator.rs

@@ -0,0 +1,171 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+#[cfg(feature = "dim3")]
+use crate::geometry::PolyhedronFace;
+use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Shape, Triangle};
+use crate::math::Isometry;
+#[cfg(feature = "dim2")]
+use crate::{
+    geometry::{triangle, CuboidFeature},
+    math::Vector,
+};
+
+pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            ctxt.prediction_distance,
+            cube1,
+            ctxt.position1,
+            triangle2,
+            ctxt.position2,
+            ctxt.manifold,
+            false,
+        );
+        ctxt.manifold.update_warmstart_multiplier();
+    } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            ctxt.prediction_distance,
+            cube2,
+            ctxt.position2,
+            triangle1,
+            ctxt.position1,
+            ctxt.manifold,
+            true,
+        );
+        ctxt.manifold.update_warmstart_multiplier();
+    }
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}
+
+pub fn generate_contacts<'a>(
+    prediction_distance: f32,
+    cube1: &'a Cuboid,
+    mut pos1: &'a Isometry<f32>,
+    triangle2: &'a Triangle,
+    mut pos2: &'a Isometry<f32>,
+    manifold: &mut ContactManifold,
+    swapped: bool,
+) {
+    let mut pos12 = pos1.inverse() * pos2;
+    let mut pos21 = pos12.inverse();
+
+    if (!swapped && manifold.try_update_contacts(&pos12))
+        || (swapped && manifold.try_update_contacts(&pos21))
+    {
+        return;
+    }
+
+    /*
+     *
+     * Point-Face cases.
+     *
+     */
+    let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12);
+    if sep1.0 > prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21);
+    if sep2.0 > prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    /*
+     *
+     * Edge-Edge cases.
+     *
+     */
+    #[cfg(feature = "dim2")]
+    let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
+    #[cfg(feature = "dim3")]
+    let sep3 =
+        sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21);
+    if sep3.0 > prediction_distance {
+        manifold.points.clear();
+        return;
+    }
+
+    /*
+     *
+     * Select the best combination of features
+     * and get the polygons to clip.
+     *
+     */
+    let mut swapped_reference = false;
+    let mut best_sep = sep1;
+
+    if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
+        // The reference shape will be the second shape.
+        // std::mem::swap(&mut cube1, &mut triangle2);
+        std::mem::swap(&mut pos1, &mut pos2);
+        std::mem::swap(&mut pos12, &mut pos21);
+        best_sep = sep2;
+        swapped_reference = true;
+    } else if sep3.0 > sep1.0 {
+        best_sep = sep3;
+    }
+
+    let feature1;
+    let mut feature2;
+
+    #[cfg(feature = "dim2")]
+    {
+        if swapped_reference {
+            feature1 = triangle::support_face(triangle2, best_sep.1);
+            feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1);
+        } else {
+            feature1 = cuboid::support_face(cube1, best_sep.1);
+            feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1);
+        }
+    }
+    #[cfg(feature = "dim3")]
+    {
+        if swapped_reference {
+            feature1 = PolyhedronFace::from(*triangle2);
+            feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1);
+        } else {
+            feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1);
+            feature2 = PolyhedronFace::from(*triangle2);
+        }
+    }
+
+    feature2.transform_by(&pos12);
+
+    if swapped ^ swapped_reference {
+        manifold.swap_identifiers();
+    }
+
+    // We do this clone to perform contact tracking and transfer impulses.
+    // FIXME: find a more efficient way of doing this.
+    let old_manifold_points = manifold.points.clone();
+    manifold.points.clear();
+
+    #[cfg(feature = "dim2")]
+    CuboidFeature::face_face_contacts(
+        prediction_distance,
+        &feature1,
+        &best_sep.1,
+        &feature2,
+        &pos21,
+        manifold,
+    );
+    #[cfg(feature = "dim3")]
+    PolyhedronFace::contacts(
+        prediction_distance,
+        &feature1,
+        &best_sep.1,
+        &feature2,
+        &pos21,
+        manifold,
+    );
+
+    manifold.local_n1 = best_sep.1;
+    manifold.local_n2 = pos21 * -best_sep.1;
+    manifold.kinematics.category = KinematicsCategory::PlanePoint;
+    manifold.kinematics.radius1 = 0.0;
+    manifold.kinematics.radius2 = 0.0;
+
+    // Transfer impulses.
+    super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference);
+}

src/geometry/contact_generator/heightfield_shape_contact_generator.rs

@@ -0,0 +1,189 @@
+use crate::geometry::contact_generator::{
+    ContactGenerationContext, PrimitiveContactGenerationContext, PrimitiveContactGenerator,
+};
+#[cfg(feature = "dim2")]
+use crate::geometry::Capsule;
+use crate::geometry::{Collider, ContactManifold, HeightField, Shape};
+use crate::ncollide::bounding_volume::BoundingVolume;
+#[cfg(feature = "dim3")]
+use crate::{geometry::Triangle, math::Point};
+use std::any::Any;
+use std::collections::hash_map::Entry;
+use std::collections::HashMap;
+
+struct SubDetector {
+    generator: PrimitiveContactGenerator,
+    manifold_id: usize,
+    timestamp: bool,
+    workspace: Option<Box<(dyn Any + Send + Sync)>>,
+}
+
+pub struct HeightFieldShapeContactGeneratorWorkspace {
+    timestamp: bool,
+    old_manifolds: Vec<ContactManifold>,
+    sub_detectors: HashMap<usize, SubDetector>,
+}
+
+impl HeightFieldShapeContactGeneratorWorkspace {
+    pub fn new() -> Self {
+        Self {
+            timestamp: false,
+            old_manifolds: Vec::new(),
+            sub_detectors: HashMap::default(),
+        }
+    }
+}
+
+pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) {
+    let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
+    let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
+
+    if let Shape::HeightField(heightfield1) = collider1.shape() {
+        do_generate_contacts(heightfield1, collider1, collider2, ctxt, false)
+    } else if let Shape::HeightField(heightfield2) = collider2.shape() {
+        do_generate_contacts(heightfield2, collider2, collider1, ctxt, true)
+    }
+}
+
+fn do_generate_contacts(
+    heightfield1: &HeightField,
+    collider1: &Collider,
+    collider2: &Collider,
+    ctxt: &mut ContactGenerationContext,
+    _flipped: bool,
+) {
+    let workspace: &mut HeightFieldShapeContactGeneratorWorkspace = ctxt
+        .pair
+        .generator_workspace
+        .as_mut()
+        .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.")
+        .downcast_mut()
+        .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace.");
+
+    /*
+     * Detect if the detector context has been reset.
+     */
+    if !ctxt.pair.manifolds.is_empty() && workspace.sub_detectors.is_empty() {
+        // Rebuild the subdetector hashmap.
+        for (manifold_id, manifold) in ctxt.pair.manifolds.iter().enumerate() {
+            let subshape_id = if manifold.pair.collider1 == ctxt.pair.pair.collider1 {
+                manifold.subshape_index_pair.0
+            } else {
+                manifold.subshape_index_pair.1
+            };
+            println!(
+                "Restoring for {} [chosen with {:?}]",
+                subshape_id, manifold.subshape_index_pair
+            );
+
+            // Use dummy shapes for the dispatch.
+            #[cfg(feature = "dim2")]
+            let sub_shape1 =
+                Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0));
+            #[cfg(feature = "dim3")]
+            let sub_shape1 = Shape::Triangle(Triangle::new(
+                Point::origin(),
+                Point::origin(),
+                Point::origin(),
+            ));
+            let (generator, workspace2) = ctxt
+                .dispatcher
+                .dispatch_primitives(&sub_shape1, collider2.shape());
+
+            let sub_detector = SubDetector {
+                generator,
+                manifold_id,
+                timestamp: workspace.timestamp,
+                workspace: workspace2,
+            };
+
+            workspace.sub_detectors.insert(subshape_id, sub_detector);
+        }
+    }
+
+    let new_timestamp = !workspace.timestamp;
+    workspace.timestamp = new_timestamp;
+
+    /*
+     * Compute interferences.
+     */
+    let pos12 = collider1.position.inverse() * collider2.position;
+    // TODO: somehow precompute the AABB and reuse it?
+    let ls_aabb2 = collider2
+        .shape()
+        .compute_aabb(&pos12)
+        .loosened(ctxt.prediction_distance);
+
+    std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds);
+    ctxt.pair.manifolds.clear();
+    let coll_pair = ctxt.pair.pair;
+    let manifolds = &mut ctxt.pair.manifolds;
+    let prediction_distance = ctxt.prediction_distance;
+    let dispatcher = ctxt.dispatcher;
+
+    heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| {
+        #[cfg(feature = "dim2")]
+        let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0));
+        #[cfg(feature = "dim3")]
+        let sub_shape1 = Shape::Triangle(*part1);
+        let sub_detector = match workspace.sub_detectors.entry(i) {
+            Entry::Occupied(entry) => {
+                let sub_detector = entry.into_mut();
+                let manifold = workspace.old_manifolds[sub_detector.manifold_id].take();
+                sub_detector.manifold_id = manifolds.len();
+                sub_detector.timestamp = new_timestamp;
+                manifolds.push(manifold);
+                sub_detector
+            }
+            Entry::Vacant(entry) => {
+                let (generator, workspace2) =
+                    dispatcher.dispatch_primitives(&sub_shape1, collider2.shape());
+                let sub_detector = SubDetector {
+                    generator,
+                    manifold_id: manifolds.len(),
+                    timestamp: new_timestamp,
+                    workspace: workspace2,
+                };
+                let manifold =
+                    ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0);
+                manifolds.push(manifold);
+
+                entry.insert(sub_detector)
+            }
+        };
+
+        let manifold = &mut manifolds[sub_detector.manifold_id];
+
+        let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 {
+            PrimitiveContactGenerationContext {
+                prediction_distance,
+                collider1: collider2,
+                collider2: collider1,
+                shape1: collider2.shape(),
+                shape2: &sub_shape1,
+                position1: collider2.position(),
+                position2: collider1.position(),
+                manifold,
+                workspace: sub_detector.workspace.as_deref_mut(),
+            }
+        } else {
+            PrimitiveContactGenerationContext {
+                prediction_distance,
+                collider1,
+                collider2,
+                shape1: &sub_shape1,
+                shape2: collider2.shape(),
+                position1: collider1.position(),
+                position2: collider2.position(),
+                manifold,
+                workspace: sub_detector.workspace.as_deref_mut(),
+            }
+        };
+
+        (sub_detector.generator.generate_contacts)(&mut ctxt2)
+    });
+
+    workspace
+        .sub_detectors
+        .retain(|_, detector| detector.timestamp == new_timestamp)
+}

src/geometry/contact_generator/mod.rs

@@ -0,0 +1,71 @@
+pub use self::ball_ball_contact_generator::generate_contacts_ball_ball;
+#[cfg(feature = "simd-is-enabled")]
+pub use self::ball_ball_contact_generator::generate_contacts_ball_ball_simd;
+pub use self::ball_convex_contact_generator::generate_contacts_ball_convex;
+pub use self::capsule_capsule_contact_generator::generate_contacts_capsule_capsule;
+pub use self::contact_dispatcher::{ContactDispatcher, DefaultContactDispatcher};
+pub use self::contact_generator::{
+    ContactGenerationContext, ContactGenerator, ContactPhase, PrimitiveContactGenerationContext,
+    PrimitiveContactGenerator,
+};
+#[cfg(feature = "simd-is-enabled")]
+pub use self::contact_generator::{
+    ContactGenerationContextSimd, PrimitiveContactGenerationContextSimd,
+};
+pub use self::cuboid_capsule_contact_generator::generate_contacts_cuboid_capsule;
+pub use self::cuboid_cuboid_contact_generator::generate_contacts_cuboid_cuboid;
+pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triangle;
+pub use self::heightfield_shape_contact_generator::{
+    generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
+};
+pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
+pub use self::trimesh_shape_contact_generator::{
+    generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
+};
+
+#[cfg(feature = "dim2")]
+pub(crate) use self::polygon_polygon_contact_generator::{
+    clip_segments, clip_segments_with_normal,
+};
+
+mod ball_ball_contact_generator;
+mod ball_convex_contact_generator;
+mod ball_polygon_contact_generator;
+mod capsule_capsule_contact_generator;
+mod contact_dispatcher;
+mod contact_generator;
+mod cuboid_capsule_contact_generator;
+mod cuboid_cuboid_contact_generator;
+mod cuboid_polygon_contact_generator;
+mod cuboid_triangle_contact_generator;
+mod heightfield_shape_contact_generator;
+mod polygon_polygon_contact_generator;
+mod trimesh_shape_contact_generator;
+
+use crate::geometry::{Contact, ContactManifold};
+
+pub(crate) fn match_contacts(
+    manifold: &mut ContactManifold,
+    old_contacts: &[Contact],
+    swapped: bool,
+) {
+    for contact in &mut manifold.points {
+        if !swapped {
+            for old_contact in old_contacts {
+                if contact.fid1 == old_contact.fid1 && contact.fid2 == old_contact.fid2 {
+                    // Transfer impulse cache.
+                    contact.impulse = old_contact.impulse;
+                    contact.tangent_impulse = old_contact.tangent_impulse;
+                }
+            }
+        } else {
+            for old_contact in old_contacts {
+                if contact.fid1 == old_contact.fid2 && contact.fid2 == old_contact.fid1 {
+                    // Transfer impulse cache.
+                    contact.impulse = old_contact.impulse;
+                    contact.tangent_impulse = old_contact.tangent_impulse;
+                }
+            }
+        }
+    }
+}

src/geometry/contact_generator/polygon_polygon_contact_generator.rs

@@ -0,0 +1,298 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon, Shape};
+use crate::math::{Isometry, Point};
+#[cfg(feature = "dim2")]
+use crate::{math::Vector, utils};
+
+pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
+        generate_contacts(
+            polygon1,
+            &ctxt.position1,
+            polygon2,
+            &ctxt.position2,
+            ctxt.manifold,
+        );
+        ctxt.manifold.update_warmstart_multiplier();
+    } else {
+        unreachable!()
+    }
+
+    ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+}
+
+fn generate_contacts<'a>(
+    mut p1: &'a Polygon,
+    mut m1: &'a Isometry<f32>,
+    mut p2: &'a Polygon,
+    mut m2: &'a Isometry<f32>,
+    manifold: &'a mut ContactManifold,
+) {
+    let mut m12 = m1.inverse() * m2;
+    let mut m21 = m12.inverse();
+
+    if manifold.try_update_contacts(&m12) {
+        return;
+    }
+
+    let mut sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12);
+    if sep1.0 > 0.0 {
+        manifold.points.clear();
+        return;
+    }
+
+    let mut sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21);
+    if sep2.0 > 0.0 {
+        manifold.points.clear();
+        return;
+    }
+
+    let mut swapped = false;
+    if sep2.0 > sep1.0 {
+        std::mem::swap(&mut sep1, &mut sep2);
+        std::mem::swap(&mut m1, &mut m2);
+        std::mem::swap(&mut p1, &mut p2);
+        std::mem::swap(&mut m12, &mut m21);
+        manifold.swap_identifiers();
+        swapped = true;
+    }
+
+    let support_face1 = sep1.1;
+    let local_n1 = p1.normals[support_face1];
+    let local_n2 = m21 * -local_n1;
+    let support_face2 = p2.support_face(&local_n2);
+    let len1 = p1.vertices.len();
+    let len2 = p2.vertices.len();
+
+    let seg1 = (
+        p1.vertices[support_face1],
+        p1.vertices[(support_face1 + 1) % len1],
+    );
+    let seg2 = (
+        m12 * p2.vertices[support_face2],
+        m12 * p2.vertices[(support_face2 + 1) % len2],
+    );
+    if let Some((clip_a, clip_b)) = clip_segments(seg1, seg2) {
+        let dist_a = (clip_a.1 - clip_a.0).dot(&local_n1);
+        let dist_b = (clip_b.1 - clip_b.0).dot(&local_n1);
+
+        let mut impulses_a = (0.0, Contact::zero_tangent_impulse());
+        let mut impulses_b = (0.0, Contact::zero_tangent_impulse());
+
+        let fids_a = (
+            ((support_face1 * 2 + clip_a.2) % (len1 * 2)) as u8,
+            ((support_face2 * 2 + clip_a.3) % (len2 * 2)) as u8,
+        );
+
+        let fids_b = (
+            ((support_face1 * 2 + clip_b.2) % (len1 * 2)) as u8,
+            ((support_face2 * 2 + clip_b.3) % (len2 * 2)) as u8,
+        );
+
+        if manifold.points.len() != 0 {
+            assert_eq!(manifold.points.len(), 2);
+
+            // We already had 2 points in the previous iteration.
+            // Match the features to see if we keep the cached impulse.
+            let original_fids_a;
+            let original_fids_b;
+
+            // NOTE: the previous manifold may have its bodies swapped wrt. our new manifold.
+            // So we have to adjust accordingly the features we will be comparing.
+            if swapped {
+                original_fids_a = (manifold.points[0].fid1, manifold.points[0].fid2);
+                original_fids_b = (manifold.points[1].fid1, manifold.points[1].fid2);
+            } else {
+                original_fids_a = (manifold.points[0].fid2, manifold.points[0].fid1);
+                original_fids_b = (manifold.points[1].fid2, manifold.points[1].fid1);
+            }
+
+            if fids_a == original_fids_a {
+                impulses_a = (
+                    manifold.points[0].impulse,
+                    manifold.points[0].tangent_impulse,
+                );
+            } else if fids_a == original_fids_b {
+                impulses_a = (
+                    manifold.points[1].impulse,
+                    manifold.points[1].tangent_impulse,
+                );
+            }
+            if fids_b == original_fids_a {
+                impulses_b = (
+                    manifold.points[0].impulse,
+                    manifold.points[0].tangent_impulse,
+                );
+            } else if fids_b == original_fids_b {
+                impulses_b = (
+                    manifold.points[1].impulse,
+                    manifold.points[1].tangent_impulse,
+                );
+            }
+        }
+
+        manifold.points.clear();
+        manifold.points.push(Contact {
+            local_p1: clip_a.0,
+            local_p2: m21 * clip_a.1,
+            impulse: impulses_a.0,
+            tangent_impulse: impulses_a.1,
+            fid1: fids_a.0,
+            fid2: fids_a.1,
+            dist: dist_a,
+        });
+
+        manifold.points.push(Contact {
+            local_p1: clip_b.0,
+            local_p2: m21 * clip_b.1,
+            impulse: impulses_b.0,
+            tangent_impulse: impulses_b.1,
+            fid1: fids_b.0,
+            fid2: fids_b.1,
+            dist: dist_b,
+        });
+
+        manifold.local_n1 = local_n1;
+        manifold.local_n2 = local_n2;
+        manifold.kinematics.category = KinematicsCategory::PlanePoint;
+        manifold.kinematics.radius1 = 0.0;
+        manifold.kinematics.radius2 = 0.0;
+    } else {
+        manifold.points.clear();
+    }
+}
+
+// Features in clipping points are:
+// 0 = First vertex.
+// 1 = On the face.
+// 2 = Second vertex.
+pub(crate) type ClippingPoints = (Point<f32>, Point<f32>, usize, usize);
+
+#[cfg(feature = "dim2")]
+pub(crate) fn clip_segments_with_normal(
+    mut seg1: (Point<f32>, Point<f32>),
+    mut seg2: (Point<f32>, Point<f32>),
+    normal: Vector<f32>,
+) -> Option<(ClippingPoints, ClippingPoints)> {
+    use crate::utils::WBasis;
+    let tangent = normal.orthonormal_basis()[0];
+
+    let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)];
+    let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)];
+    let mut features1 = [0, 2];
+    let mut features2 = [0, 2];
+
+    if range1[1] < range1[0] {
+        range1.swap(0, 1);
+        features1.swap(0, 1);
+        std::mem::swap(&mut seg1.0, &mut seg1.1);
+    }
+
+    if range2[1] < range2[0] {
+        range2.swap(0, 1);
+        features2.swap(0, 1);
+        std::mem::swap(&mut seg2.0, &mut seg2.1);
+    }
+
+    if range2[0] > range1[1] || range1[0] > range2[1] {
+        // No clip point.
+        return None;
+    }
+
+    let ca = if range2[0] > range1[0] {
+        let bcoord = (range2[0] - range1[0]) * utils::inv(range1[1] - range1[0]);
+        let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord;
+        let p2 = seg2.0;
+
+        (p1, p2, 1, features2[0])
+    } else {
+        let bcoord = (range1[0] - range2[0]) * utils::inv(range2[1] - range2[0]);
+        let p1 = seg1.0;
+        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
+
+        (p1, p2, features1[0], 1)
+    };
+
+    let cb = if range2[1] < range1[1] {
+        let bcoord = (range2[1] - range1[0]) * utils::inv(range1[1] - range1[0]);
+        let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord;
+        let p2 = seg2.1;
+
+        (p1, p2, 1, features2[1])
+    } else {
+        let bcoord = (range1[1] - range2[0]) * utils::inv(range2[1] - range2[0]);
+        let p1 = seg1.1;
+        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
+
+        (p1, p2, features1[1], 1)
+    };
+
+    Some((ca, cb))
+}
+
+pub(crate) fn clip_segments(
+    mut seg1: (Point<f32>, Point<f32>),
+    mut seg2: (Point<f32>, Point<f32>),
+) -> Option<(ClippingPoints, ClippingPoints)> {
+    // NOTE: no need to normalize the tangent.
+    let tangent1 = seg1.1 - seg1.0;
+    let sqnorm_tangent1 = tangent1.norm_squared();
+
+    let mut range1 = [0.0, sqnorm_tangent1];
+    let mut range2 = [
+        (seg2.0 - seg1.0).dot(&tangent1),
+        (seg2.1 - seg1.0).dot(&tangent1),
+    ];
+    let mut features1 = [0, 2];
+    let mut features2 = [0, 2];
+
+    if range1[1] < range1[0] {
+        range1.swap(0, 1);
+        features1.swap(0, 1);
+        std::mem::swap(&mut seg1.0, &mut seg1.1);
+    }
+
+    if range2[1] < range2[0] {
+        range2.swap(0, 1);
+        features2.swap(0, 1);
+        std::mem::swap(&mut seg2.0, &mut seg2.1);
+    }
+
+    if range2[0] > range1[1] || range1[0] > range2[1] {
+        // No clip point.
+        return None;
+    }
+
+    let length1 = range1[1] - range1[0];
+    let length2 = range2[1] - range2[0];
+
+    let ca = if range2[0] > range1[0] {
+        let bcoord = (range2[0] - range1[0]) / length1;
+        let p1 = seg1.0 + tangent1 * bcoord;
+        let p2 = seg2.0;
+
+        (p1, p2, 1, features2[0])
+    } else {
+        let bcoord = (range1[0] - range2[0]) / length2;
+        let p1 = seg1.0;
+        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
+
+        (p1, p2, features1[0], 1)
+    };
+
+    let cb = if range2[1] < range1[1] {
+        let bcoord = (range2[1] - range1[0]) / length1;
+        let p1 = seg1.0 + tangent1 * bcoord;
+        let p2 = seg2.1;
+
+        (p1, p2, 1, features2[1])
+    } else {
+        let bcoord = (range1[1] - range2[0]) / length2;
+        let p1 = seg1.1;
+        let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
+
+        (p1, p2, features1[1], 1)
+    };
+
+    Some((ca, cb))
+}

src/geometry/contact_generator/trimesh_shape_contact_generator.rs

@@ -0,0 +1,194 @@
+use crate::geometry::contact_generator::{
+    ContactGenerationContext, PrimitiveContactGenerationContext,
+};
+use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyIntersections};
+use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
+
+pub struct TrimeshShapeContactGeneratorWorkspace {
+    interferences: WAABBHierarchyIntersections,
+    local_aabb2: AABB<f32>,
+    old_interferences: Vec<usize>,
+    old_manifolds: Vec<ContactManifold>,
+}
+
+impl TrimeshShapeContactGeneratorWorkspace {
+    pub fn new() -> Self {
+        Self {
+            interferences: WAABBHierarchyIntersections::new(),
+            local_aabb2: AABB::new_invalid(),
+            old_interferences: Vec::new(),
+            old_manifolds: Vec::new(),
+        }
+    }
+}
+
+pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) {
+    let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
+    let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
+
+    if let Shape::Trimesh(trimesh1) = collider1.shape() {
+        do_generate_contacts(trimesh1, collider1, collider2, ctxt, false)
+    } else if let Shape::Trimesh(trimesh2) = collider2.shape() {
+        do_generate_contacts(trimesh2, collider2, collider1, ctxt, true)
+    }
+}
+
+fn do_generate_contacts(
+    trimesh1: &Trimesh,
+    collider1: &Collider,
+    collider2: &Collider,
+    ctxt: &mut ContactGenerationContext,
+    flipped: bool,
+) {
+    let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt
+        .pair
+        .generator_workspace
+        .as_mut()
+        .expect("The TrimeshShapeContactGeneratorWorkspace is missing.")
+        .downcast_mut()
+        .expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace.");
+
+    /*
+     * Compute interferences.
+     */
+    let pos12 = collider1.position.inverse() * collider2.position;
+    // TODO: somehow precompute the AABB and reuse it?
+    let mut new_local_aabb2 = collider2
+        .shape()
+        .compute_aabb(&pos12)
+        .loosened(ctxt.prediction_distance);
+    let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2);
+
+    if !same_local_aabb2 {
+        let extra_margin =
+            (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1));
+        new_local_aabb2.mins -= extra_margin;
+        new_local_aabb2.maxs += extra_margin;
+
+        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(),
+        );
+        std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds);
+        ctxt.pair.manifolds.clear();
+
+        if workspace.old_interferences.is_empty() && !workspace.old_manifolds.is_empty() {
+            // This happens if for some reasons the contact generator context was lost
+            // and rebuilt. In this case, we hate to reconstruct the `old_interferences`
+            // array using the subshape ids from the contact manifolds.
+            // TODO: always rely on the subshape ids instead of maintaining `.ord_interferences` ?
+            let ctxt_collider1 = ctxt.pair.pair.collider1;
+            workspace.old_interferences = workspace
+                .old_manifolds
+                .iter()
+                .map(|manifold| {
+                    if manifold.pair.collider1 == ctxt_collider1 {
+                        manifold.subshape_index_pair.0
+                    } else {
+                        manifold.subshape_index_pair.1
+                    }
+                })
+                .collect();
+        }
+        assert_eq!(
+            workspace
+                .old_interferences
+                .len()
+                .min(trimesh1.num_triangles()),
+            workspace.old_manifolds.len()
+        );
+
+        trimesh1
+            .waabbs()
+            .compute_interferences_with(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 mut old_inter_it = workspace.old_interferences.drain(..).peekable();
+    let mut old_manifolds_it = workspace.old_manifolds.drain(..);
+
+    for (i, triangle_id) in new_interferences.iter().enumerate() {
+        if *triangle_id >= trimesh1.num_triangles() {
+            // Because of SIMD padding, the broad-phase may return tiangle indices greater
+            // than the max.
+            continue;
+        }
+        if !same_local_aabb2 {
+            loop {
+                match old_inter_it.peek() {
+                    Some(old_triangle_id) if *old_triangle_id < *triangle_id => {
+                        old_inter_it.next();
+                        old_manifolds_it.next();
+                    }
+                    _ => break,
+                }
+            }
+
+            let manifold = if old_inter_it.peek() != Some(triangle_id) {
+                // We don't have a manifold for this triangle yet.
+                if flipped {
+                    ContactManifold::with_subshape_indices(
+                        ctxt.pair.pair,
+                        collider2,
+                        collider1,
+                        *triangle_id,
+                        0,
+                    )
+                } else {
+                    ContactManifold::with_subshape_indices(
+                        ctxt.pair.pair,
+                        collider1,
+                        collider2,
+                        0,
+                        *triangle_id,
+                    )
+                }
+            } else {
+                // We already have a manifold for this triangle.
+                old_inter_it.next();
+                old_manifolds_it.next().unwrap()
+            };
+
+            ctxt.pair.manifolds.push(manifold);
+        }
+
+        let manifold = &mut ctxt.pair.manifolds[i];
+        let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
+        let (generator, mut workspace2) = ctxt
+            .dispatcher
+            .dispatch_primitives(&triangle1, collider2.shape());
+
+        let mut ctxt2 = if ctxt.pair.pair.collider1 != manifold.pair.collider1 {
+            PrimitiveContactGenerationContext {
+                prediction_distance: ctxt.prediction_distance,
+                collider1: collider2,
+                collider2: collider1,
+                shape1: collider2.shape(),
+                shape2: &triangle1,
+                position1: collider2.position(),
+                position2: collider1.position(),
+                manifold,
+                workspace: workspace2.as_deref_mut(),
+            }
+        } else {
+            PrimitiveContactGenerationContext {
+                prediction_distance: ctxt.prediction_distance,
+                collider1,
+                collider2,
+                shape1: &triangle1,
+                shape2: collider2.shape(),
+                position1: collider1.position(),
+                position2: collider2.position(),
+                manifold,
+                workspace: workspace2.as_deref_mut(),
+            }
+        };
+
+        (generator.generate_contacts)(&mut ctxt2);
+    }
+}

src/geometry/cuboid.rs

@@ -0,0 +1,234 @@
+#[cfg(feature = "dim3")]
+use crate::geometry::PolyhedronFace;
+use crate::geometry::{Cuboid, CuboidFeature, CuboidFeatureFace};
+use crate::math::{Point, Vector};
+use crate::utils::WSign;
+
+pub fn local_support_point(cube: &Cuboid, local_dir: Vector<f32>) -> Point<f32> {
+    local_dir.copy_sign_to(cube.half_extents).into()
+}
+
+// #[cfg(feature = "dim2")]
+// pub fn polygon_ref(
+//     cuboid: Cuboid,
+//     out_vertices: &mut [Point<f32>; 4],
+//     out_normals: &mut [Vector<f32>; 4],
+// ) -> PolygonRef {
+//     *out_vertices = [
+//         Point::new(cuboid.half_extents.x, -cuboid.half_extents.y),
+//         Point::new(cuboid.half_extents.x, cuboid.half_extents.y),
+//         Point::new(-cuboid.half_extents.x, cuboid.half_extents.y),
+//         Point::new(-cuboid.half_extents.x, -cuboid.half_extents.y),
+//     ];
+//     *out_normals = [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()];
+//
+//     PolygonRef {
+//         vertices: &out_vertices[..],
+//         normals: &out_normals[..],
+//     }
+// }
+
+#[cfg(feature = "dim2")]
+pub fn vertex_feature_id(vertex: Point<f32>) -> u8 {
+    ((vertex.x.to_bits() >> 31) & 0b001 | (vertex.y.to_bits() >> 30) & 0b010) as u8
+}
+
+// #[cfg(feature = "dim3")]
+// pub fn vertex_feature_id(vertex: Point<f32>) -> u8 {
+//     ((vertex.x.to_bits() >> 31) & 0b001
+//         | (vertex.y.to_bits() >> 30) & 0b010
+//         | (vertex.z.to_bits() >> 29) & 0b100) as u8
+// }
+
+#[cfg(feature = "dim3")]
+pub fn polyhedron_support_face(cube: &Cuboid, local_dir: Vector<f32>) -> PolyhedronFace {
+    support_face(cube, local_dir).into()
+}
+
+#[cfg(feature = "dim2")]
+pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeature {
+    // In 2D, it is best for stability to always return a face.
+    // It won't have any notable impact on performances anyway.
+    CuboidFeature::Face(support_face(cube, local_dir))
+
+    /*
+    let amax = local_dir.amax();
+
+    const MAX_DOT_THRESHOLD: f32 = 0.98480775301; // 10 degrees.
+
+    if amax > MAX_DOT_THRESHOLD {
+        // Support face.
+        CuboidFeature::Face(cube.support_face(local_dir))
+    } else {
+        // Support vertex
+        CuboidFeature::Vertex(cube.support_vertex(local_dir))
+    }
+    */
+}
+
+#[cfg(feature = "dim3")]
+pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeature {
+    CuboidFeature::Face(support_face(cube, local_dir))
+    /*
+    const MAX_DOT_THRESHOLD: f32 = crate::utils::COS_10_DEGREES;
+    const MIN_DOT_THRESHOLD: f32 = 1.0 - MAX_DOT_THRESHOLD;
+
+    let amax = local_dir.amax();
+    let amin = local_dir.amin();
+
+    if amax > MAX_DOT_THRESHOLD {
+        // Support face.
+        CuboidFeature::Face(support_face(cube, local_dir))
+    } else if amin < MIN_DOT_THRESHOLD {
+        // Support edge.
+        CuboidFeature::Edge(support_edge(cube, local_dir))
+    } else {
+        // Support vertex.
+        CuboidFeature::Vertex(support_vertex(cube, local_dir))
+    }
+    */
+}
+
+// #[cfg(feature = "dim3")]
+// pub(crate) fn support_vertex(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureVertex {
+//     let vertex = local_support_point(cube, local_dir);
+//     let vid = vertex_feature_id(vertex);
+//
+//     CuboidFeatureVertex { vertex, vid }
+// }
+
+// #[cfg(feature = "dim3")]
+// pub(crate) fn support_edge(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureEdge {
+//     let he = cube.half_extents;
+//     let i = local_dir.iamin();
+//     let j = (i + 1) % 3;
+//     let k = (i + 2) % 3;
+//     let mut a = Point::origin();
+//     a[i] = he[i];
+//     a[j] = local_dir[j].copy_sign_to(he[j]);
+//     a[k] = local_dir[k].copy_sign_to(he[k]);
+//
+//     let mut b = a;
+//     b[i] = -he[i];
+//
+//     let vid1 = vertex_feature_id(a);
+//     let vid2 = vertex_feature_id(b);
+//     let eid = (vid1.max(vid2) << 3) | vid1.min(vid2) | 0b11_000_000;
+//
+//     CuboidFeatureEdge {
+//         vertices: [a, b],
+//         vids: [vid1, vid2],
+//         eid,
+//     }
+// }
+
+#[cfg(feature = "dim2")]
+pub fn support_face(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureFace {
+    let he = cube.half_extents;
+    let i = local_dir.iamin();
+    let j = (i + 1) % 2;
+    let mut a = Point::origin();
+    a[i] = he[i];
+    a[j] = local_dir[j].copy_sign_to(he[j]);
+
+    let mut b = a;
+    b[i] = -he[i];
+
+    let vid1 = vertex_feature_id(a);
+    let vid2 = vertex_feature_id(b);
+    let fid = (vid1.max(vid2) << 2) | vid1.min(vid2) | 0b11_00_00;
+
+    CuboidFeatureFace {
+        vertices: [a, b],
+        vids: [vid1, vid2],
+        fid,
+    }
+}
+
+#[cfg(feature = "dim3")]
+pub(crate) fn support_face(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureFace {
+    // NOTE: can we use the orthonormal basis of local_dir
+    // to make this AoSoA friendly?
+    let he = cube.half_extents;
+    let iamax = local_dir.iamax();
+    let sign = local_dir[iamax].copy_sign_to(1.0);
+
+    let vertices = match iamax {
+        0 => [
+            Point::new(he.x * sign, he.y, he.z),
+            Point::new(he.x * sign, -he.y, he.z),
+            Point::new(he.x * sign, -he.y, -he.z),
+            Point::new(he.x * sign, he.y, -he.z),
+        ],
+        1 => [
+            Point::new(he.x, he.y * sign, he.z),
+            Point::new(-he.x, he.y * sign, he.z),
+            Point::new(-he.x, he.y * sign, -he.z),
+            Point::new(he.x, he.y * sign, -he.z),
+        ],
+        2 => [
+            Point::new(he.x, he.y, he.z * sign),
+            Point::new(he.x, -he.y, he.z * sign),
+            Point::new(-he.x, -he.y, he.z * sign),
+            Point::new(-he.x, he.y, he.z * sign),
+        ],
+        _ => unreachable!(),
+    };
+
+    pub fn vid(i: u8) -> u8 {
+        // Each vertex has an even feature id.
+        i * 2
+    }
+
+    let sign_index = ((sign as i8 + 1) / 2) as usize;
+    // The vertex id as numbered depending on the sign of the vertex
+    // component. A + sign means the corresponding bit is 0 while a -
+    // sign means the corresponding bit is 1.
+    // For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011
+    let vids = match iamax {
+        0 => [
+            [vid(0b000), vid(0b010), vid(0b011), vid(0b001)],
+            [vid(0b100), vid(0b110), vid(0b111), vid(0b101)],
+        ][sign_index],
+        1 => [
+            [vid(0b000), vid(0b100), vid(0b101), vid(0b001)],
+            [vid(0b010), vid(0b110), vid(0b111), vid(0b011)],
+        ][sign_index],
+        2 => [
+            [vid(0b000), vid(0b010), vid(0b110), vid(0b100)],
+            [vid(0b001), vid(0b011), vid(0b111), vid(0b101)],
+        ][sign_index],
+        _ => unreachable!(),
+    };
+
+    // The feature ids of edges is obtained from the vertex ids
+    // of their endpoints.
+    // Assuming vid1 > vid2, we do:   (vid1 << 3) | vid2 | 0b11000000
+    //
+    let eids = match iamax {
+        0 => [
+            [0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000],
+            [0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100],
+        ][sign_index],
+        1 => [
+            [0b11_100_000, 0b11_101_100, 0b11_101_001, 0b11_001_000],
+            [0b11_110_010, 0b11_111_110, 0b11_111_011, 0b11_011_010],
+        ][sign_index],
+        2 => [
+            [0b11_010_000, 0b11_110_010, 0b11_110_100, 0b11_100_000],
+            [0b11_011_001, 0b11_111_011, 0b11_111_101, 0b11_101_001],
+        ][sign_index],
+        _ => unreachable!(),
+    };
+
+    // The face with normals [x, y, z] are numbered [10, 11, 12].
+    // The face with negated normals are numbered [13, 14, 15].
+    let fid = iamax + sign_index * 3 + 10;
+
+    CuboidFeatureFace {
+        vertices,
+        vids,
+        eids,
+        fid: fid as u8,
+    }
+}

src/geometry/cuboid_feature2d.rs

@@ -0,0 +1,128 @@
+use crate::geometry::{self, Contact, ContactManifold};
+use crate::math::{Isometry, Point, Vector};
+use ncollide::shape::Segment;
+
+#[derive(Debug)]
+#[allow(dead_code)]
+pub enum CuboidFeature {
+    Face(CuboidFeatureFace),
+    Vertex(CuboidFeatureVertex),
+}
+
+#[derive(Debug)]
+pub struct CuboidFeatureVertex {
+    pub vertex: Point<f32>,
+    pub vid: u8,
+}
+
+impl CuboidFeatureVertex {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        self.vertex = iso * self.vertex;
+    }
+}
+
+#[derive(Debug)]
+pub struct CuboidFeatureFace {
+    pub vertices: [Point<f32>; 2],
+    pub vids: [u8; 2],
+    pub fid: u8,
+}
+
+impl From<Segment<f32>> for CuboidFeatureFace {
+    fn from(seg: Segment<f32>) -> Self {
+        CuboidFeatureFace {
+            vertices: [seg.a, seg.b],
+            vids: [0, 2],
+            fid: 1,
+        }
+    }
+}
+
+impl CuboidFeatureFace {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        self.vertices[0] = iso * self.vertices[0];
+        self.vertices[1] = iso * self.vertices[1];
+    }
+}
+
+impl CuboidFeature {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        match self {
+            CuboidFeature::Face(face) => face.transform_by(iso),
+            CuboidFeature::Vertex(vertex) => vertex.transform_by(iso),
+        }
+    }
+
+    /// Compute contacts points between a face and a vertex.
+    ///
+    /// This method assume we already know that at least one contact exists.
+    pub fn face_vertex_contacts(
+        face1: &CuboidFeatureFace,
+        sep_axis1: &Vector<f32>,
+        vertex2: &CuboidFeatureVertex,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        let tangent1 = face1.vertices[1] - face1.vertices[0];
+        let normal1 = Vector::new(-tangent1.y, tangent1.x);
+        let denom = -normal1.dot(&sep_axis1);
+        let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom;
+        let local_p2 = vertex2.vertex;
+        let local_p1 = vertex2.vertex - dist * sep_axis1;
+
+        manifold.points.push(Contact {
+            local_p1,
+            local_p2: pos21 * local_p2,
+            impulse: 0.0,
+            tangent_impulse: Contact::zero_tangent_impulse(),
+            fid1: face1.fid,
+            fid2: vertex2.vid,
+            dist,
+        });
+    }
+
+    pub fn face_face_contacts(
+        _prediction_distance: f32,
+        face1: &CuboidFeatureFace,
+        normal1: &Vector<f32>,
+        face2: &CuboidFeatureFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        if let Some((clip_a, clip_b)) = geometry::clip_segments(
+            (face1.vertices[0], face1.vertices[1]),
+            (face2.vertices[0], face2.vertices[1]),
+        ) {
+            let fids1 = [face1.vids[0], face1.fid, face1.vids[1]];
+            let fids2 = [face2.vids[0], face2.fid, face2.vids[1]];
+
+            let dist = (clip_a.1 - clip_a.0).dot(normal1);
+            if true {
+                // dist < prediction_distance {
+                manifold.points.push(Contact {
+                    local_p1: clip_a.0,
+                    local_p2: pos21 * clip_a.1,
+                    impulse: 0.0,
+                    tangent_impulse: Contact::zero_tangent_impulse(),
+                    fid1: fids1[clip_a.2],
+                    fid2: fids2[clip_a.3],
+                    dist,
+                });
+            }
+
+            let dist = (clip_b.1 - clip_b.0).dot(normal1);
+            if true {
+                // dist < prediction_distance {
+                manifold.points.push(Contact {
+                    local_p1: clip_b.0,
+                    local_p2: pos21 * clip_b.1,
+                    impulse: 0.0,
+                    tangent_impulse: Contact::zero_tangent_impulse(),
+                    fid1: fids1[clip_b.2],
+                    fid2: fids2[clip_b.3],
+                    dist,
+                });
+            }
+        }
+    }
+}

src/geometry/cuboid_feature3d.rs

@@ -0,0 +1,516 @@
+use crate::geometry::{Contact, ContactManifold};
+use crate::math::{Isometry, Point, Vector};
+use crate::utils::WBasis;
+use na::Point2;
+
+#[derive(Debug)]
+#[allow(dead_code)]
+pub(crate) enum CuboidFeature {
+    Face(CuboidFeatureFace),
+    Edge(CuboidFeatureEdge),
+    Vertex(CuboidFeatureVertex),
+}
+
+#[derive(Debug)]
+pub(crate) struct CuboidFeatureVertex {
+    pub vertex: Point<f32>,
+    pub vid: u8,
+}
+
+impl CuboidFeatureVertex {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        self.vertex = iso * self.vertex;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct CuboidFeatureEdge {
+    pub vertices: [Point<f32>; 2],
+    pub vids: [u8; 2],
+    pub eid: u8,
+}
+
+impl CuboidFeatureEdge {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        self.vertices[0] = iso * self.vertices[0];
+        self.vertices[1] = iso * self.vertices[1];
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct CuboidFeatureFace {
+    pub vertices: [Point<f32>; 4],
+    pub vids: [u8; 4], // Feature ID of the vertices.
+    pub eids: [u8; 4], // Feature ID of the edges.
+    pub fid: u8,       // Feature ID of the face.
+}
+
+impl CuboidFeatureFace {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        self.vertices[0] = iso * self.vertices[0];
+        self.vertices[1] = iso * self.vertices[1];
+        self.vertices[2] = iso * self.vertices[2];
+        self.vertices[3] = iso * self.vertices[3];
+    }
+}
+
+impl CuboidFeature {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        match self {
+            CuboidFeature::Face(face) => face.transform_by(iso),
+            CuboidFeature::Edge(edge) => edge.transform_by(iso),
+            CuboidFeature::Vertex(vertex) => vertex.transform_by(iso),
+        }
+    }
+
+    /// Compute contacts points between a face and a vertex.
+    ///
+    /// This method assume we already know that at least one contact exists.
+    pub fn face_vertex_contacts(
+        face1: &CuboidFeatureFace,
+        sep_axis1: &Vector<f32>,
+        vertex2: &CuboidFeatureVertex,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        let normal1 =
+            (face1.vertices[0] - face1.vertices[1]).cross(&(face1.vertices[2] - face1.vertices[1]));
+        let denom = -normal1.dot(&sep_axis1);
+        let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom;
+        let local_p2 = vertex2.vertex;
+        let local_p1 = vertex2.vertex - dist * sep_axis1;
+
+        manifold.points.push(Contact {
+            local_p1,
+            local_p2: pos21 * local_p2,
+            impulse: 0.0,
+            tangent_impulse: Contact::zero_tangent_impulse(),
+            fid1: face1.fid,
+            fid2: vertex2.vid,
+            dist,
+        });
+    }
+
+    /// Compute contacts points between a face and an edge.
+    ///
+    /// This method assume we already know that at least one contact exists.
+    pub fn face_edge_contacts(
+        prediction_distance: f32,
+        face1: &CuboidFeatureFace,
+        sep_axis1: &Vector<f32>,
+        edge2: &CuboidFeatureEdge,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+        flipped: bool,
+    ) {
+        // Project the faces to a 2D plane for contact clipping.
+        // The plane they are projected onto has normal sep_axis1
+        // and contains the origin (this is numerically OK because
+        // we are not working in world-space here).
+        let basis = sep_axis1.orthonormal_basis();
+        let projected_face1 = [
+            Point2::new(
+                face1.vertices[0].coords.dot(&basis[0]),
+                face1.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[1].coords.dot(&basis[0]),
+                face1.vertices[1].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[2].coords.dot(&basis[0]),
+                face1.vertices[2].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[3].coords.dot(&basis[0]),
+                face1.vertices[3].coords.dot(&basis[1]),
+            ),
+        ];
+        let projected_edge2 = [
+            Point2::new(
+                edge2.vertices[0].coords.dot(&basis[0]),
+                edge2.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                edge2.vertices[1].coords.dot(&basis[0]),
+                edge2.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+
+        // Now we have to compute the intersection between all pairs of
+        // edges from the face 1 with the edge 2
+        for i in 0..4 {
+            let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]];
+
+            if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
+                if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
+                    // Found a contact between the two edges.
+                    let edge1 = [face1.vertices[i], face1.vertices[(i + 1) % 4]];
+                    let local_p1 = edge1[0] * (1.0 - bcoords.0) + edge1[1].coords * bcoords.0;
+                    let local_p2 = edge2.vertices[0] * (1.0 - bcoords.1)
+                        + edge2.vertices[1].coords * bcoords.1;
+                    let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+                    if dist < prediction_distance {
+                        if flipped {
+                            manifold.points.push(Contact {
+                                local_p1: local_p2,
+                                // All points are expressed in the locale space of the first shape
+                                // (even if there was a flip). So the point we need to transform by
+                                // pos21 is the one that will go into local_p2.
+                                local_p2: pos21 * local_p1,
+                                impulse: 0.0,
+                                tangent_impulse: Contact::zero_tangent_impulse(),
+                                fid1: edge2.eid,
+                                fid2: face1.eids[i],
+                                dist,
+                            });
+                        } else {
+                            manifold.points.push(Contact {
+                                local_p1,
+                                local_p2: pos21 * local_p2,
+                                impulse: 0.0,
+                                tangent_impulse: Contact::zero_tangent_impulse(),
+                                fid1: face1.eids[i],
+                                fid2: edge2.eid,
+                                dist,
+                            });
+                        }
+                    }
+                }
+            }
+        }
+
+        // Project the two points from the edge into the face.
+        let normal1 =
+            (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1]));
+
+        'point_loop2: for i in 0..2 {
+            let p2 = projected_edge2[i];
+
+            let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3]));
+            for j in 0..3 {
+                let new_sign =
+                    (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j]));
+                if new_sign * sign < 0.0 {
+                    // The point lies outside.
+                    continue 'point_loop2;
+                }
+            }
+
+            // All the perp had the same sign: the point is inside of the other shapes projection.
+            // Output the contact.
+            let denom = -normal1.dot(&sep_axis1);
+            let dist = (face1.vertices[0] - edge2.vertices[i]).dot(&normal1) / denom;
+            let local_p2 = edge2.vertices[i];
+            let local_p1 = edge2.vertices[i] - dist * sep_axis1;
+
+            if dist < prediction_distance {
+                if flipped {
+                    manifold.points.push(Contact {
+                        local_p1: local_p2,
+                        // All points are expressed in the locale space of the first shape
+                        // (even if there was a flip). So the point we need to transform by
+                        // pos21 is the one that will go into local_p2.
+                        local_p2: pos21 * local_p1,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: edge2.vids[i],
+                        fid2: face1.fid,
+                        dist,
+                    });
+                } else {
+                    manifold.points.push(Contact {
+                        local_p1,
+                        local_p2: pos21 * local_p2,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: face1.fid,
+                        fid2: edge2.vids[i],
+                        dist,
+                    });
+                }
+            }
+        }
+    }
+
+    /// Compute contacts points between two edges.
+    ///
+    /// This method assume we already know that at least one contact exists.
+    pub fn edge_edge_contacts(
+        edge1: &CuboidFeatureEdge,
+        sep_axis1: &Vector<f32>,
+        edge2: &CuboidFeatureEdge,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        let basis = sep_axis1.orthonormal_basis();
+        let projected_edge1 = [
+            Point2::new(
+                edge1.vertices[0].coords.dot(&basis[0]),
+                edge1.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                edge1.vertices[1].coords.dot(&basis[0]),
+                edge1.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+        let projected_edge2 = [
+            Point2::new(
+                edge2.vertices[0].coords.dot(&basis[0]),
+                edge2.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                edge2.vertices[1].coords.dot(&basis[0]),
+                edge2.vertices[1].coords.dot(&basis[1]),
+            ),
+        ];
+
+        if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
+            let local_p1 =
+                edge1.vertices[0] * (1.0 - bcoords.0) + edge1.vertices[1].coords * bcoords.0;
+            let local_p2 =
+                edge2.vertices[0] * (1.0 - bcoords.1) + edge2.vertices[1].coords * bcoords.1;
+            let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+            manifold.points.push(Contact {
+                local_p1,
+                local_p2: pos21 * local_p2, // NOTE: local_p2 is expressed in the local space of cube1.
+                impulse: 0.0,
+                tangent_impulse: Contact::zero_tangent_impulse(),
+                fid1: edge1.eid,
+                fid2: edge2.eid,
+                dist,
+            });
+        }
+    }
+
+    pub fn face_face_contacts(
+        _prediction_distance: f32,
+        face1: &CuboidFeatureFace,
+        sep_axis1: &Vector<f32>,
+        face2: &CuboidFeatureFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        // Project the faces to a 2D plane for contact clipping.
+        // The plane they are projected onto has normal sep_axis1
+        // and contains the origin (this is numerically OK because
+        // we are not working in world-space here).
+        let basis = sep_axis1.orthonormal_basis();
+        let projected_face1 = [
+            Point2::new(
+                face1.vertices[0].coords.dot(&basis[0]),
+                face1.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[1].coords.dot(&basis[0]),
+                face1.vertices[1].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[2].coords.dot(&basis[0]),
+                face1.vertices[2].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[3].coords.dot(&basis[0]),
+                face1.vertices[3].coords.dot(&basis[1]),
+            ),
+        ];
+        let projected_face2 = [
+            Point2::new(
+                face2.vertices[0].coords.dot(&basis[0]),
+                face2.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[1].coords.dot(&basis[0]),
+                face2.vertices[1].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[2].coords.dot(&basis[0]),
+                face2.vertices[2].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[3].coords.dot(&basis[0]),
+                face2.vertices[3].coords.dot(&basis[1]),
+            ),
+        ];
+
+        // Also find all the vertices located inside of the other projected face.
+        let normal1 =
+            (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1]));
+        let normal2 =
+            (face2.vertices[2] - face2.vertices[1]).cross(&(face2.vertices[0] - face2.vertices[1]));
+
+        // NOTE: The loop iterating on all the vertices for face1 is different from
+        // the one iterating on all the vertices of face2. In the second loop, we
+        // classify every point wrt. every edge on the other face. This will give
+        // us bit masks to filter out several edge-edge intersections.
+        'point_loop1: for i in 0..4 {
+            let p1 = projected_face1[i];
+
+            let sign = (projected_face2[0] - projected_face2[3]).perp(&(p1 - projected_face2[3]));
+            for j in 0..3 {
+                let new_sign =
+                    (projected_face2[j + 1] - projected_face2[j]).perp(&(p1 - projected_face2[j]));
+                if new_sign * sign < 0.0 {
+                    // The point lies outside.
+                    continue 'point_loop1;
+                }
+            }
+
+            // All the perp had the same sign: the point is inside of the other shapes projection.
+            // Output the contact.
+            let denom = normal2.dot(&sep_axis1);
+            let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
+            let local_p1 = face1.vertices[i];
+            let local_p2 = face1.vertices[i] + dist * sep_axis1;
+
+            if true {
+                // dist <= prediction_distance {
+                manifold.points.push(Contact {
+                    local_p1,
+                    local_p2: pos21 * local_p2,
+                    impulse: 0.0,
+                    tangent_impulse: Contact::zero_tangent_impulse(),
+                    fid1: face1.vids[i],
+                    fid2: face2.fid,
+                    dist,
+                });
+            }
+        }
+
+        let is_clockwise1 = (projected_face1[0] - projected_face1[1])
+            .perp(&(projected_face1[2] - projected_face1[1]))
+            >= 0.0;
+        let mut vertex_class2 = [0u8; 4];
+
+        for i in 0..4 {
+            let p2 = projected_face2[i];
+
+            let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3]));
+            vertex_class2[i] |= ((sign >= 0.0) as u8) << 3;
+
+            for j in 0..3 {
+                let sign =
+                    (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j]));
+                vertex_class2[i] |= ((sign >= 0.0) as u8) << j;
+            }
+
+            if !is_clockwise1 {
+                vertex_class2[i] = (!vertex_class2[i]) & 0b01111;
+            }
+
+            if vertex_class2[i] == 0 {
+                // All the perp had the same sign: the point is inside of the other shapes projection.
+                // Output the contact.
+                let denom = -normal1.dot(&sep_axis1);
+                let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
+                let local_p2 = face2.vertices[i];
+                let local_p1 = face2.vertices[i] - dist * sep_axis1;
+
+                if true {
+                    // dist < prediction_distance {
+                    manifold.points.push(Contact {
+                        local_p1,
+                        local_p2: pos21 * local_p2,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: face1.fid,
+                        fid2: face2.vids[i],
+                        dist,
+                    });
+                }
+            }
+        }
+
+        // Now we have to compute the intersection between all pairs of
+        // edges from the face 1 and from the face2.
+        for j in 0..4 {
+            let projected_edge2 = [projected_face2[j], projected_face2[(j + 1) % 4]];
+
+            if (vertex_class2[j] & vertex_class2[(j + 1) % 4]) != 0 {
+                continue;
+            }
+
+            let edge_class2 = vertex_class2[j] | vertex_class2[(j + 1) % 4];
+
+            for i in 0..4 {
+                if (edge_class2 & (1 << i)) != 0 {
+                    let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]];
+                    if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
+                        if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0
+                        {
+                            // Found a contact between the two edges.
+                            let edge1 = (face1.vertices[i], face1.vertices[(i + 1) % 4]);
+                            let edge2 = (face2.vertices[j], face2.vertices[(j + 1) % 4]);
+                            let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
+                            let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
+                            let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+                            if true {
+                                // dist <= prediction_distance {
+                                manifold.points.push(Contact {
+                                    local_p1,
+                                    local_p2: pos21 * local_p2,
+                                    impulse: 0.0,
+                                    tangent_impulse: Contact::zero_tangent_impulse(),
+                                    fid1: face1.eids[i],
+                                    fid2: face2.eids[j],
+                                    dist,
+                                });
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+/// Compute the barycentric coordinates of the intersection between the two given lines.
+/// Returns `None` if the lines are parallel.
+fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
+    use approx::AbsDiffEq;
+
+    // Inspired by Real-time collision detection by Christer Ericson.
+    let dir1 = edge1[1] - edge1[0];
+    let dir2 = edge2[1] - edge2[0];
+    let r = edge1[0] - edge2[0];
+
+    let a = dir1.norm_squared();
+    let e = dir2.norm_squared();
+    let f = dir2.dot(&r);
+
+    let eps = f32::default_epsilon();
+
+    if a <= eps && e <= eps {
+        Some((0.0, 0.0))
+    } else if a <= eps {
+        Some((0.0, f / e))
+    } else {
+        let c = dir1.dot(&r);
+        if e <= eps {
+            Some((-c / a, 0.0))
+        } else {
+            let b = dir1.dot(&dir2);
+            let ae = a * e;
+            let bb = b * b;
+            let denom = ae - bb;
+
+            // Use absolute and ulps error to test collinearity.
+            let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
+
+            let s = if !parallel {
+                (b * f - c * e) / denom
+            } else {
+                0.0
+            };
+
+            if parallel {
+                None
+            } else {
+                Some((s, (b * s + f) / e))
+            }
+        }
+    }
+}

src/geometry/interaction_graph.rs

@@ -0,0 +1,259 @@
+use crate::data::graph::{Direction, EdgeIndex, Graph, NodeIndex};
+use crate::geometry::ColliderHandle;
+
+/// Index of a node of the interaction graph.
+pub type ColliderGraphIndex = NodeIndex;
+/// Index of a node of the interaction graph.
+pub type RigidBodyGraphIndex = NodeIndex;
+/// Temporary index to and edge of the interaction graph.
+pub type TemporaryInteractionIndex = EdgeIndex;
+
+/// A graph where nodes are collision objects and edges are contact or proximity algorithms.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct InteractionGraph<T> {
+    pub(crate) graph: Graph<ColliderHandle, T>,
+}
+
+impl<T> InteractionGraph<T> {
+    /// Creates a new empty collection of collision objects.
+    pub fn new() -> Self {
+        InteractionGraph {
+            graph: Graph::with_capacity(10, 10),
+        }
+    }
+
+    /// The underlying raw graph structure of this interaction graph.
+    pub fn raw_graph(&self) -> &Graph<ColliderHandle, T> {
+        &self.graph
+    }
+
+    pub(crate) fn invalid_graph_index() -> ColliderGraphIndex {
+        ColliderGraphIndex::new(crate::INVALID_U32)
+    }
+
+    pub(crate) fn is_graph_index_valid(index: ColliderGraphIndex) -> bool {
+        index.index() != crate::INVALID_USIZE
+    }
+
+    pub(crate) fn add_edge(
+        &mut self,
+        index1: ColliderGraphIndex,
+        index2: ColliderGraphIndex,
+        interaction: T,
+    ) -> TemporaryInteractionIndex {
+        self.graph.add_edge(index1, index2, interaction)
+    }
+
+    pub(crate) fn remove_edge(
+        &mut self,
+        index1: ColliderGraphIndex,
+        index2: ColliderGraphIndex,
+    ) -> Option<T> {
+        let id = self.graph.find_edge(index1, index2)?;
+        self.graph.remove_edge(id)
+    }
+
+    /// Removes a handle from this graph and returns a handle that must have its graph index changed to `id`.
+    ///
+    /// When a node is removed, another node of the graph takes it place. This means that the `ColliderGraphIndex`
+    /// of the collision object returned by this method will be equal to `id`. Thus if you maintain
+    /// a map between `CollisionObjectSlabHandle` and `ColliderGraphIndex`, then you should update this
+    /// map to associate `id` to the handle returned by this method. For example:
+    ///
+    /// ```.ignore
+    /// // Let `id` be the graph index of the collision object we want to remove.
+    /// if let Some(other_handle) = graph.remove_node(id) {
+    ///    // The graph index of `other_handle` changed to `id` due to the removal.
+    ///    map.insert(other_handle, id) ;
+    /// }
+    /// ```
+    #[must_use = "The graph index of the collision object returned by this method has been changed to `id`."]
+    pub(crate) fn remove_node(&mut self, id: ColliderGraphIndex) -> Option<ColliderHandle> {
+        let _ = self.graph.remove_node(id);
+        self.graph.node_weight(id).cloned()
+    }
+
+    /// All the interactions pairs on this graph.
+    pub fn interaction_pairs(&self) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
+        self.graph.raw_edges().iter().map(move |edge| {
+            (
+                self.graph[edge.source()],
+                self.graph[edge.target()],
+                &edge.weight,
+            )
+        })
+    }
+
+    /// The interaction between the two collision objects identified by their graph index.
+    pub fn interaction_pair(
+        &self,
+        id1: ColliderGraphIndex,
+        id2: ColliderGraphIndex,
+    ) -> Option<(ColliderHandle, ColliderHandle, &T)> {
+        self.graph.find_edge(id1, id2).and_then(|edge| {
+            let endpoints = self.graph.edge_endpoints(edge)?;
+            let h1 = self.graph.node_weight(endpoints.0)?;
+            let h2 = self.graph.node_weight(endpoints.1)?;
+            let weight = self.graph.edge_weight(edge)?;
+            Some((*h1, *h2, weight))
+        })
+    }
+
+    /// The interaction between the two collision objects identified by their graph index.
+    pub fn interaction_pair_mut(
+        &mut self,
+        id1: ColliderGraphIndex,
+        id2: ColliderGraphIndex,
+    ) -> Option<(ColliderHandle, ColliderHandle, &mut T)> {
+        let edge = self.graph.find_edge(id1, id2)?;
+        let endpoints = self.graph.edge_endpoints(edge)?;
+        let h1 = *self.graph.node_weight(endpoints.0)?;
+        let h2 = *self.graph.node_weight(endpoints.1)?;
+        let weight = self.graph.edge_weight_mut(edge)?;
+        Some((h1, h2, weight))
+    }
+
+    /// All the interaction involving the collision object with graph index `id`.
+    pub fn interactions_with(
+        &self,
+        id: ColliderGraphIndex,
+    ) -> impl Iterator<Item = (ColliderHandle, ColliderHandle, &T)> {
+        self.graph.edges(id).filter_map(move |e| {
+            let endpoints = self.graph.edge_endpoints(e.id()).unwrap();
+            Some((self.graph[endpoints.0], self.graph[endpoints.1], e.weight()))
+        })
+    }
+
+    /// Gets the interaction with the given index.
+    pub fn index_interaction(
+        &self,
+        id: TemporaryInteractionIndex,
+    ) -> Option<(ColliderHandle, ColliderHandle, &T)> {
+        if let (Some(e), Some(endpoints)) =
+            (self.graph.edge_weight(id), self.graph.edge_endpoints(id))
+        {
+            Some((self.graph[endpoints.0], self.graph[endpoints.1], e))
+        } else {
+            None
+        }
+    }
+
+    /// All the mutable references to interactions involving the collision object with graph index `id`.
+    pub fn interactions_with_mut(
+        &mut self,
+        id: ColliderGraphIndex,
+    ) -> impl Iterator<
+        Item = (
+            ColliderHandle,
+            ColliderHandle,
+            TemporaryInteractionIndex,
+            &mut T,
+        ),
+    > {
+        let incoming_edge = self.graph.first_edge(id, Direction::Incoming);
+        let outgoing_edge = self.graph.first_edge(id, Direction::Outgoing);
+
+        InteractionsWithMut {
+            graph: &mut self.graph,
+            incoming_edge,
+            outgoing_edge,
+        }
+    }
+
+    // /// All the collision object handles of collision objects interacting with the collision object with graph index `id`.
+    // pub fn colliders_interacting_with<'a>(
+    //     &'a self,
+    //     id: ColliderGraphIndex,
+    // ) -> impl Iterator<Item = ColliderHandle> + 'a {
+    //     self.graph.edges(id).filter_map(move |e| {
+    //         let inter = e.weight();
+    //
+    //         if e.source() == id {
+    //             Some(self.graph[e.target()])
+    //         } else {
+    //             Some(self.graph[e.source()])
+    //         }
+    //     })
+    // }
+
+    // /// All the collision object handles of collision objects in contact with the collision object with graph index `id`.
+    // pub fn colliders_in_contact_with<'a>(
+    //     &'a self,
+    //     id: ColliderGraphIndex,
+    // ) -> impl Iterator<Item = ColliderHandle> + 'a {
+    //     self.graph.edges(id).filter_map(move |e| {
+    //         let inter = e.weight();
+    //
+    //         if inter.is_contact() && Self::is_interaction_effective(inter) {
+    //             if e.source() == id {
+    //                 Some(self.graph[e.target()])
+    //             } else {
+    //                 Some(self.graph[e.source()])
+    //             }
+    //         } else {
+    //             None
+    //         }
+    //     })
+    // }
+    //
+    // /// All the collision object handles of collision objects in proximity of with the collision object with graph index `id`.
+    // /// for details.
+    // pub fn colliders_in_proximity_of<'a>(
+    //     &'a self,
+    //     id: ColliderGraphIndex,
+    // ) -> impl Iterator<Item = ColliderHandle> + 'a {
+    //     self.graph.edges(id).filter_map(move |e| {
+    //         if let Interaction::Proximity(_, prox) = e.weight() {
+    //             if *prox == Proximity::Intersecting {
+    //                 if e.source() == id {
+    //                     return Some(self.graph[e.target()]);
+    //                 } else {
+    //                     return Some(self.graph[e.source()]);
+    //                 }
+    //             }
+    //         }
+    //
+    //         None
+    //     })
+    // }
+}
+
+pub struct InteractionsWithMut<'a, T> {
+    graph: &'a mut Graph<ColliderHandle, T>,
+    incoming_edge: Option<EdgeIndex>,
+    outgoing_edge: Option<EdgeIndex>,
+}
+
+impl<'a, T> Iterator for InteractionsWithMut<'a, T> {
+    type Item = (
+        ColliderHandle,
+        ColliderHandle,
+        TemporaryInteractionIndex,
+        &'a mut T,
+    );
+
+    #[inline]
+    fn next(
+        &mut self,
+    ) -> Option<(
+        ColliderHandle,
+        ColliderHandle,
+        TemporaryInteractionIndex,
+        &'a mut T,
+    )> {
+        if let Some(edge) = self.incoming_edge {
+            self.incoming_edge = self.graph.next_edge(edge, Direction::Incoming);
+            let endpoints = self.graph.edge_endpoints(edge).unwrap();
+            let (co1, co2) = (self.graph[endpoints.0], self.graph[endpoints.1]);
+            let interaction = &mut self.graph[edge];
+            return Some((co1, co2, edge, unsafe { std::mem::transmute(interaction) }));
+        }
+
+        let edge = self.outgoing_edge?;
+        self.outgoing_edge = self.graph.next_edge(edge, Direction::Outgoing);
+        let endpoints = self.graph.edge_endpoints(edge).unwrap();
+        let (co1, co2) = (self.graph[endpoints.0], self.graph[endpoints.1]);
+        let interaction = &mut self.graph[edge];
+        Some((co1, co2, edge, unsafe { std::mem::transmute(interaction) }))
+    }
+}

src/geometry/mod.rs

@@ -0,0 +1,80 @@
+//! Structures related to geometry: colliders, shapes, etc.
+
+pub use self::broad_phase_multi_sap::BroadPhase;
+pub use self::capsule::Capsule;
+pub use self::collider::{Collider, ColliderBuilder, Shape};
+pub use self::collider_set::{ColliderHandle, ColliderSet};
+pub use self::contact::{
+    Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory,
+};
+pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
+#[cfg(feature = "dim2")]
+pub(crate) use self::cuboid_feature2d::{CuboidFeature, CuboidFeatureFace};
+#[cfg(feature = "dim3")]
+pub(crate) use self::cuboid_feature3d::{CuboidFeature, CuboidFeatureFace};
+pub use self::interaction_graph::{
+    ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
+};
+pub use self::narrow_phase::NarrowPhase;
+pub use self::polygon::Polygon;
+pub use self::proximity::ProximityPair;
+pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher};
+pub use self::trimesh::Trimesh;
+pub use ncollide::query::Proximity;
+
+/// A cuboid shape.
+pub type Cuboid = ncollide::shape::Cuboid<f32>;
+/// A triangle shape.
+pub type Triangle = ncollide::shape::Triangle<f32>;
+/// A ball shape.
+pub type Ball = ncollide::shape::Ball<f32>;
+/// A heightfield shape.
+pub type HeightField = ncollide::shape::HeightField<f32>;
+/// An axis-aligned bounding box.
+pub type AABB = ncollide::bounding_volume::AABB<f32>;
+/// Event triggered when two non-sensor colliders start or stop being in contact.
+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>;
+
+#[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;
+#[cfg(feature = "simd-is-enabled")]
+pub(crate) use self::contact::WContact;
+#[cfg(feature = "dim2")]
+pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal};
+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::z_order::z_cmp_floats;
+
+mod ball;
+mod broad_phase;
+mod broad_phase_multi_sap;
+mod capsule;
+mod collider;
+mod collider_set;
+mod contact;
+mod contact_generator;
+pub(crate) mod cuboid;
+#[cfg(feature = "dim2")]
+mod cuboid_feature2d;
+#[cfg(feature = "dim3")]
+mod cuboid_feature3d;
+mod interaction_graph;
+mod narrow_phase;
+mod polygon;
+#[cfg(feature = "dim3")]
+mod polyhedron_feature3d;
+mod proximity;
+mod proximity_detector;
+pub(crate) mod sat;
+pub(crate) mod triangle;
+mod trimesh;
+#[cfg(feature = "simd-is-enabled")]
+mod waabb;
+//mod z_order;

src/geometry/narrow_phase.rs

@@ -0,0 +1,483 @@
+#[cfg(feature = "parallel")]
+use rayon::prelude::*;
+
+use crate::dynamics::RigidBodySet;
+use crate::geometry::contact_generator::{
+    ContactDispatcher, ContactGenerationContext, DefaultContactDispatcher,
+};
+use crate::geometry::proximity_detector::{
+    DefaultProximityDispatcher, ProximityDetectionContext, ProximityDispatcher,
+};
+//#[cfg(feature = "simd-is-enabled")]
+//use crate::geometry::{
+//    contact_generator::ContactGenerationContextSimd,
+//    proximity_detector::ProximityDetectionContextSimd, WBall,
+//};
+use crate::geometry::{
+    BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair,
+};
+use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
+//#[cfg(feature = "simd-is-enabled")]
+//use crate::math::{SimdFloat, SIMD_WIDTH};
+use crate::ncollide::query::Proximity;
+use crate::pipeline::EventHandler;
+//use simba::simd::SimdValue;
+
+/// The narrow-phase responsible for computing precise contact information between colliders.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct NarrowPhase {
+    contact_graph: InteractionGraph<ContactPair>,
+    proximity_graph: InteractionGraph<ProximityPair>,
+    //    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>,
+    //    shape_shape_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>,
+}
+
+pub(crate) type ContactManifoldIndex = usize;
+
+impl NarrowPhase {
+    /// Creates a new empty narrow-phase.
+    pub fn new() -> Self {
+        Self {
+            contact_graph: InteractionGraph::new(),
+            proximity_graph: InteractionGraph::new(),
+            //            ball_ball: Vec::new(),
+            //            shape_shape: Vec::new(),
+            //            ball_ball_prox: Vec::new(),
+            //            shape_shape_prox: Vec::new(),
+        }
+    }
+
+    /// The contact graph containing all contact pairs and their contact information.
+    pub fn contact_graph(&self) -> &InteractionGraph<ContactPair> {
+        &self.contact_graph
+    }
+
+    /// The proximity graph containing all proximity pairs and their proximity information.
+    pub fn proximity_graph(&self) -> &InteractionGraph<ProximityPair> {
+        &self.proximity_graph
+    }
+
+    // #[cfg(feature = "parallel")]
+    // pub fn contact_pairs(&self) -> &[ContactPair] {
+    //     &self.contact_graph.interactions
+    // }
+
+    // pub fn contact_pairs_mut(&mut self) -> &mut [ContactPair] {
+    //     &mut self.contact_graph.interactions
+    // }
+
+    // #[cfg(feature = "parallel")]
+    // pub(crate) fn contact_pairs_vec_mut(&mut self) -> &mut Vec<ContactPair> {
+    //     &mut self.contact_graph.interactions
+    // }
+
+    pub(crate) fn remove_colliders(
+        &mut self,
+        handles: &[ColliderHandle],
+        colliders: &mut ColliderSet,
+        bodies: &mut RigidBodySet,
+    ) {
+        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(parent) = colliders.get(a).map(|c| c.parent) {
+                        bodies.wake_up(parent)
+                    }
+
+                    if let Some(parent) = colliders.get(b).map(|c| c.parent) {
+                        bodies.wake_up(parent)
+                    }
+                }
+
+                // We have to manage the fact that one other collider will
+                // hive its graph index changed because of the node's swap-remove.
+                if let Some(replacement) = self
+                    .proximity_graph
+                    .remove_node(proximity_graph_id)
+                    .and_then(|h| colliders.get_mut(h))
+                {
+                    replacement.proximity_graph_index = proximity_graph_id;
+                }
+
+                if let Some(replacement) = self
+                    .contact_graph
+                    .remove_node(contact_graph_id)
+                    .and_then(|h| colliders.get_mut(h))
+                {
+                    replacement.contact_graph_index = contact_graph_id;
+                }
+            }
+        }
+    }
+
+    pub(crate) fn register_pairs(
+        &mut self,
+        colliders: &mut ColliderSet,
+        broad_phase_events: &[BroadPhasePairEvent],
+        events: &dyn EventHandler,
+    ) {
+        for event in broad_phase_events {
+            match event {
+                BroadPhasePairEvent::AddPair(pair) => {
+                    // println!("Adding pair: {:?}", *pair);
+                    if let (Some(co1), Some(co2)) =
+                        colliders.get2_mut_internal(pair.collider1, pair.collider2)
+                    {
+                        if co1.is_sensor() || co2.is_sensor() {
+                            let gid1 = co1.proximity_graph_index;
+                            let gid2 = co2.proximity_graph_index;
+
+                            // NOTE: the collider won't have a graph index as long
+                            // as it does not interact with anything.
+                            if !InteractionGraph::<ProximityPair>::is_graph_index_valid(gid1) {
+                                co1.proximity_graph_index =
+                                    self.proximity_graph.graph.add_node(pair.collider1);
+                            }
+
+                            if !InteractionGraph::<ProximityPair>::is_graph_index_valid(gid2) {
+                                co2.proximity_graph_index =
+                                    self.proximity_graph.graph.add_node(pair.collider2);
+                            }
+
+                            if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() {
+                                let dispatcher = DefaultProximityDispatcher;
+                                let generator = dispatcher.dispatch(co1.shape(), co2.shape());
+                                let interaction =
+                                    ProximityPair::new(*pair, generator.0, generator.1);
+                                let _ = self.proximity_graph.add_edge(
+                                    co1.proximity_graph_index,
+                                    co2.proximity_graph_index,
+                                    interaction,
+                                );
+                            }
+                        } else {
+                            // NOTE: same code as above, but for the contact graph.
+                            // TODO: refactor both pieces of code somehow?
+                            let gid1 = co1.contact_graph_index;
+                            let gid2 = co2.contact_graph_index;
+
+                            // NOTE: the collider won't have a graph index as long
+                            // as it does not interact with anything.
+                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(gid1) {
+                                co1.contact_graph_index =
+                                    self.contact_graph.graph.add_node(pair.collider1);
+                            }
+
+                            if !InteractionGraph::<ContactPair>::is_graph_index_valid(gid2) {
+                                co2.contact_graph_index =
+                                    self.contact_graph.graph.add_node(pair.collider2);
+                            }
+
+                            if self.contact_graph.graph.find_edge(gid1, gid2).is_none() {
+                                let dispatcher = DefaultContactDispatcher;
+                                let generator = dispatcher.dispatch(co1.shape(), co2.shape());
+                                let interaction = ContactPair::new(*pair, generator.0, generator.1);
+                                let _ = self.contact_graph.add_edge(
+                                    co1.contact_graph_index,
+                                    co2.contact_graph_index,
+                                    interaction,
+                                );
+                            }
+                        }
+                    }
+                }
+                BroadPhasePairEvent::DeletePair(pair) => {
+                    if let (Some(co1), Some(co2)) =
+                        colliders.get2_mut_internal(pair.collider1, pair.collider2)
+                    {
+                        if co1.is_sensor() || co2.is_sensor() {
+                            let prox_pair = self
+                                .proximity_graph
+                                .remove_edge(co1.proximity_graph_index, co2.proximity_graph_index);
+
+                            // Emit a proximity lost event if we had a proximity before removing the edge.
+                            if let Some(prox) = prox_pair {
+                                if prox.proximity != Proximity::Disjoint {
+                                    let prox_event = ProximityEvent::new(
+                                        pair.collider1,
+                                        pair.collider2,
+                                        prox.proximity,
+                                        Proximity::Disjoint,
+                                    );
+                                    events.handle_proximity_event(prox_event)
+                                }
+                            }
+                        } else {
+                            let contact_pair = self
+                                .contact_graph
+                                .remove_edge(co1.contact_graph_index, co2.contact_graph_index);
+
+                            // Emit a contact stopped event if we had a proximity before removing the edge.
+                            if let Some(ctct) = contact_pair {
+                                if ctct.has_any_active_contact() {
+                                    events.handle_contact_event(ContactEvent::Stopped(
+                                        pair.collider1,
+                                        pair.collider2,
+                                    ))
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    pub(crate) fn compute_proximities(
+        &mut self,
+        prediction_distance: f32,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        events: &dyn EventHandler,
+    ) {
+        par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| {
+            let pair = &mut edge.weight;
+            let co1 = &colliders[pair.pair.collider1];
+            let co2 = &colliders[pair.pair.collider2];
+
+            // FIXME: avoid lookup into bodies.
+            let rb1 = &bodies[co1.parent];
+            let rb2 = &bodies[co2.parent];
+
+            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
+            {
+                // No need to update this contact because nothing moved.
+                return;
+            }
+
+            let dispatcher = DefaultProximityDispatcher;
+            if pair.detector.is_none() {
+                // We need a redispatch for this detector.
+                // This can happen, e.g., after restoring a snapshot of the narrow-phase.
+                let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
+                pair.detector = Some(detector);
+                pair.detector_workspace = workspace;
+            }
+
+            let context = ProximityDetectionContext {
+                dispatcher: &dispatcher,
+                prediction_distance,
+                colliders,
+                pair,
+            };
+
+            context
+                .pair
+                .detector
+                .unwrap()
+                .detect_proximity(context, events);
+        });
+
+        /*
+        // First, group pairs.
+        // NOTE: the transmutes here are OK because the Vec are all cleared
+        // before we leave this method.
+        // We do this in order to avoid reallocating those vecs each time
+        // we compute the contacts. Unsafe is necessary because we can't just
+        // store a Vec<&mut ProximityPair> into the NarrowPhase struct without
+        // polluting the World with lifetimes.
+        let ball_ball_prox: &mut Vec<&mut ProximityPair> =
+            unsafe { std::mem::transmute(&mut self.ball_ball_prox) };
+        let shape_shape_prox: &mut Vec<&mut ProximityPair> =
+            unsafe { std::mem::transmute(&mut self.shape_shape_prox) };
+
+        let bodies = &bodies.bodies;
+
+        // FIXME: don't iterate through all the interactions.
+        for pair in &mut self.proximity_graph.interactions {
+            let co1 = &colliders[pair.pair.collider1];
+            let co2 = &colliders[pair.pair.collider2];
+
+            // FIXME: avoid lookup into bodies.
+            let rb1 = &bodies[co1.parent];
+            let rb2 = &bodies[co2.parent];
+
+            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
+            {
+                // No need to update this proximity because nothing moved.
+                continue;
+            }
+
+            match (co1.shape(), co2.shape()) {
+                (Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair),
+                _ => shape_shape_prox.push(pair),
+            }
+        }
+
+        par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| {
+            let context = ProximityDetectionContextSimd {
+                dispatcher: &DefaultProximityDispatcher,
+                prediction_distance,
+                colliders,
+                pairs,
+            };
+            context.pairs[0]
+                .detector
+                .detect_proximity_simd(context, events);
+        });
+
+        par_iter_mut!(shape_shape_prox).for_each(|pair| {
+            let context = ProximityDetectionContext {
+                dispatcher: &DefaultProximityDispatcher,
+                prediction_distance,
+                colliders,
+                pair,
+            };
+
+            context.pair.detector.detect_proximity(context, events);
+        });
+
+        ball_ball_prox.clear();
+        shape_shape_prox.clear();
+        */
+    }
+
+    pub(crate) fn compute_contacts(
+        &mut self,
+        prediction_distance: f32,
+        bodies: &RigidBodySet,
+        colliders: &ColliderSet,
+        events: &dyn EventHandler,
+    ) {
+        par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
+            let pair = &mut edge.weight;
+            let co1 = &colliders[pair.pair.collider1];
+            let co2 = &colliders[pair.pair.collider2];
+
+            // FIXME: avoid lookup into bodies.
+            let rb1 = &bodies[co1.parent];
+            let rb2 = &bodies[co2.parent];
+
+            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
+            {
+                // No need to update this contact because nothing moved.
+                return;
+            }
+
+            let dispatcher = DefaultContactDispatcher;
+            if pair.generator.is_none() {
+                // We need a redispatch for this generator.
+                // This can happen, e.g., after restoring a snapshot of the narrow-phase.
+                let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
+                pair.generator = Some(generator);
+                pair.generator_workspace = workspace;
+            }
+
+            let context = ContactGenerationContext {
+                dispatcher: &dispatcher,
+                prediction_distance,
+                colliders,
+                pair,
+            };
+
+            context
+                .pair
+                .generator
+                .unwrap()
+                .generate_contacts(context, events);
+        });
+
+        /*
+        // First, group pairs.
+        // NOTE: the transmutes here are OK because the Vec are all cleared
+        // before we leave this method.
+        // We do this in order to avoid reallocating those vecs each time
+        // we compute the contacts. Unsafe is necessary because we can't just
+        // store a Vec<&mut ContactPair> into the NarrowPhase struct without
+        // polluting the World with lifetimes.
+        let ball_ball: &mut Vec<&mut ContactPair> =
+            unsafe { std::mem::transmute(&mut self.ball_ball) };
+        let shape_shape: &mut Vec<&mut ContactPair> =
+            unsafe { std::mem::transmute(&mut self.shape_shape) };
+
+        let bodies = &bodies.bodies;
+
+        // FIXME: don't iterate through all the interactions.
+        for pair in &mut self.contact_graph.interactions {
+            let co1 = &colliders[pair.pair.collider1];
+            let co2 = &colliders[pair.pair.collider2];
+
+            // FIXME: avoid lookup into bodies.
+            let rb1 = &bodies[co1.parent];
+            let rb2 = &bodies[co2.parent];
+
+            if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
+            {
+                // No need to update this contact because nothing moved.
+                continue;
+            }
+
+            match (co1.shape(), co2.shape()) {
+                (Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair),
+                _ => shape_shape.push(pair),
+            }
+        }
+
+        par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| {
+            let context = ContactGenerationContextSimd {
+                dispatcher: &DefaultContactDispatcher,
+                prediction_distance,
+                colliders,
+                pairs,
+            };
+            context.pairs[0]
+                .generator
+                .generate_contacts_simd(context, events);
+        });
+
+        par_iter_mut!(shape_shape).for_each(|pair| {
+            let context = ContactGenerationContext {
+                dispatcher: &DefaultContactDispatcher,
+                prediction_distance,
+                colliders,
+                pair,
+            };
+
+            context.pair.generator.generate_contacts(context, events);
+        });
+
+        ball_ball.clear();
+        shape_shape.clear();
+        */
+    }
+
+    /// Retrieve all the interactions with at least one contact point, happening between two active bodies.
+    // NOTE: this is very similar to the code from JointSet::select_active_interactions.
+    pub(crate) fn sort_and_select_active_contacts<'a>(
+        &'a mut self,
+        bodies: &RigidBodySet,
+        out_manifolds: &mut Vec<&'a mut ContactManifold>,
+        out: &mut Vec<Vec<ContactManifoldIndex>>,
+    ) {
+        for out_island in &mut out[..bodies.num_islands()] {
+            out_island.clear();
+        }
+
+        // FIXME: don't iterate through all the interactions.
+        for inter in self.contact_graph.graph.edges.iter_mut() {
+            for manifold in &mut inter.weight.manifolds {
+                let rb1 = &bodies[manifold.body_pair.body1];
+                let rb2 = &bodies[manifold.body_pair.body2];
+                if manifold.num_active_contacts() != 0
+                    && (!rb1.is_dynamic() || !rb1.is_sleeping())
+                    && (!rb2.is_dynamic() || !rb2.is_sleeping())
+                {
+                    let island_index = if !rb1.is_dynamic() {
+                        rb2.active_island_id
+                    } else {
+                        rb1.active_island_id
+                    };
+
+                    out[island_index].push(out_manifolds.len());
+                    out_manifolds.push(manifold);
+                }
+            }
+        }
+    }
+}

src/geometry/polygon.rs

@@ -0,0 +1,76 @@
+use crate::math::{Isometry, Point, Vector};
+use ncollide::bounding_volume::AABB;
+
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A convex planar polygon.
+pub struct Polygon {
+    pub(crate) vertices: Vec<Point<f32>>,
+    pub(crate) normals: Vec<Vector<f32>>,
+}
+
+impl Polygon {
+    /// Builds a new polygon from a set of vertices and normals.
+    ///
+    /// The vertices must be ordered in such a way that two consecutive
+    /// vertices determines an edge of the polygon. For example `vertices[0], vertices[1]`
+    /// is an edge, `vertices[1], vertices[2]` is the next edge, etc. The last edge will
+    /// be `vertices[vertices.len() - 1], vertices[0]`.
+    /// The vertices must be given in counter-clockwise order.
+    /// The vertices must form a convex polygon.
+    ///
+    /// One normal must be provided per edge and mut point towards the outside of the polygon.
+    pub fn new(vertices: Vec<Point<f32>>, normals: Vec<Vector<f32>>) -> Self {
+        Self { vertices, normals }
+    }
+
+    /// Compute the axis-aligned bounding box of the polygon.
+    pub fn aabb(&self, pos: &Isometry<f32>) -> AABB<f32> {
+        let p0 = pos * self.vertices[0];
+        let mut mins = p0;
+        let mut maxs = p0;
+
+        for pt in &self.vertices[1..] {
+            let pt = pos * pt;
+            mins = mins.inf(&pt);
+            maxs = maxs.sup(&pt);
+        }
+
+        AABB::new(mins.into(), maxs.into())
+    }
+
+    /// The vertices of this polygon.
+    pub fn vertices(&self) -> &[Point<f32>] {
+        &self.vertices
+    }
+
+    pub(crate) fn support_point(&self, dir: &Vector<f32>) -> usize {
+        let mut best_dot = -f32::MAX;
+        let mut best_i = 0;
+
+        for (i, pt) in self.vertices.iter().enumerate() {
+            let dot = pt.coords.dot(&dir);
+            if dot > best_dot {
+                best_dot = dot;
+                best_i = i;
+            }
+        }
+
+        best_i
+    }
+
+    pub(crate) fn support_face(&self, dir: &Vector<f32>) -> usize {
+        let mut max_dot = -f32::MAX;
+        let mut max_dot_i = 0;
+
+        for (i, normal) in self.normals.iter().enumerate() {
+            let dot = normal.dot(dir);
+            if dot > max_dot {
+                max_dot = dot;
+                max_dot_i = i;
+            }
+        }
+
+        max_dot_i
+    }
+}

src/geometry/polygon_intersection.rs

@@ -0,0 +1,263 @@
+use na::{Point2, Real};
+
+use shape::SegmentPointLocation;
+use utils::{self, SegmentsIntersection, TriangleOrientation};
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+enum InFlag {
+    PIn,
+    QIn,
+    Unknown,
+}
+
+/// Location of a point on a polyline.
+pub enum PolylinePointLocation<N> {
+    /// Point on a vertex.
+    OnVertex(usize),
+    /// Point on an edge.
+    OnEdge(usize, usize, [N; 2]),
+}
+
+impl<N: Real> PolylinePointLocation<N> {
+    /// Computes the point corresponding to this location.
+    pub fn to_point(&self, pts: &[Point2<N>]) -> Point2<N> {
+        match self {
+            PolylinePointLocation::OnVertex(i) => pts[*i],
+            PolylinePointLocation::OnEdge(i1, i2, bcoords) => {
+                pts[*i1] * bcoords[0] + pts[*i2].coords * bcoords[1]
+            }
+        }
+    }
+
+    fn from_segment_point_location(a: usize, b: usize, loc: SegmentPointLocation<N>) -> Self {
+        match loc {
+            SegmentPointLocation::OnVertex(0) => PolylinePointLocation::OnVertex(a),
+            SegmentPointLocation::OnVertex(1) => PolylinePointLocation::OnVertex(b),
+            SegmentPointLocation::OnVertex(_) => unreachable!(),
+            SegmentPointLocation::OnEdge(bcoords) => PolylinePointLocation::OnEdge(a, b, bcoords),
+        }
+    }
+}
+
+/// Computes the intersection points of two convex polygons.
+///
+/// The resulting polygon is output vertex-by-vertex to the `out` closure.
+pub fn convex_polygons_intersection_points<N: Real>(
+    poly1: &[Point2<N>],
+    poly2: &[Point2<N>],
+    out: &mut Vec<Point2<N>>,
+) {
+    convex_polygons_intersection(poly1, poly2, |loc1, loc2| {
+        if let Some(loc1) = loc1 {
+            out.push(loc1.to_point(poly1))
+        } else if let Some(loc2) = loc2 {
+            out.push(loc2.to_point(poly2))
+        }
+    })
+}
+
+/// Computes the intersection of two convex polygons.
+///
+/// The resulting polygon is output vertex-by-vertex to the `out` closure.
+pub fn convex_polygons_intersection<N: Real>(
+    poly1: &[Point2<N>],
+    poly2: &[Point2<N>],
+    mut out: impl FnMut(Option<PolylinePointLocation<N>>, Option<PolylinePointLocation<N>>),
+) {
+    // FIXME: this does not handle correctly the case where the
+    // first triangle of the polygon is degenerate.
+    let rev1 = poly1.len() > 2
+        && utils::triangle_orientation(&poly1[0], &poly1[1], &poly1[2])
+            == TriangleOrientation::Clockwise;
+    let rev2 = poly2.len() > 2
+        && utils::triangle_orientation(&poly2[0], &poly2[1], &poly2[2])
+            == TriangleOrientation::Clockwise;
+
+    // println!("rev1: {}, rev2: {}", rev1, rev2);
+
+    let n = poly1.len();
+    let m = poly2.len();
+
+    let mut a = 0;
+    let mut b = 0;
+    let mut aa = 0;
+    let mut ba = 0;
+    let mut inflag = InFlag::Unknown;
+    let mut first_point_found = false;
+
+    // Quit when both adv. indices have cycled, or one has cycled twice.
+    while (aa < n || ba < m) && aa < 2 * n && ba < 2 * m {
+        let (a1, a2) = if rev1 {
+            ((n - a) % n, n - a - 1)
+        } else {
+            ((a + n - 1) % n, a)
+        };
+
+        let (b1, b2) = if rev2 {
+            ((m - b) % m, m - b - 1)
+        } else {
+            ((b + m - 1) % m, b)
+        };
+
+        // println!("Current indices: ({}, {}), ({}, {})", a1, a2, b1, b2);
+
+        let dir_edge1 = poly1[a2] - poly1[a1];
+        let dir_edge2 = poly2[b2] - poly2[b1];
+
+        let cross = utils::triangle_orientation(
+            &Point2::origin(),
+            &Point2::from_coordinates(dir_edge1),
+            &Point2::from_coordinates(dir_edge2),
+        );
+        let aHB = utils::triangle_orientation(&poly2[b1], &poly2[b2], &poly1[a2]);
+        let bHA = utils::triangle_orientation(&poly1[a1], &poly1[a2], &poly2[b2]);
+
+        // If edge1 & edge2 intersect, update inflag.
+        if let Some(inter) =
+            utils::segments_intersection(&poly1[a1], &poly1[a2], &poly2[b1], &poly2[b2])
+        {
+            match inter {
+                SegmentsIntersection::Point { loc1, loc2 } => {
+                    let loc1 = PolylinePointLocation::from_segment_point_location(a1, a2, loc1);
+                    let loc2 = PolylinePointLocation::from_segment_point_location(b1, b2, loc2);
+                    out(Some(loc1), Some(loc2));
+
+                    if inflag == InFlag::Unknown && !first_point_found {
+                        // This is the first point.
+                        aa = 0;
+                        ba = 0;
+                        first_point_found = true;
+                    }
+
+                    // Update inflag.
+                    if aHB == TriangleOrientation::Counterclockwise {
+                        inflag = InFlag::PIn;
+                    } else if bHA == TriangleOrientation::Counterclockwise {
+                        inflag = InFlag::QIn;
+                    }
+                }
+                SegmentsIntersection::Segment {
+                    first_loc1,
+                    first_loc2,
+                    second_loc1,
+                    second_loc2,
+                } => {
+                    // Special case: edge1 & edge2 overlap and oppositely oriented.
+                    if dir_edge1.dot(&dir_edge2) < N::zero() {
+                        let loc1 =
+                            PolylinePointLocation::from_segment_point_location(a1, a2, first_loc1);
+                        let loc2 =
+                            PolylinePointLocation::from_segment_point_location(b1, b2, first_loc2);
+                        out(Some(loc1), Some(loc2));
+
+                        let loc1 =
+                            PolylinePointLocation::from_segment_point_location(a1, a2, second_loc1);
+                        let loc2 =
+                            PolylinePointLocation::from_segment_point_location(b1, b2, second_loc2);
+                        out(Some(loc1), Some(loc2));
+
+                        return;
+                    }
+                }
+            }
+        }
+
+        // Special case: edge1 & edge2 parallel and separated.
+        if cross == TriangleOrientation::Degenerate
+            && aHB == TriangleOrientation::Clockwise
+            && bHA == TriangleOrientation::Clockwise
+        {
+            return;
+        }
+        // Special case: edge1 & edge2 collinear.
+        else if cross == TriangleOrientation::Degenerate
+            && aHB == TriangleOrientation::Degenerate
+            && bHA == TriangleOrientation::Degenerate
+        {
+            // Advance but do not output point.
+            if inflag == InFlag::PIn {
+                b = advance(b, &mut ba, m);
+            } else {
+                a = advance(a, &mut aa, n);
+            }
+        }
+        // Generic cases.
+        else if cross == TriangleOrientation::Counterclockwise {
+            if bHA == TriangleOrientation::Counterclockwise {
+                if inflag == InFlag::PIn {
+                    out(Some(PolylinePointLocation::OnVertex(a2)), None)
+                }
+                a = advance(a, &mut aa, n);
+            } else {
+                if inflag == InFlag::QIn {
+                    out(None, Some(PolylinePointLocation::OnVertex(b2)))
+                }
+                b = advance(b, &mut ba, m);
+            }
+        } else {
+            // We have cross == TriangleOrientation::Clockwise.
+            if aHB == TriangleOrientation::Counterclockwise {
+                if inflag == InFlag::QIn {
+                    out(None, Some(PolylinePointLocation::OnVertex(b2)))
+                }
+                b = advance(b, &mut ba, m);
+            } else {
+                if inflag == InFlag::PIn {
+                    out(Some(PolylinePointLocation::OnVertex(a2)), None)
+                }
+                a = advance(a, &mut aa, n);
+            }
+        }
+    }
+
+    if !first_point_found {
+        // No intersection: test if one polygon completely encloses the other.
+        let mut orient = TriangleOrientation::Degenerate;
+        let mut ok = true;
+
+        for a in 0..n {
+            let a1 = (a + n - 1) % n; // a - 1
+            let new_orient = utils::triangle_orientation(&poly1[a1], &poly1[a], &poly2[0]);
+
+            if orient == TriangleOrientation::Degenerate {
+                orient = new_orient
+            } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate {
+                ok = false;
+                break;
+            }
+        }
+
+        if ok {
+            for b in 0..m {
+                out(None, Some(PolylinePointLocation::OnVertex(b)))
+            }
+        }
+
+        let mut orient = TriangleOrientation::Degenerate;
+        let mut ok = true;
+
+        for b in 0..m {
+            let b1 = (b + m - 1) % m; // b - 1
+            let new_orient = utils::triangle_orientation(&poly2[b1], &poly2[b], &poly1[0]);
+
+            if orient == TriangleOrientation::Degenerate {
+                orient = new_orient
+            } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate {
+                ok = false;
+                break;
+            }
+        }
+
+        if ok {
+            for a in 0..n {
+                out(Some(PolylinePointLocation::OnVertex(a)), None)
+            }
+        }
+    }
+}
+
+#[inline]
+fn advance(a: usize, aa: &mut usize, n: usize) -> usize {
+    *aa += 1;
+    (a + 1) % n
+}

src/geometry/polyhedron_feature3d.rs

@@ -0,0 +1,284 @@
+use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle};
+use crate::math::{Isometry, Point, Vector};
+use crate::utils::WBasis;
+use na::Point2;
+use ncollide::shape::Segment;
+
+#[derive(Debug)]
+pub struct PolyhedronFace {
+    pub vertices: [Point<f32>; 4],
+    pub vids: [u8; 4], // Feature ID of the vertices.
+    pub eids: [u8; 4], // Feature ID of the edges.
+    pub fid: u8,       // Feature ID of the face.
+    pub num_vertices: usize,
+}
+
+impl From<CuboidFeatureFace> for PolyhedronFace {
+    fn from(face: CuboidFeatureFace) -> Self {
+        Self {
+            vertices: face.vertices,
+            vids: face.vids,
+            eids: face.eids,
+            fid: face.fid,
+            num_vertices: 4,
+        }
+    }
+}
+
+impl From<Triangle> for PolyhedronFace {
+    fn from(tri: Triangle) -> Self {
+        Self {
+            vertices: [tri.a, tri.b, tri.c, tri.c],
+            vids: [0, 2, 4, 4],
+            eids: [1, 3, 5, 5],
+            fid: 0,
+            num_vertices: 3,
+        }
+    }
+}
+
+impl From<Segment<f32>> for PolyhedronFace {
+    fn from(seg: Segment<f32>) -> Self {
+        Self {
+            vertices: [seg.a, seg.b, seg.b, seg.b],
+            vids: [0, 2, 2, 2],
+            eids: [1, 1, 1, 1],
+            fid: 0,
+            num_vertices: 2,
+        }
+    }
+}
+
+impl PolyhedronFace {
+    pub fn transform_by(&mut self, iso: &Isometry<f32>) {
+        for v in &mut self.vertices[0..self.num_vertices] {
+            *v = iso * *v;
+        }
+    }
+
+    pub fn contacts(
+        prediction_distance: f32,
+        face1: &PolyhedronFace,
+        sep_axis1: &Vector<f32>,
+        face2: &PolyhedronFace,
+        pos21: &Isometry<f32>,
+        manifold: &mut ContactManifold,
+    ) {
+        // Project the faces to a 2D plane for contact clipping.
+        // The plane they are projected onto has normal sep_axis1
+        // and contains the origin (this is numerically OK because
+        // we are not working in world-space here).
+        let basis = sep_axis1.orthonormal_basis();
+        let projected_face1 = [
+            Point2::new(
+                face1.vertices[0].coords.dot(&basis[0]),
+                face1.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[1].coords.dot(&basis[0]),
+                face1.vertices[1].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[2].coords.dot(&basis[0]),
+                face1.vertices[2].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face1.vertices[3].coords.dot(&basis[0]),
+                face1.vertices[3].coords.dot(&basis[1]),
+            ),
+        ];
+        let projected_face2 = [
+            Point2::new(
+                face2.vertices[0].coords.dot(&basis[0]),
+                face2.vertices[0].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[1].coords.dot(&basis[0]),
+                face2.vertices[1].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[2].coords.dot(&basis[0]),
+                face2.vertices[2].coords.dot(&basis[1]),
+            ),
+            Point2::new(
+                face2.vertices[3].coords.dot(&basis[0]),
+                face2.vertices[3].coords.dot(&basis[1]),
+            ),
+        ];
+
+        // Also find all the vertices located inside of the other projected face.
+        if face2.num_vertices > 2 {
+            let normal2 = (face2.vertices[2] - face2.vertices[1])
+                .cross(&(face2.vertices[0] - face2.vertices[1]));
+
+            let last_index2 = face2.num_vertices as usize - 1;
+            'point_loop1: for i in 0..face1.num_vertices as usize {
+                let p1 = projected_face1[i];
+
+                let sign = (projected_face2[0] - projected_face2[last_index2])
+                    .perp(&(p1 - projected_face2[last_index2]));
+                for j in 0..last_index2 {
+                    let new_sign = (projected_face2[j + 1] - projected_face2[j])
+                        .perp(&(p1 - projected_face2[j]));
+                    if new_sign * sign < 0.0 {
+                        // The point lies outside.
+                        continue 'point_loop1;
+                    }
+                }
+
+                // All the perp had the same sign: the point is inside of the other shapes projection.
+                // Output the contact.
+                let denom = normal2.dot(&sep_axis1);
+                let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
+                let local_p1 = face1.vertices[i];
+                let local_p2 = face1.vertices[i] + dist * sep_axis1;
+
+                if dist <= prediction_distance {
+                    manifold.points.push(Contact {
+                        local_p1,
+                        local_p2: pos21 * local_p2,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: face1.vids[i],
+                        fid2: face2.fid,
+                        dist,
+                    });
+                }
+            }
+        }
+
+        if face1.num_vertices > 2 {
+            let normal1 = (face1.vertices[2] - face1.vertices[1])
+                .cross(&(face1.vertices[0] - face1.vertices[1]));
+
+            let last_index1 = face1.num_vertices as usize - 1;
+            'point_loop2: for i in 0..face2.num_vertices as usize {
+                let p2 = projected_face2[i];
+
+                let sign = (projected_face1[0] - projected_face1[last_index1])
+                    .perp(&(p2 - projected_face1[last_index1]));
+                for j in 0..last_index1 {
+                    let new_sign = (projected_face1[j + 1] - projected_face1[j])
+                        .perp(&(p2 - projected_face1[j]));
+
+                    if new_sign * sign < 0.0 {
+                        // The point lies outside.
+                        continue 'point_loop2;
+                    }
+                }
+
+                // All the perp had the same sign: the point is inside of the other shapes projection.
+                // Output the contact.
+                let denom = -normal1.dot(&sep_axis1);
+                let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
+                let local_p2 = face2.vertices[i];
+                let local_p1 = face2.vertices[i] - dist * sep_axis1;
+
+                if true {
+                    // dist <= prediction_distance {
+                    manifold.points.push(Contact {
+                        local_p1,
+                        local_p2: pos21 * local_p2,
+                        impulse: 0.0,
+                        tangent_impulse: Contact::zero_tangent_impulse(),
+                        fid1: face1.fid,
+                        fid2: face2.vids[i],
+                        dist,
+                    });
+                }
+            }
+        }
+
+        // Now we have to compute the intersection between all pairs of
+        // edges from the face 1 and from the face2.
+        for j in 0..face2.num_vertices {
+            let projected_edge2 = [
+                projected_face2[j],
+                projected_face2[(j + 1) % face2.num_vertices],
+            ];
+
+            for i in 0..face1.num_vertices {
+                let projected_edge1 = [
+                    projected_face1[i],
+                    projected_face1[(i + 1) % face1.num_vertices],
+                ];
+                if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
+                    if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
+                        // Found a contact between the two edges.
+                        let edge1 = (
+                            face1.vertices[i],
+                            face1.vertices[(i + 1) % face1.num_vertices],
+                        );
+                        let edge2 = (
+                            face2.vertices[j],
+                            face2.vertices[(j + 1) % face2.num_vertices],
+                        );
+                        let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
+                        let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
+                        let dist = (local_p2 - local_p1).dot(&sep_axis1);
+
+                        if dist <= prediction_distance {
+                            manifold.points.push(Contact {
+                                local_p1,
+                                local_p2: pos21 * local_p2,
+                                impulse: 0.0,
+                                tangent_impulse: Contact::zero_tangent_impulse(),
+                                fid1: face1.eids[i],
+                                fid2: face2.eids[j],
+                                dist,
+                            });
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+/// Compute the barycentric coordinates of the intersection between the two given lines.
+/// Returns `None` if the lines are parallel.
+fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
+    use approx::AbsDiffEq;
+
+    // Inspired by Real-time collision detection by Christer Ericson.
+    let dir1 = edge1[1] - edge1[0];
+    let dir2 = edge2[1] - edge2[0];
+    let r = edge1[0] - edge2[0];
+
+    let a = dir1.norm_squared();
+    let e = dir2.norm_squared();
+    let f = dir2.dot(&r);
+
+    let eps = f32::default_epsilon();
+
+    if a <= eps && e <= eps {
+        Some((0.0, 0.0))
+    } else if a <= eps {
+        Some((0.0, f / e))
+    } else {
+        let c = dir1.dot(&r);
+        if e <= eps {
+            Some((-c / a, 0.0))
+        } else {
+            let b = dir1.dot(&dir2);
+            let ae = a * e;
+            let bb = b * b;
+            let denom = ae - bb;
+
+            // Use absolute and ulps error to test collinearity.
+            let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
+
+            let s = if !parallel {
+                (b * f - c * e) / denom
+            } else {
+                0.0
+            };
+
+            if parallel {
+                None
+            } else {
+                Some((s, (b * s + f) / e))
+            }
+        }
+    }
+}

src/geometry/proximity.rs

@@ -0,0 +1,31 @@
+use crate::geometry::proximity_detector::ProximityPhase;
+use crate::geometry::{ColliderPair, Proximity};
+use std::any::Any;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The description of the proximity of two colliders.
+pub struct ProximityPair {
+    /// The pair of collider involved.
+    pub pair: ColliderPair,
+    /// The state of proximity between the two colliders.
+    pub proximity: Proximity,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pub(crate) detector: Option<ProximityPhase>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pub(crate) detector_workspace: Option<Box<dyn Any + Send + Sync>>,
+}
+
+impl ProximityPair {
+    pub(crate) fn new(
+        pair: ColliderPair,
+        detector: ProximityPhase,
+        detector_workspace: Option<Box<dyn Any + Send + Sync>>,
+    ) -> Self {
+        Self {
+            pair,
+            proximity: Proximity::Disjoint,
+            detector: Some(detector),
+            detector_workspace,
+        }
+    }
+}

src/geometry/proximity_detector/ball_ball_proximity_detector.rs

@@ -0,0 +1,68 @@
+use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
+
+use crate::geometry::Proximity;
+use crate::math::Point;
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::geometry::{proximity_detector::PrimitiveProximityDetectionContextSimd, WBall},
+    crate::math::{SimdFloat, SIMD_WIDTH},
+    simba::simd::SimdValue,
+};
+
+#[cfg(feature = "simd-is-enabled")]
+fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdFloat {
+    let dcenter = ball2.center - ball1.center;
+    let center_dist = dcenter.magnitude();
+    center_dist - ball1.radius - ball2.radius
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub fn detect_proximity_ball_ball_simd(
+    ctxt: &mut PrimitiveProximityDetectionContextSimd,
+) -> [Proximity; SIMD_WIDTH] {
+    let pos_ba = ctxt.positions2.inverse() * ctxt.positions1;
+    let radii_a =
+        SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
+    let radii_b =
+        SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
+
+    let wball_a = WBall::new(Point::origin(), radii_a);
+    let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b);
+    let distances = ball_distance_simd(&wball_a, &wball_b);
+    let mut proximities = [Proximity::Disjoint; SIMD_WIDTH];
+
+    for i in 0..SIMD_WIDTH {
+        // FIXME: compare the dist before computing the proximity.
+        let dist = distances.extract(i);
+        if dist > ctxt.prediction_distance {
+            proximities[i] = Proximity::Disjoint;
+        } else if dist > 0.0 {
+            proximities[i] = Proximity::WithinMargin;
+        } else {
+            proximities[i] = Proximity::Intersecting
+        }
+    }
+
+    proximities
+}
+
+pub fn detect_proximity_ball_ball(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
+    let pos_ba = ctxt.position2.inverse() * ctxt.position1;
+    let radius_a = ctxt.shape1.as_ball().unwrap().radius;
+    let radius_b = ctxt.shape2.as_ball().unwrap().radius;
+
+    let center_a = Point::origin();
+    let center_b = pos_ba.inverse_transform_point(&Point::origin());
+
+    let dcenter = center_b - center_a;
+    let center_dist = dcenter.magnitude();
+    let dist = center_dist - radius_a - radius_b;
+
+    if dist > ctxt.prediction_distance {
+        Proximity::Disjoint
+    } else if dist > 0.0 {
+        Proximity::WithinMargin
+    } else {
+        Proximity::Intersecting
+    }
+}

src/geometry/proximity_detector/ball_convex_proximity_detector.rs

@@ -0,0 +1,53 @@
+use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
+use crate::geometry::{Ball, Proximity, Shape};
+use crate::math::Isometry;
+use ncollide::query::PointQuery;
+
+pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
+    if let Shape::Ball(ball1) = ctxt.shape1 {
+        match ctxt.shape2 {
+            Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt),
+            Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt),
+            _ => unimplemented!(),
+        }
+    } else if let Shape::Ball(ball2) = ctxt.shape2 {
+        match ctxt.shape1 {
+            Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt),
+            Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt),
+            _ => unimplemented!(),
+        }
+    } else {
+        panic!("Invalid shape types provide.")
+    }
+}
+
+fn do_detect_proximity<P: PointQuery<f32>>(
+    point_query1: &P,
+    ball2: &Ball,
+    ctxt: &PrimitiveProximityDetectionContext,
+) -> Proximity {
+    let local_p2_1 = ctxt
+        .position1
+        .inverse_transform_point(&ctxt.position2.translation.vector.into());
+
+    // TODO: add a `project_local_point` to the PointQuery trait to avoid
+    // the identity isometry.
+    let proj =
+        point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3"));
+    let dpos = local_p2_1 - proj.point;
+    let dist = dpos.norm();
+
+    if proj.is_inside {
+        return Proximity::Intersecting;
+    }
+
+    if dist <= ball2.radius + ctxt.prediction_distance {
+        if dist <= ball2.radius {
+            Proximity::Intersecting
+        } else {
+            Proximity::WithinMargin
+        }
+    } else {
+        Proximity::Disjoint
+    }
+}

src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs

@@ -0,0 +1,79 @@
+use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
+use crate::geometry::{sat, Proximity, Shape};
+use crate::math::Isometry;
+use ncollide::shape::Cuboid;
+
+pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
+    if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+        detect_proximity(
+            ctxt.prediction_distance,
+            cube1,
+            ctxt.position1,
+            cube2,
+            ctxt.position2,
+        )
+    } else {
+        unreachable!()
+    }
+}
+
+pub fn detect_proximity<'a>(
+    prediction_distance: f32,
+    cube1: &'a Cuboid<f32>,
+    pos1: &'a Isometry<f32>,
+    cube2: &'a Cuboid<f32>,
+    pos2: &'a Isometry<f32>,
+) -> Proximity {
+    let pos12 = pos1.inverse() * pos2;
+    let pos21 = pos12.inverse();
+
+    /*
+     *
+     * Point-Face
+     *
+     */
+    let sep1 =
+        sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21).0;
+    if sep1 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    let sep2 =
+        sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12).0;
+    if sep2 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    /*
+     *
+     * Edge-Edge cases
+     *
+     */
+    #[cfg(feature = "dim2")]
+    let sep3 = -f32::MAX; // This case does not exist in 2D.
+    #[cfg(feature = "dim3")]
+    let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21).0;
+    if sep3 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    if sep2 > sep1 && sep2 > sep3 {
+        if sep2 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    } else if sep3 > sep1 {
+        if sep3 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    } else {
+        if sep1 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    }
+}

src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs

@@ -0,0 +1,88 @@
+use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
+use crate::geometry::{sat, Cuboid, Proximity, Shape, Triangle};
+use crate::math::Isometry;
+
+pub fn detect_proximity_cuboid_triangle(
+    ctxt: &mut PrimitiveProximityDetectionContext,
+) -> Proximity {
+    if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) {
+        detect_proximity(
+            ctxt.prediction_distance,
+            cube1,
+            ctxt.position1,
+            triangle2,
+            ctxt.position2,
+        )
+    } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
+        detect_proximity(
+            ctxt.prediction_distance,
+            cube2,
+            ctxt.position2,
+            triangle1,
+            ctxt.position1,
+        )
+    } else {
+        panic!("Invalid shape types")
+    }
+}
+
+pub fn detect_proximity<'a>(
+    prediction_distance: f32,
+    cube1: &'a Cuboid,
+    pos1: &'a Isometry<f32>,
+    triangle2: &'a Triangle,
+    pos2: &'a Isometry<f32>,
+) -> Proximity {
+    let pos12 = pos1.inverse() * pos2;
+    let pos21 = pos12.inverse();
+
+    /*
+     *
+     * Point-Face cases.
+     *
+     */
+    let sep1 =
+        sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0;
+    if sep1 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21).0;
+    if sep2 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    /*
+     *
+     * Edge-Edge cases.
+     *
+     */
+    #[cfg(feature = "dim2")]
+    let sep3 = -f32::MAX; // This case does not exist in 2D.
+    #[cfg(feature = "dim3")]
+    let sep3 =
+        sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21).0;
+    if sep3 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    if sep2 > sep1 && sep2 > sep3 {
+        if sep2 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    } else if sep3 > sep1 {
+        if sep3 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    } else {
+        if sep1 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    }
+}

src/geometry/proximity_detector/mod.rs

@@ -0,0 +1,30 @@
+pub use self::ball_ball_proximity_detector::detect_proximity_ball_ball;
+#[cfg(feature = "simd-is-enabled")]
+pub use self::ball_ball_proximity_detector::detect_proximity_ball_ball_simd;
+pub use self::ball_convex_proximity_detector::detect_proximity_ball_convex;
+pub use self::cuboid_cuboid_proximity_detector::detect_proximity_cuboid_cuboid;
+pub use self::cuboid_triangle_proximity_detector::detect_proximity_cuboid_triangle;
+pub use self::polygon_polygon_proximity_detector::detect_proximity_polygon_polygon;
+pub use self::proximity_detector::{
+    PrimitiveProximityDetectionContext, PrimitiveProximityDetector, ProximityDetectionContext,
+    ProximityDetector, ProximityPhase,
+};
+#[cfg(feature = "simd-is-enabled")]
+pub use self::proximity_detector::{
+    PrimitiveProximityDetectionContextSimd, ProximityDetectionContextSimd,
+};
+pub use self::proximity_dispatcher::{DefaultProximityDispatcher, ProximityDispatcher};
+pub use self::trimesh_shape_proximity_detector::{
+    detect_proximity_trimesh_shape, TrimeshShapeProximityDetectorWorkspace,
+};
+
+mod ball_ball_proximity_detector;
+mod ball_convex_proximity_detector;
+mod ball_polygon_proximity_detector;
+mod cuboid_cuboid_proximity_detector;
+mod cuboid_polygon_proximity_detector;
+mod cuboid_triangle_proximity_detector;
+mod polygon_polygon_proximity_detector;
+mod proximity_detector;
+mod proximity_dispatcher;
+mod trimesh_shape_proximity_detector;

src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs

@@ -0,0 +1,54 @@
+use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
+use crate::geometry::{sat, Polygon, Proximity, Shape};
+use crate::math::Isometry;
+
+pub fn detect_proximity_polygon_polygon(
+    ctxt: &mut PrimitiveProximityDetectionContext,
+) -> Proximity {
+    if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
+        detect_proximity(
+            ctxt.prediction_distance,
+            polygon1,
+            &ctxt.position1,
+            polygon2,
+            &ctxt.position2,
+        )
+    } else {
+        unreachable!()
+    }
+}
+
+fn detect_proximity<'a>(
+    prediction_distance: f32,
+    p1: &'a Polygon,
+    m1: &'a Isometry<f32>,
+    p2: &'a Polygon,
+    m2: &'a Isometry<f32>,
+) -> Proximity {
+    let m12 = m1.inverse() * m2;
+    let m21 = m12.inverse();
+
+    let sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12);
+    if sep1.0 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    let sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21);
+    if sep2.0 > prediction_distance {
+        return Proximity::Disjoint;
+    }
+
+    if sep2.0 > sep1.0 {
+        if sep2.0 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    } else {
+        if sep1.0 > 0.0 {
+            Proximity::WithinMargin
+        } else {
+            Proximity::Intersecting
+        }
+    }
+}

src/geometry/proximity_detector/proximity_detector.rs

@@ -0,0 +1,212 @@
+use crate::geometry::{
+    Collider, ColliderSet, Proximity, ProximityDispatcher, ProximityEvent, ProximityPair, Shape,
+};
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::{SimdFloat, SIMD_WIDTH};
+use crate::pipeline::EventHandler;
+use std::any::Any;
+
+#[derive(Copy, Clone)]
+pub enum ProximityPhase {
+    NearPhase(ProximityDetector),
+    ExactPhase(PrimitiveProximityDetector),
+}
+
+impl ProximityPhase {
+    #[inline]
+    pub fn detect_proximity(
+        self,
+        mut context: ProximityDetectionContext,
+        events: &dyn EventHandler,
+    ) {
+        let proximity = match self {
+            Self::NearPhase(gen) => (gen.detect_proximity)(&mut context),
+            Self::ExactPhase(gen) => {
+                // Build the primitive context from the non-primitive context and dispatch.
+                let collider1 = &context.colliders[context.pair.pair.collider1];
+                let collider2 = &context.colliders[context.pair.pair.collider2];
+
+                let mut context2 = PrimitiveProximityDetectionContext {
+                    prediction_distance: context.prediction_distance,
+                    collider1,
+                    collider2,
+                    shape1: collider1.shape(),
+                    shape2: collider2.shape(),
+                    position1: collider1.position(),
+                    position2: collider2.position(),
+                    workspace: context.pair.detector_workspace.as_mut().map(|w| &mut **w),
+                };
+
+                (gen.detect_proximity)(&mut context2)
+            }
+        };
+
+        if context.pair.proximity != proximity {
+            events.handle_proximity_event(ProximityEvent::new(
+                context.pair.pair.collider1,
+                context.pair.pair.collider2,
+                context.pair.proximity,
+                proximity,
+            ))
+        }
+
+        context.pair.proximity = proximity;
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    #[inline]
+    pub fn detect_proximity_simd(
+        self,
+        mut context: ProximityDetectionContextSimd,
+        events: &dyn EventHandler,
+    ) {
+        let proximities = match self {
+            Self::NearPhase(gen) => (gen.detect_proximity_simd)(&mut context),
+            Self::ExactPhase(gen) => {
+                // Build the primitive context from the non-primitive context and dispatch.
+                use arrayvec::ArrayVec;
+                let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> =
+                    ArrayVec::new();
+                let mut workspace_arr: ArrayVec<
+                    [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH],
+                > = ArrayVec::new();
+
+                for pair in context.pairs.iter_mut() {
+                    let collider1 = &context.colliders[pair.pair.collider1];
+                    let collider2 = &context.colliders[pair.pair.collider2];
+                    colliders_arr.push((collider1, collider2));
+                    workspace_arr.push(pair.detector_workspace.as_mut().map(|w| &mut **w));
+                }
+
+                let max_index = colliders_arr.len() - 1;
+                let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH];
+                let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH];
+
+                let mut context2 = PrimitiveProximityDetectionContextSimd {
+                    prediction_distance: context.prediction_distance,
+                    colliders1,
+                    colliders2,
+                    shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH],
+                    shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH],
+                    positions1: &Isometry::from(
+                        array![|ii| *colliders1[ii].position(); SIMD_WIDTH],
+                    ),
+                    positions2: &Isometry::from(
+                        array![|ii| *colliders2[ii].position(); SIMD_WIDTH],
+                    ),
+                    workspaces: workspace_arr.as_mut_slice(),
+                };
+
+                (gen.detect_proximity_simd)(&mut context2)
+            }
+        };
+
+        for (i, pair) in context.pairs.iter_mut().enumerate() {
+            if pair.proximity != proximities[i] {
+                events.handle_proximity_event(ProximityEvent::new(
+                    pair.pair.collider1,
+                    pair.pair.collider2,
+                    pair.proximity,
+                    proximities[i],
+                ))
+            }
+            pair.proximity = proximities[i];
+        }
+    }
+}
+
+pub struct PrimitiveProximityDetectionContext<'a> {
+    pub prediction_distance: f32,
+    pub collider1: &'a Collider,
+    pub collider2: &'a Collider,
+    pub shape1: &'a Shape,
+    pub shape2: &'a Shape,
+    pub position1: &'a Isometry<f32>,
+    pub position2: &'a Isometry<f32>,
+    pub workspace: Option<&'a mut (dyn Any + Send + Sync)>,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> {
+    pub prediction_distance: f32,
+    pub colliders1: [&'a Collider; SIMD_WIDTH],
+    pub colliders2: [&'a Collider; SIMD_WIDTH],
+    pub shapes1: [&'a Shape; SIMD_WIDTH],
+    pub shapes2: [&'a Shape; SIMD_WIDTH],
+    pub positions1: &'a Isometry<SimdFloat>,
+    pub positions2: &'a Isometry<SimdFloat>,
+    pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],
+}
+
+#[derive(Copy, Clone)]
+pub struct PrimitiveProximityDetector {
+    pub detect_proximity: fn(&mut PrimitiveProximityDetectionContext) -> Proximity,
+    #[cfg(feature = "simd-is-enabled")]
+    pub detect_proximity_simd:
+        fn(&mut PrimitiveProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH],
+}
+
+impl PrimitiveProximityDetector {
+    fn unimplemented_fn(_ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
+        Proximity::Disjoint
+    }
+    #[cfg(feature = "simd-is-enabled")]
+    fn unimplemented_simd_fn(
+        _ctxt: &mut PrimitiveProximityDetectionContextSimd,
+    ) -> [Proximity; SIMD_WIDTH] {
+        [Proximity::Disjoint; SIMD_WIDTH]
+    }
+}
+
+impl Default for PrimitiveProximityDetector {
+    fn default() -> Self {
+        Self {
+            detect_proximity: Self::unimplemented_fn,
+            #[cfg(feature = "simd-is-enabled")]
+            detect_proximity_simd: Self::unimplemented_simd_fn,
+        }
+    }
+}
+
+pub struct ProximityDetectionContext<'a> {
+    pub dispatcher: &'a dyn ProximityDispatcher,
+    pub prediction_distance: f32,
+    pub colliders: &'a ColliderSet,
+    pub pair: &'a mut ProximityPair,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+pub struct ProximityDetectionContextSimd<'a, 'b> {
+    pub dispatcher: &'a dyn ProximityDispatcher,
+    pub prediction_distance: f32,
+    pub colliders: &'a ColliderSet,
+    pub pairs: &'a mut [&'b mut ProximityPair],
+}
+
+#[derive(Copy, Clone)]
+pub struct ProximityDetector {
+    pub detect_proximity: fn(&mut ProximityDetectionContext) -> Proximity,
+    #[cfg(feature = "simd-is-enabled")]
+    pub detect_proximity_simd: fn(&mut ProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH],
+}
+
+impl ProximityDetector {
+    fn unimplemented_fn(_ctxt: &mut ProximityDetectionContext) -> Proximity {
+        Proximity::Disjoint
+    }
+    #[cfg(feature = "simd-is-enabled")]
+    fn unimplemented_simd_fn(_ctxt: &mut ProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH] {
+        [Proximity::Disjoint; SIMD_WIDTH]
+    }
+}
+
+impl Default for ProximityDetector {
+    fn default() -> Self {
+        Self {
+            detect_proximity: Self::unimplemented_fn,
+            #[cfg(feature = "simd-is-enabled")]
+            detect_proximity_simd: Self::unimplemented_simd_fn,
+        }
+    }
+}

src/geometry/proximity_detector/proximity_dispatcher.rs

@@ -0,0 +1,136 @@
+use crate::geometry::proximity_detector::{
+    PrimitiveProximityDetector, ProximityDetector, ProximityPhase,
+    TrimeshShapeProximityDetectorWorkspace,
+};
+use crate::geometry::Shape;
+use std::any::Any;
+
+/// Trait implemented by structures responsible for selecting a collision-detection algorithm
+/// for a given pair of shapes.
+pub trait ProximityDispatcher {
+    /// Select the proximity detection algorithm for the given pair of primitive shapes.
+    fn dispatch_primitives(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (
+        PrimitiveProximityDetector,
+        Option<Box<dyn Any + Send + Sync>>,
+    );
+    /// Select the proximity detection algorithm for the given pair of non-primitive shapes.
+    fn dispatch(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>);
+}
+
+/// The default proximity dispatcher used by Rapier.
+pub struct DefaultProximityDispatcher;
+
+impl ProximityDispatcher for DefaultProximityDispatcher {
+    fn dispatch_primitives(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (
+        PrimitiveProximityDetector,
+        Option<Box<dyn Any + Send + Sync>>,
+    ) {
+        match (shape1, shape2) {
+            (Shape::Ball(_), Shape::Ball(_)) => (
+                PrimitiveProximityDetector {
+                    #[cfg(feature = "simd-is-enabled")]
+                    detect_proximity_simd: super::detect_proximity_ball_ball_simd,
+                    detect_proximity: super::detect_proximity_ball_ball,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Cuboid(_), Shape::Cuboid(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_cuboid_cuboid,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Polygon(_), Shape::Polygon(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_polygon_polygon,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Triangle(_), Shape::Ball(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_ball_convex,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Ball(_), Shape::Triangle(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_ball_convex,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Cuboid(_), Shape::Ball(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_ball_convex,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Ball(_), Shape::Cuboid(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_ball_convex,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Triangle(_), Shape::Cuboid(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_cuboid_triangle,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            (Shape::Cuboid(_), Shape::Triangle(_)) => (
+                PrimitiveProximityDetector {
+                    detect_proximity: super::detect_proximity_cuboid_triangle,
+                    ..PrimitiveProximityDetector::default()
+                },
+                None,
+            ),
+            _ => (PrimitiveProximityDetector::default(), None),
+        }
+    }
+
+    fn dispatch(
+        &self,
+        shape1: &Shape,
+        shape2: &Shape,
+    ) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>) {
+        match (shape1, shape2) {
+            (Shape::Trimesh(_), _) => (
+                ProximityPhase::NearPhase(ProximityDetector {
+                    detect_proximity: super::detect_proximity_trimesh_shape,
+                    ..ProximityDetector::default()
+                }),
+                Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())),
+            ),
+            (_, Shape::Trimesh(_)) => (
+                ProximityPhase::NearPhase(ProximityDetector {
+                    detect_proximity: super::detect_proximity_trimesh_shape,
+                    ..ProximityDetector::default()
+                }),
+                Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())),
+            ),
+            _ => {
+                let (gen, workspace) = self.dispatch_primitives(shape1, shape2);
+                (ProximityPhase::ExactPhase(gen), workspace)
+            }
+        }
+    }
+}

src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs

@@ -0,0 +1,133 @@
+use crate::geometry::proximity_detector::{
+    PrimitiveProximityDetectionContext, ProximityDetectionContext,
+};
+use crate::geometry::{Collider, Proximity, Shape, Trimesh, WAABBHierarchyIntersections};
+use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
+
+pub struct TrimeshShapeProximityDetectorWorkspace {
+    interferences: WAABBHierarchyIntersections,
+    local_aabb2: AABB<f32>,
+    old_interferences: Vec<usize>,
+}
+
+impl TrimeshShapeProximityDetectorWorkspace {
+    pub fn new() -> Self {
+        Self {
+            interferences: WAABBHierarchyIntersections::new(),
+            local_aabb2: AABB::new_invalid(),
+            old_interferences: Vec::new(),
+        }
+    }
+}
+
+pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> Proximity {
+    let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
+    let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
+
+    if let Shape::Trimesh(trimesh1) = collider1.shape() {
+        do_detect_proximity(trimesh1, collider1, collider2, ctxt)
+    } else if let Shape::Trimesh(trimesh2) = collider2.shape() {
+        do_detect_proximity(trimesh2, collider2, collider1, ctxt)
+    } else {
+        panic!("Invalid shape types provided.")
+    }
+}
+
+fn do_detect_proximity(
+    trimesh1: &Trimesh,
+    collider1: &Collider,
+    collider2: &Collider,
+    ctxt: &mut ProximityDetectionContext,
+) -> Proximity {
+    let workspace: &mut TrimeshShapeProximityDetectorWorkspace = ctxt
+        .pair
+        .detector_workspace
+        .as_mut()
+        .expect("The TrimeshShapeProximityDetectorWorkspace is missing.")
+        .downcast_mut()
+        .expect("Invalid workspace type, expected a TrimeshShapeProximityDetectorWorkspace.");
+
+    /*
+     * Compute interferences.
+     */
+    let pos12 = collider1.position.inverse() * collider2.position;
+    // TODO: somehow precompute the AABB and reuse it?
+    let mut new_local_aabb2 = collider2
+        .shape()
+        .compute_aabb(&pos12)
+        .loosened(ctxt.prediction_distance);
+    let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2);
+
+    if !same_local_aabb2 {
+        let extra_margin =
+            (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1));
+        new_local_aabb2.mins -= extra_margin;
+        new_local_aabb2.maxs += extra_margin;
+
+        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,
+            &mut workspace.interferences.computed_interferences_mut(),
+        );
+
+        trimesh1
+            .waabbs()
+            .compute_interferences_with(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 mut old_inter_it = workspace.old_interferences.drain(..).peekable();
+    let mut best_proximity = Proximity::Disjoint;
+
+    for triangle_id in new_interferences.iter() {
+        if *triangle_id >= trimesh1.num_triangles() {
+            // Because of SIMD padding, the broad-phase may return tiangle indices greater
+            // than the max.
+            continue;
+        }
+
+        if !same_local_aabb2 {
+            loop {
+                match old_inter_it.peek() {
+                    Some(old_triangle_id) if *old_triangle_id < *triangle_id => {
+                        old_inter_it.next();
+                    }
+                    _ => break,
+                }
+            }
+
+            if old_inter_it.peek() != Some(triangle_id) {
+            } else {
+                old_inter_it.next();
+            };
+        }
+
+        let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
+        let (proximity_detector, mut workspace2) = ctxt
+            .dispatcher
+            .dispatch_primitives(&triangle1, collider2.shape());
+
+        let mut ctxt2 = PrimitiveProximityDetectionContext {
+            prediction_distance: ctxt.prediction_distance,
+            collider1,
+            collider2,
+            shape1: &triangle1,
+            shape2: collider2.shape(),
+            position1: collider1.position(),
+            position2: collider2.position(),
+            workspace: workspace2.as_mut().map(|w| &mut **w),
+        };
+
+        match (proximity_detector.detect_proximity)(&mut ctxt2) {
+            Proximity::Intersecting => return Proximity::Intersecting,
+            Proximity::WithinMargin => best_proximity = Proximity::WithinMargin,
+            Proximity::Disjoint => {}
+        }
+    }
+
+    best_proximity
+}

src/geometry/sat.rs

@@ -0,0 +1,294 @@
+use crate::geometry::{cuboid, Cuboid, Polygon, Triangle};
+use crate::math::{Isometry, Point, Vector, DIM};
+use crate::utils::WSign;
+use na::Unit;
+use ncollide::shape::{Segment, SupportMap};
+
+pub fn polygon_polygon_compute_separation_features(
+    p1: &Polygon,
+    p2: &Polygon,
+    m12: &Isometry<f32>,
+) -> (f32, usize, usize) {
+    let mut max_separation = -f32::MAX;
+    let mut separation_features = (0, 0);
+
+    for (i, (p1, n1)) in p1.vertices.iter().zip(p1.normals.iter()).enumerate() {
+        let j = p2.support_point(&m12.inverse_transform_vector(&-n1));
+        let dpt = m12 * p2.vertices[j] - p1;
+        let separation = dpt.dot(n1);
+
+        if separation > max_separation {
+            max_separation = separation;
+            separation_features = (i, j);
+        }
+    }
+
+    (max_separation, separation_features.0, separation_features.1)
+}
+
+#[cfg(feature = "dim3")]
+pub fn cuboid_cuboid_compute_separation_wrt_local_line(
+    cube1: &Cuboid,
+    cube2: &Cuboid,
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+    axis1: &Vector<f32>,
+) -> (f32, Vector<f32>) {
+    let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0);
+    let axis1 = axis1 * signum;
+    let local_pt1 = cuboid::local_support_point(cube1, axis1);
+    let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1);
+    let pt2 = pos12 * local_pt2;
+    let separation = (pt2 - local_pt1).dot(&axis1);
+    (separation, axis1)
+}
+
+#[cfg(feature = "dim3")]
+pub fn cuboid_cuboid_find_local_separating_edge_twoway(
+    cube1: &Cuboid,
+    cube2: &Cuboid,
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    use approx::AbsDiffEq;
+    let mut best_separation = -std::f32::MAX;
+    let mut best_dir = Vector::zeros();
+
+    let x2 = pos12 * Vector::x();
+    let y2 = pos12 * Vector::y();
+    let z2 = pos12 * Vector::z();
+
+    // We have 3 * 3 = 9 axii to test.
+    let axii = [
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -x2.z, x2.y),
+        Vector::new(x2.z, 0.0, -x2.x),
+        Vector::new(-x2.y, x2.x, 0.0),
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -y2.z, y2.y),
+        Vector::new(y2.z, 0.0, -y2.x),
+        Vector::new(-y2.y, y2.x, 0.0),
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -z2.z, z2.y),
+        Vector::new(z2.z, 0.0, -z2.x),
+        Vector::new(-z2.y, z2.x, 0.0),
+    ];
+
+    for axis1 in &axii {
+        let norm1 = axis1.norm();
+        if norm1 > f32::default_epsilon() {
+            let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line(
+                cube1,
+                cube2,
+                pos12,
+                pos21,
+                &(axis1 / norm1),
+            );
+
+            if separation > best_separation {
+                best_separation = separation;
+                best_dir = axis1;
+            }
+        }
+    }
+
+    (best_separation, best_dir)
+}
+
+pub fn cuboid_cuboid_find_local_separating_normal_oneway(
+    cube1: &Cuboid,
+    cube2: &Cuboid,
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let mut best_separation = -std::f32::MAX;
+    let mut best_dir = Vector::zeros();
+
+    for i in 0..DIM {
+        let sign = pos12.translation.vector[i].copy_sign_to(1.0);
+        let axis1 = Vector::ith(i, sign);
+        let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1);
+        let pt2 = pos12 * local_pt2;
+        let separation = pt2[i] * sign - cube1.half_extents[i];
+
+        if separation > best_separation {
+            best_separation = separation;
+            best_dir = axis1;
+        }
+    }
+
+    (best_separation, best_dir)
+}
+
+/*
+ *
+ *
+ * Triangles.
+ *
+ *
+ */
+
+#[cfg(feature = "dim3")]
+pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
+    cube1: &Cuboid,
+    shape2: &S,
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+    axis1: &Unit<Vector<f32>>,
+) -> (f32, Unit<Vector<f32>>) {
+    let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0);
+    let axis1 = Unit::new_unchecked(**axis1 * signum);
+    let local_pt1 = cuboid::local_support_point(cube1, *axis1);
+    let local_pt2 = shape2.local_support_point_toward(&(pos21 * -axis1));
+    let pt2 = pos12 * local_pt2;
+    let separation = (pt2 - local_pt1).dot(&axis1);
+    (separation, axis1)
+}
+
+#[cfg(feature = "dim3")]
+pub fn cube_support_map_find_local_separating_edge_twoway(
+    cube1: &Cuboid,
+    shape2: &impl SupportMap<f32>,
+    axii: &[Vector<f32>],
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    use approx::AbsDiffEq;
+    let mut best_separation = -std::f32::MAX;
+    let mut best_dir = Vector::zeros();
+
+    for axis1 in axii {
+        if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) {
+            let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line(
+                cube1, shape2, pos12, pos21, &axis1,
+            );
+
+            if separation > best_separation {
+                best_separation = separation;
+                best_dir = *axis1;
+            }
+        }
+    }
+
+    (best_separation, best_dir)
+}
+
+#[cfg(feature = "dim3")]
+pub fn cube_triangle_find_local_separating_edge_twoway(
+    cube1: &Cuboid,
+    triangle2: &Triangle,
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let x2 = pos12 * (triangle2.b - triangle2.a);
+    let y2 = pos12 * (triangle2.c - triangle2.b);
+    let z2 = pos12 * (triangle2.a - triangle2.c);
+
+    // We have 3 * 3 = 3 axii to test.
+    let axii = [
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -x2.z, x2.y),
+        Vector::new(x2.z, 0.0, -x2.x),
+        Vector::new(-x2.y, x2.x, 0.0),
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -y2.z, y2.y),
+        Vector::new(y2.z, 0.0, -y2.x),
+        Vector::new(-y2.y, y2.x, 0.0),
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -z2.z, z2.y),
+        Vector::new(z2.z, 0.0, -z2.x),
+        Vector::new(-z2.y, z2.x, 0.0),
+    ];
+
+    cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21)
+}
+
+#[cfg(feature = "dim3")]
+pub fn cube_segment_find_local_separating_edge_twoway(
+    cube1: &Cuboid,
+    segment2: &Segment<f32>,
+    pos12: &Isometry<f32>,
+    pos21: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let x2 = pos12 * (segment2.b - segment2.a);
+
+    let axii = [
+        // Vector::{x, y ,z}().cross(y2)
+        Vector::new(0.0, -x2.z, x2.y),
+        Vector::new(x2.z, 0.0, -x2.x),
+        Vector::new(-x2.y, x2.x, 0.0),
+    ];
+
+    cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21)
+}
+
+pub fn cube_support_map_find_local_separating_normal_oneway<S: SupportMap<f32>>(
+    cube1: &Cuboid,
+    shape2: &S,
+    pos12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let mut best_separation = -std::f32::MAX;
+    let mut best_dir = Vector::zeros();
+
+    for i in 0..DIM {
+        for sign in &[-1.0, 1.0] {
+            let axis1 = Vector::ith(i, *sign);
+            let pt2 = shape2.support_point_toward(&pos12, &Unit::new_unchecked(-axis1));
+            let separation = pt2[i] * *sign - cube1.half_extents[i];
+
+            if separation > best_separation {
+                best_separation = separation;
+                best_dir = axis1;
+            }
+        }
+    }
+
+    (best_separation, best_dir)
+}
+
+// NOTE: this only works with cuboid on the rhs because it has its symmetry origin at zero
+// (therefore we can check only one normal direction).
+pub fn point_cuboid_find_local_separating_normal_oneway(
+    point1: Point<f32>,
+    normal1: Option<Unit<Vector<f32>>>,
+    shape2: &Cuboid,
+    pos12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    let mut best_separation = -std::f32::MAX;
+    let mut best_dir = Vector::zeros();
+
+    if let Some(normal1) = normal1 {
+        let axis1 = if (pos12.translation.vector - point1.coords).dot(&normal1) >= 0.0 {
+            normal1
+        } else {
+            -normal1
+        };
+
+        let pt2 = shape2.support_point_toward(&pos12, &-axis1);
+        let separation = (pt2 - point1).dot(&axis1);
+
+        if separation > best_separation {
+            best_separation = separation;
+            best_dir = *axis1;
+        }
+    }
+
+    (best_separation, best_dir)
+}
+
+pub fn triangle_cuboid_find_local_separating_normal_oneway(
+    triangle1: &Triangle,
+    shape2: &Cuboid,
+    pos12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    point_cuboid_find_local_separating_normal_oneway(triangle1.a, triangle1.normal(), shape2, pos12)
+}
+
+#[cfg(feature = "dim2")]
+pub fn segment_cuboid_find_local_separating_normal_oneway(
+    segment1: &Segment<f32>,
+    shape2: &Cuboid,
+    pos12: &Isometry<f32>,
+) -> (f32, Vector<f32>) {
+    point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12)
+}

src/geometry/triangle.rs

@@ -0,0 +1,9 @@
+#[cfg(feature = "dim2")]
+use crate::geometry::{CuboidFeatureFace, Triangle};
+#[cfg(feature = "dim2")]
+use crate::math::Vector;
+
+#[cfg(feature = "dim2")]
+pub fn support_face(_triangle: &Triangle, _local_dir: Vector<f32>) -> CuboidFeatureFace {
+    unimplemented!()
+}

src/geometry/trimesh.rs

@@ -0,0 +1,122 @@
+use crate::geometry::{Triangle, WAABBHierarchy};
+use crate::math::{Isometry, Point};
+use na::Point3;
+use ncollide::bounding_volume::{HasBoundingVolume, AABB};
+
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A triangle mesh.
+pub struct Trimesh {
+    waabb_tree: WAABBHierarchy,
+    aabb: AABB<f32>,
+    vertices: Vec<Point<f32>>,
+    indices: Vec<Point3<u32>>,
+}
+
+impl Trimesh {
+    /// Creates a new triangle mesh from a vertex buffer and an index buffer.
+    pub fn new(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
+        assert!(
+            vertices.len() > 1,
+            "A triangle mesh must contain at least one point."
+        );
+        assert!(
+            indices.len() > 1,
+            "A triangle mesh must contain at least one triangle."
+        );
+
+        // z-sort the indices.
+        //        indices.sort_unstable_by(|idx, jdx| {
+        //            let ti = Triangle::new(
+        //                vertices[idx[0] as usize],
+        //                vertices[idx[1] as usize],
+        //                vertices[idx[2] as usize],
+        //            );
+        //            let tj = Triangle::new(
+        //                vertices[jdx[0] as usize],
+        //                vertices[jdx[1] as usize],
+        //                vertices[jdx[2] as usize],
+        //            );
+        //            let center_i = (ti.a.coords + ti.b.coords + ti.c.coords) / 3.0;
+        //            let center_j = (tj.a.coords + tj.b.coords + tj.c.coords) / 3.0;
+        //            crate::geometry::z_cmp_floats(center_i.as_slice(), center_j.as_slice())
+        //                .unwrap_or(std::cmp::Ordering::Equal)
+        //        });
+        let aabb = AABB::from_points(&vertices);
+
+        let aabbs: Vec<_> = indices
+            .iter()
+            .map(|idx| {
+                Triangle::new(
+                    vertices[idx[0] as usize],
+                    vertices[idx[1] as usize],
+                    vertices[idx[2] as usize],
+                )
+                .local_bounding_volume()
+            })
+            .collect();
+
+        let waabb_tree = WAABBHierarchy::new(&aabbs);
+
+        Self {
+            waabb_tree,
+            aabb,
+            vertices,
+            indices,
+        }
+    }
+
+    /// Compute the axis-aligned bounding box of this triangle mesh.
+    pub fn aabb(&self, pos: &Isometry<f32>) -> AABB<f32> {
+        self.aabb.transform_by(pos)
+    }
+
+    pub(crate) fn waabbs(&self) -> &WAABBHierarchy {
+        &self.waabb_tree
+    }
+
+    /// The number of triangles forming this mesh.
+    pub fn num_triangles(&self) -> usize {
+        self.indices.len()
+    }
+
+    /// An iterator through all the triangles of this mesh.
+    pub fn triangles(&self) -> impl Iterator<Item = Triangle> + '_ {
+        self.indices.iter().map(move |ids| {
+            Triangle::new(
+                self.vertices[ids.x as usize],
+                self.vertices[ids.y as usize],
+                self.vertices[ids.z as usize],
+            )
+        })
+    }
+
+    /// Get the `i`-th triangle of this mesh.
+    pub fn triangle(&self, i: usize) -> Triangle {
+        let idx = self.indices[i];
+        Triangle::new(
+            self.vertices[idx.x as usize],
+            self.vertices[idx.y as usize],
+            self.vertices[idx.z as usize],
+        )
+    }
+
+    /// The vertex buffer of this mesh.
+    pub fn vertices(&self) -> &[Point<f32>] {
+        &self.vertices[..]
+    }
+
+    /// The index buffer of this mesh.
+    pub fn indices(&self) -> &[Point3<u32>] {
+        &self.indices
+    }
+
+    /// A flat view of the index buffer of this mesh.
+    pub fn flat_indices(&self) -> &[u32] {
+        unsafe {
+            let len = self.indices.len() * 3;
+            let data = self.indices.as_ptr() as *const u32;
+            std::slice::from_raw_parts(data, len)
+        }
+    }
+}

src/geometry/waabb.rs

@@ -0,0 +1,116 @@
+#[cfg(feature = "serde-serialize")]
+use crate::math::DIM;
+use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH};
+use ncollide::bounding_volume::AABB;
+use simba::simd::{SimdPartialOrd, SimdValue};
+
+#[derive(Debug, Copy, Clone)]
+pub(crate) struct WAABB {
+    pub mins: Point<SimdFloat>,
+    pub maxs: Point<SimdFloat>,
+}
+
+#[cfg(feature = "serde-serialize")]
+impl serde::Serialize for WAABB {
+    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
+    where
+        S: serde::Serializer,
+    {
+        use serde::ser::SerializeStruct;
+
+        let mins: Point<[f32; SIMD_WIDTH]> = Point::from(
+            self.mins
+                .coords
+                .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
+        );
+        let maxs: Point<[f32; SIMD_WIDTH]> = Point::from(
+            self.maxs
+                .coords
+                .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]),
+        );
+        let mut waabb = serializer.serialize_struct("WAABB", 2)?;
+        waabb.serialize_field("mins", &mins)?;
+        waabb.serialize_field("maxs", &maxs)?;
+        waabb.end()
+    }
+}
+
+#[cfg(feature = "serde-serialize")]
+impl<'de> serde::Deserialize<'de> for WAABB {
+    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
+    where
+        D: serde::Deserializer<'de>,
+    {
+        struct Visitor {};
+        impl<'de> serde::de::Visitor<'de> for Visitor {
+            type Value = WAABB;
+            fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
+                write!(
+                    formatter,
+                    "two arrays containing at least {} floats",
+                    SIMD_WIDTH * DIM * 2
+                )
+            }
+
+            fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
+            where
+                A: serde::de::SeqAccess<'de>,
+            {
+                let mins: Point<[f32; SIMD_WIDTH]> = seq
+                    .next_element()?
+                    .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
+                let maxs: Point<[f32; SIMD_WIDTH]> = seq
+                    .next_element()?
+                    .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?;
+                let mins = Point::from(mins.coords.map(|e| SimdFloat::from(e)));
+                let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e)));
+                Ok(WAABB { mins, maxs })
+            }
+        }
+
+        deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {})
+    }
+}
+
+impl WAABB {
+    pub fn new(mins: Point<SimdFloat>, maxs: Point<SimdFloat>) -> Self {
+        Self { mins, maxs }
+    }
+
+    pub fn splat(aabb: AABB<f32>) -> Self {
+        Self {
+            mins: Point::splat(aabb.mins),
+            maxs: Point::splat(aabb.maxs),
+        }
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool {
+        self.mins.x.simd_le(other.maxs.x)
+            & other.mins.x.simd_le(self.maxs.x)
+            & self.mins.y.simd_le(other.maxs.y)
+            & other.mins.y.simd_le(self.maxs.y)
+    }
+
+    #[cfg(feature = "dim3")]
+    pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool {
+        self.mins.x.simd_le(other.maxs.x)
+            & other.mins.x.simd_le(self.maxs.x)
+            & self.mins.y.simd_le(other.maxs.y)
+            & other.mins.y.simd_le(self.maxs.y)
+            & self.mins.z.simd_le(other.maxs.z)
+            & other.mins.z.simd_le(self.maxs.z)
+    }
+}
+
+impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
+    fn from(aabbs: [AABB<f32>; SIMD_WIDTH]) -> Self {
+        let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
+        let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH];
+
+        WAABB {
+            mins: Point::from(mins),
+            maxs: Point::from(maxs),
+        }
+    }
+}

src/geometry/z_order.rs

@@ -0,0 +1,70 @@
+use num_traits::float::FloatCore;
+use std::cmp::Ordering;
+
+#[allow(dead_code)] // We don't use this currently, but migth in the future.
+pub fn z_cmp_ints(lhs: &[usize], rhs: &[usize]) -> Ordering {
+    assert_eq!(
+        lhs.len(),
+        rhs.len(),
+        "Cannot compare array with different lengths."
+    );
+
+    let mut msd = 0;
+
+    for dim in 1..rhs.len() {
+        if less_msb(lhs[msd] ^ rhs[msd], lhs[dim] ^ rhs[dim]) {
+            msd = dim;
+        }
+    }
+
+    lhs[msd].cmp(&rhs[msd])
+}
+
+fn less_msb(x: usize, y: usize) -> bool {
+    x < y && x < (x ^ y)
+}
+
+// Fast construction of k-Nearest Neighbor Graphs for Point Clouds
+// Michael Connor, Piyush Kumar
+// Algorithm 1
+//
+// http://compgeom.com/~piyush/papers/tvcg_stann.pdf
+pub fn z_cmp_floats(p1: &[f32], p2: &[f32]) -> Option<Ordering> {
+    assert_eq!(
+        p1.len(),
+        p2.len(),
+        "Cannot compare array with different lengths."
+    );
+    let mut x = 0;
+    let mut dim = 0;
+
+    for j in 0..p1.len() {
+        let y = xor_msb_float(p1[j], p2[j]);
+        if x < y {
+            x = y;
+            dim = j;
+        }
+    }
+
+    p1[dim].partial_cmp(&p2[dim])
+}
+
+fn xor_msb_float(fa: f32, fb: f32) -> i16 {
+    let (mantissa1, exponent1, _sign1) = fa.integer_decode();
+    let (mantissa2, exponent2, _sign2) = fb.integer_decode();
+    let x = exponent1; // To keep the same notation as the paper.
+    let y = exponent2; // To keep the same notation as the paper.
+
+    if x == y {
+        let z = msdb(mantissa1, mantissa2);
+        x - z
+    } else if y < x {
+        x
+    } else {
+        y
+    }
+}
+
+fn msdb(x: u64, y: u64) -> i16 {
+    64i16 - (x ^ y).leading_zeros() as i16
+}