First public release of Rapier.

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);
+    }
+}