First public release of Rapier.v0.1.0

12 files changed, 853 insertions, 0 deletions

diff --git a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs
new file mode 100644
index 0000000..2106c9f
--- /dev/null
+++ b/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
+    }
+}
diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs
new file mode 100644
index 0000000..b00337d
--- /dev/null
+++ b/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
+    }
+}
diff --git a/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs b/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs
diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs
new file mode 100644
index 0000000..b68ebf9
--- /dev/null
+++ b/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
+        }
+    }
+}
diff --git a/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs
diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs
new file mode 100644
index 0000000..12f3b4a
--- /dev/null
+++ b/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
+        }
+    }
+}
diff --git a/src/geometry/proximity_detector/mod.rs b/src/geometry/proximity_detector/mod.rs
new file mode 100644
index 0000000..a99372f
--- /dev/null
+++ b/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;
diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs
new file mode 100644
index 0000000..f0e049f
--- /dev/null
+++ b/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
+        }
+    }
+}
diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs
new file mode 100644
index 0000000..76e8cd7
--- /dev/null
+++ b/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,
+        }
+    }
+}
diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs
new file mode 100644
index 0000000..6d6b4c5
--- /dev/null
+++ b/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)
+            }
+        }
+    }
+}
diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs
new file mode 100644
index 0000000..3dd7381
--- /dev/null
+++ b/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 ma