First public release of Rapier.v0.1.0

15 files changed, 1998 insertions, 0 deletions

diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs
new file mode 100644
index 0000000..4889aaf
--- /dev/null
+++ b/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);
+}
diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs
new file mode 100644
index 0000000..a187832
--- /dev/null
+++ b/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();
+        }
+    }
+}
diff --git a/src/geometry/contact_generator/ball_polygon_contact_generator.rs b/src/geometry/contact_generator/ball_polygon_contact_generator.rs
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/src/geometry/contact_generator/ball_polygon_contact_generator.rs
diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs
new file mode 100644
index 0000000..3800ce6
--- /dev/null
+++ b/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();
+        }
+    }
+}
diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
new file mode 100644
index 0000000..8c846e0
--- /dev/null
+++ b/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)
+            }
+        }
+    }
+}
diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs
new file mode 100644
index 0000000..9dd0050
--- /dev/null
+++ b/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,
+        }
+    }
+}
diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
new file mode 100644
index 0000000..a7857a1
--- /dev/null
+++ b/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>,
+