First public release of Rapier.

src/counters/ccd_counters.rs

@@ -0,0 +1,49 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to continuous collision detection (CCD).
+#[derive(Default, Clone, Copy)]
+pub struct CCDCounters {
+    /// The number of substeps actually performed by the CCD resolution.
+    pub num_substeps: usize,
+    /// The total time spent for TOI computation in the CCD resolution.
+    pub toi_computation_time: Timer,
+    /// The total time spent for force computation and integration in the CCD resolution.
+    pub solver_time: Timer,
+    /// The total time spent by the broad-phase in the CCD resolution.
+    pub broad_phase_time: Timer,
+    /// The total time spent by the narrow-phase in the CCD resolution.
+    pub narrow_phase_time: Timer,
+}
+
+impl CCDCounters {
+    /// Creates a new counter initialized to zero.
+    pub fn new() -> Self {
+        CCDCounters {
+            num_substeps: 0,
+            toi_computation_time: Timer::new(),
+            solver_time: Timer::new(),
+            broad_phase_time: Timer::new(),
+            narrow_phase_time: Timer::new(),
+        }
+    }
+
+    /// Resets this counter to 0.
+    pub fn reset(&mut self) {
+        self.num_substeps = 0;
+        self.toi_computation_time.reset();
+        self.solver_time.reset();
+        self.broad_phase_time.reset();
+        self.narrow_phase_time.reset();
+    }
+}
+
+impl Display for CCDCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Number of substeps: {}", self.num_substeps)?;
+        writeln!(f, "TOI computation time: {}", self.toi_computation_time)?;
+        writeln!(f, "Constraints solver time: {}", self.solver_time)?;
+        writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
+        writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
+    }
+}

src/counters/collision_detection_counters.rs

@@ -0,0 +1,32 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to collision detection.
+#[derive(Default, Clone, Copy)]
+pub struct CollisionDetectionCounters {
+    /// Number of contact pairs detected.
+    pub ncontact_pairs: usize,
+    /// Time spent for the broad-phase of the collision detection.
+    pub broad_phase_time: Timer,
+    /// Time spent for the narrow-phase of the collision detection.
+    pub narrow_phase_time: Timer,
+}
+
+impl CollisionDetectionCounters {
+    /// Creates a new counter initialized to zero.
+    pub fn new() -> Self {
+        CollisionDetectionCounters {
+            ncontact_pairs: 0,
+            broad_phase_time: Timer::new(),
+            narrow_phase_time: Timer::new(),
+        }
+    }
+}
+
+impl Display for CollisionDetectionCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?;
+        writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?;
+        writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time)
+    }
+}

src/counters/mod.rs

@@ -0,0 +1,225 @@
+//! Counters for benchmarking various parts of the physics engine.
+
+use std::fmt::{Display, Formatter, Result};
+
+pub use self::ccd_counters::CCDCounters;
+pub use self::collision_detection_counters::CollisionDetectionCounters;
+pub use self::solver_counters::SolverCounters;
+pub use self::stages_counters::StagesCounters;
+pub use self::timer::Timer;
+
+mod ccd_counters;
+mod collision_detection_counters;
+mod solver_counters;
+mod stages_counters;
+mod timer;
+
+/// Aggregation of all the performances counters tracked by nphysics.
+#[derive(Clone, Copy)]
+pub struct Counters {
+    /// Whether thi counter is enabled or not.
+    pub enabled: bool,
+    /// Timer for a whole timestep.
+    pub step_time: Timer,
+    /// Timer used for debugging.
+    pub custom: Timer,
+    /// Counters of every satge of one time step.
+    pub stages: StagesCounters,
+    /// Counters of the collision-detection stage.
+    pub cd: CollisionDetectionCounters,
+    /// Counters of the constraints resolution and force computation stage.
+    pub solver: SolverCounters,
+    /// Counters for the CCD resolution stage.
+    pub ccd: CCDCounters,
+}
+
+impl Counters {
+    /// Create a new set of counters initialized to wero.
+    pub fn new(enabled: bool) -> Self {
+        Counters {
+            enabled,
+            step_time: Timer::new(),
+            custom: Timer::new(),
+            stages: StagesCounters::new(),
+            cd: CollisionDetectionCounters::new(),
+            solver: SolverCounters::new(),
+            ccd: CCDCounters::new(),
+        }
+    }
+
+    /// Enable all the counters.
+    pub fn enable(&mut self) {
+        self.enabled = true;
+    }
+
+    /// Return `true` if the counters are enabled.
+    pub fn enabled(&self) -> bool {
+        self.enabled
+    }
+
+    /// Disable all the counters.
+    pub fn disable(&mut self) {
+        self.enabled = false;
+    }
+
+    /// Notify that the time-step has started.
+    pub fn step_started(&mut self) {
+        if self.enabled {
+            self.step_time.start();
+        }
+    }
+
+    /// Notfy that the time-step has finished.
+    pub fn step_completed(&mut self) {
+        if self.enabled {
+            self.step_time.pause();
+        }
+    }
+
+    /// Total time spent for one  of the physics engine.
+    pub fn step_time(&self) -> f64 {
+        self.step_time.time()
+    }
+
+    /// Notify that the custom operation has started.
+    pub fn custom_started(&mut self) {
+        if self.enabled {
+            self.custom.start();
+        }
+    }
+
+    /// Notfy that the custom operation has finished.
+    pub fn custom_completed(&mut self) {
+        if self.enabled {
+            self.custom.pause();
+        }
+    }
+
+    /// Total time of a custom event.
+    pub fn custom_time(&self) -> f64 {
+        self.custom.time()
+    }
+
+    /// Set the number of constraints generated.
+    pub fn set_nconstraints(&mut self, n: usize) {
+        self.solver.nconstraints = n;
+    }
+
+    /// Set the number of contacts generated.
+    pub fn set_ncontacts(&mut self, n: usize) {
+        self.solver.ncontacts = n;
+    }
+
+    /// Set the number of contact pairs generated.
+    pub fn set_ncontact_pairs(&mut self, n: usize) {
+        self.cd.ncontact_pairs = n;
+    }
+}
+
+macro_rules! measure_method {
+    ($started:ident, $stopped:ident, $time:ident, $info:ident. $timer:ident) => {
+        impl Counters {
+            /// Start this timer.
+            pub fn $started(&mut self) {
+                if self.enabled {
+                    self.$info.$timer.start()
+                }
+            }
+
+            /// Stop this timer.
+            pub fn $stopped(&mut self) {
+                if self.enabled {
+                    self.$info.$timer.pause()
+                }
+            }
+
+            /// Gets the time elapsed for this timer.
+            pub fn $time(&self) -> f64 {
+                if self.enabled {
+                    self.$info.$timer.time()
+                } else {
+                    0.0
+                }
+            }
+        }
+    };
+}
+
+measure_method!(
+    update_started,
+    update_completed,
+    update_time,
+    stages.update_time
+);
+measure_method!(
+    collision_detection_started,
+    collision_detection_completed,
+    collision_detection_time,
+    stages.collision_detection_time
+);
+measure_method!(
+    island_construction_started,
+    island_construction_completed,
+    island_construction_time,
+    stages.island_construction_time
+);
+measure_method!(
+    solver_started,
+    solver_completed,
+    solver_time,
+    stages.solver_time
+);
+measure_method!(ccd_started, ccd_completed, ccd_time, stages.ccd_time);
+
+measure_method!(
+    assembly_started,
+    assembly_completed,
+    assembly_time,
+    solver.velocity_assembly_time
+);
+measure_method!(
+    velocity_resolution_started,
+    velocity_resolution_completed,
+    velocity_resolution_time,
+    solver.velocity_resolution_time
+);
+measure_method!(
+    velocity_update_started,
+    velocity_update_completed,
+    velocity_update_time,
+    solver.velocity_update_time
+);
+measure_method!(
+    position_resolution_started,
+    position_resolution_completed,
+    position_resolution_time,
+    solver.position_resolution_time
+);
+measure_method!(
+    broad_phase_started,
+    broad_phase_completed,
+    broad_phase_time,
+    cd.broad_phase_time
+);
+measure_method!(
+    narrow_phase_started,
+    narrow_phase_completed,
+    narrow_phase_time,
+    cd.narrow_phase_time
+);
+
+impl Display for Counters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Total timestep time: {}", self.step_time)?;
+        self.stages.fmt(f)?;
+        self.cd.fmt(f)?;
+        self.solver.fmt(f)?;
+        writeln!(f, "Custom timer: {}", self.custom)
+    }
+}
+
+impl Default for Counters {
+    fn default() -> Self {
+        Self::new(false)
+    }
+}

src/counters/solver_counters.rs

@@ -0,0 +1,67 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to constraints resolution.
+#[derive(Default, Clone, Copy)]
+pub struct SolverCounters {
+    /// Number of constraints generated.
+    pub nconstraints: usize,
+    /// Number of contacts found.
+    pub ncontacts: usize,
+    /// Time spent for the resolution of the constraints (force computation).
+    pub velocity_resolution_time: Timer,
+    /// Time spent for the assembly of all the velocity constraints.
+    pub velocity_assembly_time: Timer,
+    /// Time spent for the update of the velocity of the bodies.
+    pub velocity_update_time: Timer,
+    /// Time spent for the assembly of all the position constraints.
+    pub position_assembly_time: Timer,
+    /// Time spent for the update of the position of the bodies.
+    pub position_resolution_time: Timer,
+}
+
+impl SolverCounters {
+    /// Creates a new counter initialized to zero.
+    pub fn new() -> Self {
+        SolverCounters {
+            nconstraints: 0,
+            ncontacts: 0,
+            velocity_assembly_time: Timer::new(),
+            velocity_resolution_time: Timer::new(),
+            velocity_update_time: Timer::new(),
+            position_assembly_time: Timer::new(),
+            position_resolution_time: Timer::new(),
+        }
+    }
+
+    /// Reset all the counters to zero.
+    pub fn reset(&mut self) {
+        self.nconstraints = 0;
+        self.ncontacts = 0;
+        self.velocity_resolution_time.reset();
+        self.velocity_assembly_time.reset();
+        self.velocity_update_time.reset();
+        self.position_assembly_time.reset();
+        self.position_resolution_time.reset();
+    }
+}
+
+impl Display for SolverCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Number of contacts: {}", self.ncontacts)?;
+        writeln!(f, "Number of constraints: {}", self.nconstraints)?;
+        writeln!(f, "Velocity assembly time: {}", self.velocity_assembly_time)?;
+        writeln!(
+            f,
+            "Velocity resolution time: {}",
+            self.velocity_resolution_time
+        )?;
+        writeln!(f, "Velocity update time: {}", self.velocity_update_time)?;
+        writeln!(f, "Position assembly time: {}", self.position_assembly_time)?;
+        writeln!(
+            f,
+            "Position resolution time: {}",
+            self.position_resolution_time
+        )
+    }
+}

src/counters/stages_counters.rs

@@ -0,0 +1,48 @@
+use crate::counters::Timer;
+use std::fmt::{Display, Formatter, Result};
+
+/// Performance counters related to each stage of the time step.
+#[derive(Default, Clone, Copy)]
+pub struct StagesCounters {
+    /// Time spent for updating the kinematic and dynamics of every body.
+    pub update_time: Timer,
+    /// Total time spent for the collision detection (including both broad- and narrow- phases).
+    pub collision_detection_time: Timer,
+    /// Time spent for the computation of collision island and body activation/deactivation (sleeping).
+    pub island_construction_time: Timer,
+    /// Total time spent for the constraints resolution and position update.t
+    pub solver_time: Timer,
+    /// Total time spent for CCD and CCD resolution.
+    pub ccd_time: Timer,
+}
+
+impl StagesCounters {
+    /// Create a new counter intialized to zero.
+    pub fn new() -> Self {
+        StagesCounters {
+            update_time: Timer::new(),
+            collision_detection_time: Timer::new(),
+            island_construction_time: Timer::new(),
+            solver_time: Timer::new(),
+            ccd_time: Timer::new(),
+        }
+    }
+}
+
+impl Display for StagesCounters {
+    fn fmt(&self, f: &mut Formatter) -> Result {
+        writeln!(f, "Update time: {}", self.update_time)?;
+        writeln!(
+            f,
+            "Collision detection time: {}",
+            self.collision_detection_time
+        )?;
+        writeln!(
+            f,
+            "Island construction time: {}",
+            self.island_construction_time
+        )?;
+        writeln!(f, "Solver time: {}", self.solver_time)?;
+        writeln!(f, "CCD time: {}", self.ccd_time)
+    }
+}

src/counters/timer.rs

@@ -0,0 +1,53 @@
+use std::fmt::{Display, Error, Formatter};
+
+/// A timer.
+#[derive(Copy, Clone, Debug, Default)]
+pub struct Timer {
+    time: f64,
+    start: Option<f64>,
+}
+
+impl Timer {
+    /// Creates a new timer initialized to zero and not started.
+    pub fn new() -> Self {
+        Timer {
+            time: 0.0,
+            start: None,
+        }
+    }
+
+    /// Resets the timer to 0.
+    pub fn reset(&mut self) {
+        self.time = 0.0
+    }
+
+    /// Start the timer.
+    pub fn start(&mut self) {
+        self.time = 0.0;
+        self.start = Some(instant::now());
+    }
+
+    /// Pause the timer.
+    pub fn pause(&mut self) {
+        if let Some(start) = self.start {
+            self.time += instant::now() - start;
+        }
+        self.start = None;
+    }
+
+    /// Resume the timer.
+    pub fn resume(&mut self) {
+        self.start = Some(instant::now());
+    }
+
+    /// The measured time between the last `.start()` and `.pause()` calls.
+    pub fn time(&self) -> f64 {
+        self.time
+    }
+}
+
+impl Display for Timer {
+    fn fmt(&self, f: &mut Formatter) -> Result<(), Error> {
+        write!(f, "{}s", self.time)
+    }
+}

src/data/graph.rs

@@ -0,0 +1,830 @@
+// This is basically a stripped down version of petgraph's UnGraph.
+// - It is not generic wrt. the index type (we always use u32).
+// - It preserves associated edge iteration order after Serialization/Deserialization.
+// - It is always undirected.
+//! A stripped-down version of petgraph's UnGraph.
+
+use std::cmp::max;
+use std::ops::{Index, IndexMut};
+
+/// Node identifier.
+#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct NodeIndex(u32);
+
+impl NodeIndex {
+    #[inline]
+    pub fn new(x: u32) -> Self {
+        NodeIndex(x)
+    }
+
+    #[inline]
+    pub fn index(self) -> usize {
+        self.0 as usize
+    }
+
+    #[inline]
+    pub fn end() -> Self {
+        NodeIndex(crate::INVALID_U32)
+    }
+
+    fn _into_edge(self) -> EdgeIndex {
+        EdgeIndex(self.0)
+    }
+}
+
+impl From<u32> for NodeIndex {
+    fn from(ix: u32) -> Self {
+        NodeIndex(ix)
+    }
+}
+
+/// Edge identifier.
+#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct EdgeIndex(u32);
+
+impl EdgeIndex {
+    #[inline]
+    pub fn new(x: u32) -> Self {
+        EdgeIndex(x)
+    }
+
+    #[inline]
+    pub fn index(self) -> usize {
+        self.0 as usize
+    }
+
+    /// An invalid `EdgeIndex` used to denote absence of an edge, for example
+    /// to end an adjacency list.
+    #[inline]
+    pub fn end() -> Self {
+        EdgeIndex(crate::INVALID_U32)
+    }
+
+    fn _into_node(self) -> NodeIndex {
+        NodeIndex(self.0)
+    }
+}
+
+impl From<u32> for EdgeIndex {
+    fn from(ix: u32) -> Self {
+        EdgeIndex(ix)
+    }
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub enum Direction {
+    Outgoing = 0,
+    Incoming = 1,
+}
+
+impl Direction {
+    fn opposite(self) -> Direction {
+        match self {
+            Direction::Outgoing => Direction::Incoming,
+            Direction::Incoming => Direction::Outgoing,
+        }
+    }
+}
+
+const DIRECTIONS: [Direction; 2] = [Direction::Outgoing, Direction::Incoming];
+
+/// The graph's node type.
+#[derive(Debug, Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Node<N> {
+    /// Associated node data.
+    pub weight: N,
+    /// Next edge in outgoing and incoming edge lists.
+    next: [EdgeIndex; 2],
+}
+
+/// The graph's edge type.
+#[derive(Debug, Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Edge<E> {
+    /// Associated edge data.
+    pub weight: E,
+    /// Next edge in outgoing and incoming edge lists.
+    next: [EdgeIndex; 2],
+    /// Start and End node index
+    node: [NodeIndex; 2],
+}
+
+impl<E> Edge<E> {
+    /// Return the source node index.
+    pub fn source(&self) -> NodeIndex {
+        self.node[0]
+    }
+
+    /// Return the target node index.
+    pub fn target(&self) -> NodeIndex {
+        self.node[1]
+    }
+}
+
+#[derive(Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct Graph<N, E> {
+    pub(crate) nodes: Vec<Node<N>>,
+    pub(crate) edges: Vec<Edge<E>>,
+}
+
+enum Pair<T> {
+    Both(T, T),
+    One(T),
+    None,
+}
+
+/// Get mutable references at index `a` and `b`.
+fn index_twice<T>(arr: &mut [T], a: usize, b: usize) -> Pair<&mut T> {
+    if max(a, b) >= arr.len() {
+        Pair::None
+    } else if a == b {
+        Pair::One(&mut arr[max(a, b)])
+    } else {
+        // safe because a, b are in bounds and distinct
+        unsafe {
+            let ar = &mut *(arr.get_unchecked_mut(a) as *mut _);
+            let br = &mut *(arr.get_unchecked_mut(b) as *mut _);
+            Pair::Both(ar, br)
+        }
+    }
+}
+
+impl<N, E> Graph<N, E> {
+    /// Create a new `Graph` with estimated capacity.
+    pub fn with_capacity(nodes: usize, edges: usize) -> Self {
+        Graph {
+            nodes: Vec::with_capacity(nodes),
+            edges: Vec::with_capacity(edges),
+        }
+    }
+
+    /// Add a node (also called vertex) with associated data `weight` to the graph.
+    ///
+    /// Computes in **O(1)** time.
+    ///
+    /// Return the index of the new node.
+    ///
+    /// **Panics** if the Graph is at the maximum number of nodes for its index
+    /// type (N/A if usize).
+    pub fn add_node(&mut self, weight: N) -> NodeIndex {
+        let node = Node {
+            weight,
+            next: [EdgeIndex::end(), EdgeIndex::end()],
+        };
+        assert!(self.nodes.len() != crate::INVALID_USIZE);
+        let node_idx = NodeIndex::new(self.nodes.len() as u32);
+        self.nodes.push(node);
+        node_idx
+    }
+
+    /// Access the weight for node `a`.
+    ///
+    /// Also available with indexing syntax: `&graph[a]`.
+    pub fn node_weight(&self, a: NodeIndex) -> Option<&N> {
+        self.nodes.get(a.index()).map(|n| &n.weight)
+    }
+
+    /// Access the weight for edge `a`.
+    ///
+    /// Also available with indexing syntax: `&graph[a]`.
+    pub fn edge_weight(&self, a: EdgeIndex) -> Option<&E> {
+        self.edges.get(a.index()).map(|e| &e.weight)
+    }
+
+    /// Access the weight for edge `a` mutably.
+    ///
+    /// Also available with indexing syntax: `&mut graph[a]`.
+    pub fn edge_weight_mut(&mut self, a: EdgeIndex) -> Option<&mut E> {
+        self.edges.get_mut(a.index()).map(|e| &mut e.weight)
+    }
+
+    /// Add an edge from `a` to `b` to the graph, with its associated
+    /// data `weight`.
+    ///
+    /// Return the index of the new edge.
+    ///
+    /// Computes in **O(1)** time.
+    ///
+    /// **Panics** if any of the nodes don't exist.<br>
+    /// **Panics** if the Graph is at the maximum number of edges for its index
+    /// type (N/A if usize).
+    ///
+    /// **Note:** `Graph` allows adding parallel (“duplicate”) edges. If you want
+    /// to avoid this, use [`.update_edge(a, b, weight)`](#method.update_edge) instead.
+    pub fn add_edge(&mut self, a: NodeIndex, b: NodeIndex, weight: E) -> EdgeIndex {
+        assert!(self.edges.len() != crate::INVALID_USIZE);
+        let edge_idx = EdgeIndex::new(self.edges.len() as u32);
+        let mut edge = Edge {
+            weight,
+            node: [a, b],
+            next: [EdgeIndex::end(); 2],
+        };
+        match index_twice(&mut self.nodes, a.index(), b.index()) {
+            Pair::None => panic!("Graph::add_edge: node indices out of bounds"),
+            Pair::One(an) => {
+                edge.next = an.next;
+                an.next[0] = edge_idx;
+                an.next[1] = edge_idx;
+            }
+            Pair::Both(an, bn) => {
+                // a and b are different indices
+                edge.next = [an.next[0], bn.next[1]];
+                an.next[0] = edge_idx;
+                bn.next[1] = edge_idx;
+            }
+        }
+        self.edges.push(edge);
+        edge_idx
+    }
+
+    /// Access the source and target nodes for `e`.
+    pub fn edge_endpoints(&self, e: EdgeIndex) -> Option<(NodeIndex, NodeIndex)> {
+        self.edges
+            .get(e.index())
+            .map(|ed| (ed.source(), ed.target()))
+    }
+
+    /// Remove `a` from the graph if it exists, and return its weight.
+    /// If it doesn't exist in the graph, return `None`.
+    ///
+    /// Apart from `a`, this invalidates the last node index in the graph
+    /// (that node will adopt the removed node index). Edge indices are
+    /// invalidated as they would be following the removal of each edge
+    /// with an endpoint in `a`.
+    ///
+    /// Computes in **O(e')** time, where **e'** is the number of affected
+    /// edges, including *n* calls to `.remove_edge()` where *n* is the number
+    /// of edges with an endpoint in `a`, and including the edges with an
+    /// endpoint in the displaced node.
+    pub fn remove_node(&mut self, a: NodeIndex) -> Option<N> {
+        self.nodes.get(a.index())?;
+        for d in &DIRECTIONS {
+            let k = *d as usize;
+
+            // Remove all edges from and to this node.
+            loop {
+                let next = self.nodes[a.index()].next[k];
+                if next == EdgeIndex::end() {
+                    break;
+                }
+                let ret = self.remove_edge(next);
+                debug_assert!(ret.is_some());
+                let _ = ret;
+            }
+        }
+
+        // Use swap_remove -- only the swapped-in node is going to change
+        // NodeIndex, so we only have to walk its edges and update them.
+
+        let node = self.nodes.swap_remove(a.index());
+
+        // Find the edge lists of the node that had to relocate.
+        // It may be that no node had to relocate, then we are done already.
+        let swap_edges = match self.nodes.get(a.index()) {
+            None => return Some(node.weight),
+            Some(ed) => ed.next,
+        };
+
+        // The swapped element's old index
+        let old_index = NodeIndex::new(self.nodes.len() as u32);
+        let new_index = a;
+
+        // Adjust the starts of the out edges, and ends of the in edges.
+        for &d in &DIRECTIONS {
+            let k = d as usize;
+            let mut edges = edges_walker_mut(&mut self.edges, swap_edges[k], d);
+            while let Some(curedge) = edges.next_edge() {
+                debug_assert!(curedge.node[k] == old_index);
+                curedge.node[k] = new_index;
+            }
+        }
+        Some(node.weight)
+    }
+
+    /// For edge `e` with endpoints `edge_node`, replace links to it,
+    /// with links to `edge_next`.
+    fn change_edge_links(
+        &mut self,
+        edge_node: [NodeIndex; 2],
+        e: EdgeIndex,
+        edge_next: [EdgeIndex; 2],
+    ) {
+        for &d in &DIRECTIONS {
+            let k = d as usize;
+            let node = match self.nodes.get_mut(edge_node[k].index()) {
+                Some(r) => r,
+                None => {
+                    debug_assert!(
+                        false,
+                        "Edge's endpoint dir={:?} index={:?} not found",
+                        d, edge_node[k]
+                    );
+                    return;
+                }
+            };
+            let fst = node.next[k];
+            if fst == e {
+                //println!("Updating first edge 0 for node {}, set to {}", edge_node[0], edge_next[0]);
+                node.next[k] = edge_next[k];
+            } else {
+                let mut edges = edges_walker_mut(&mut self.edges, fst, d);
+                while let Some(curedge) = edges.next_edge() {
+                    if curedge.next[k] == e {
+                        curedge.next[k] = edge_next[k];
+                        break; // the edge can only be present once in the list.
+                    }
+                }
+            }
+        }
+    }
+
+    /// Remove an edge and return its edge weight, or `None` if it didn't exist.
+    ///
+    /// Apart from `e`, this invalidates the last edge index in the graph
+    /// (that edge will adopt the removed edge index).
+    ///
+    /// Computes in **O(e')** time, where **e'** is the size of four particular edge lists, for
+    /// the vertices of `e` and the vertices of another affected edge.
+    pub fn remove_edge(&mut self, e: EdgeIndex) -> Option<E> {
+        // every edge is part of two lists,
+        // outgoing and incoming edges.
+        // Remove it from both
+        let (edge_node, edge_next) = match self.edges.get(e.index()) {
+            None => return None,
+            Some(x) => (x.node, x.next),
+        };
+        // Remove the edge from its in and out lists by replacing it with
+        // a link to the next in the list.
+        self.change_edge_links(edge_node, e, edge_next);
+        self.remove_edge_adjust_indices(e)
+    }
+
+    fn remove_edge_adjust_indices(&mut self, e: EdgeIndex) -> Option<E> {
+        // swap_remove the edge -- only the removed edge
+        // and the edge swapped into place are affected and need updating
+        // indices.
+        let edge = self.edges.swap_remove(e.index());
+        let swap = match self.edges.get(e.index()) {
+            // no elment needed to be swapped.
+            None => return Some(edge.weight),
+            Some(ed) => ed.node,
+        };
+        let swapped_e = EdgeIndex::new(self.edges.len() as u32);
+
+        // Update the edge lists by replacing links to the old index by references to the new
+        // edge index.
+        self.change_edge_links(swap, swapped_e, [e, e]);
+        Some(edge.weight)
+    }
+
+    /// Return an iterator of all edges of `a`.
+    ///
+    /// - `Directed`: Outgoing edges from `a`.
+    /// - `Undirected`: All edges connected to `a`.
+    ///
+    /// Produces an empty iterator if the node doesn't exist.<br>
+    /// Iterator element type is `EdgeReference<E, Ix>`.
+    pub fn edges(&self, a: NodeIndex) -> Edges<E> {
+        self.edges_directed(a, Direction::Outgoing)
+    }
+
+    /// Return an iterator of all edges of `a`, in the specified direction.
+    ///
+    /// - `Directed`, `Outgoing`: All edges from `a`.
+    /// - `Directed`, `Incoming`: All edges to `a`.
+    /// - `Undirected`, `Outgoing`: All edges connected to `a`, with `a` being the source of each
+    ///   edge.
+    /// - `Undirected`, `Incoming`: All edges connected to `a`, with `a` being the target of each
+    ///   edge.
+    ///
+    /// Produces an empty iterator if the node `a` doesn't exist.<br>
+    /// Iterator element type is `EdgeReference<E, Ix>`.
+    pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges<E> {
+        Edges {
+            skip_start: a,
+            edges: &self.edges,
+            direction: dir,
+            next: match self.nodes.get(a.index()) {
+                None => [EdgeIndex::end(), EdgeIndex::end()],
+                Some(n) => n.next,
+            },
+        }
+    }
+
+    /*
+    /// Return an iterator over all the edges connecting `a` and `b`.
+    ///
+    /// - `Directed`: Outgoing edges from `a`.
+    /// - `Undirected`: All edges connected to `a`.
+    ///
+    /// Iterator element type is `EdgeReference<E, Ix>`.
+    pub fn edges_connecting(&self, a: NodeIndex, b: NodeIndex) -> EdgesConnecting<E, Ty, Ix> {
+        EdgesConnecting {
+            target_node: b,
+            edges: self.edges_directed(a, Direction::Outgoing),
+            ty: PhantomData,
+        }
+    }
+    */
+
+    /// Lookup an edge from `a` to `b`.
+    ///
+    /// Computes in **O(e')** time, where **e'** is the number of edges
+    /// connected to `a` (and `b`, if the graph edges are undirected).
+    pub fn find_edge(&self, a: NodeIndex, b: NodeIndex) -> Option<EdgeIndex> {
+        self.find_edge_undirected(a, b).map(|(ix, _)| ix)
+    }
+
+    /// Lookup an edge between `a` and `b`, in either direction.
+    ///
+    /// If the graph is undirected, then this is equivalent to `.find_edge()`.
+    ///
+    /// Return the edge index and its directionality, with `Outgoing` meaning
+    /// from `a` to `b` and `Incoming` the reverse,
+    /// or `None` if the edge does not exist.
+    pub fn find_edge_undirected(
+        &self,
+        a: NodeIndex,
+        b: NodeIndex,
+    ) -> Option<(EdgeIndex, Direction)> {
+        match self.nodes.get(a.index()) {
+            None => None,
+            Some(node) => self.find_edge_undirected_from_node(node, b),
+        }
+    }
+
+    fn find_edge_undirected_from_node(
+        &self,
+        node: &Node<N>,
+        b: NodeIndex,
+    ) -> Option<(EdgeIndex, Direction)> {
+        for &d in &DIRECTIONS {
+            let k = d as usize;
+            let mut edix = node.next[k];
+            while let Some(edge) = self.edges.get(edix.index()) {
+                if edge.node[1 - k] == b {
+                    return Some((edix, d));
+                }
+                edix = edge.next[k];
+            }
+        }
+        None
+    }
+
+    /// Access the internal node array.
+    pub fn raw_nodes(&self) -> &[Node<N>] {
+        &self.nodes
+    }
+
+    /// Access the internal edge array.
+    pub fn raw_edges(&self) -> &[Edge<E>] {
+        &self.edges
+    }
+
+    /// Accessor for data structure internals: the first edge in the given direction.
+    pub fn first_edge(&self, a: NodeIndex, dir: Direction) -> Option<EdgeIndex> {
+        match self.nodes.get(a.index()) {
+            None => None,
+            Some(node) => {
+                let edix = node.next[dir as usize];
+                if edix == EdgeIndex::end() {
+                    None
+                } else {
+                    Some(edix)
+                }
+            }
+        }
+    }
+
+    /// Accessor for data structure internals: the next edge for the given direction.
+    pub fn next_edge(&self, e: EdgeIndex, dir: Direction) -> Option<EdgeIndex> {
+        match self.edges.get(e.index()) {
+            None => None,
+            Some(node) => {
+                let edix = node.next[dir as usize];
+                if edix == EdgeIndex::end() {
+                    None
+                } else {
+                    Some(edix)
+                }
+            }
+        }
+    }
+}
+
+/// An iterator over either the nodes without edges to them or from them.
+pub struct Externals<'a, N: 'a> {
+    iter: std::iter::Enumerate<std::slice::Iter<'a, Node<N>>>,
+    dir: Direction,
+}
+
+impl<'a, N: 'a> Iterator for Externals<'a, N> {
+    type Item = NodeIndex;
+    fn next(&mut self) -> Option<NodeIndex> {
+        let k = self.dir as usize;
+        loop {
+            match self.iter.next() {
+                None => return None,
+                Some((index, node)) => {
+                    if node.next[k] == EdgeIndex::end() && node.next[1 - k] == EdgeIndex::end() {
+                        return Some(NodeIndex::new(index as u32));
+                    } else {
+                        continue;
+                    }
+                }
+            }
+        }
+    }
+}
+
+/// Iterator over the neighbors of a node.
+///
+/// Iterator element type is `NodeIndex`.
+///
+/// Created with [`.neighbors()`][1], [`.neighbors_directed()`][2] or
+/// [`.neighbors_undirected()`][3].
+///
+/// [1]: struct.Graph.html#method.neighbors
+/// [2]: struct.Graph.html#method.neighbors_directed
+/// [3]: struct.Graph.html#method.neighbors_undirected
+pub struct Neighbors<'a, E: 'a> {
+    /// starting node to skip over
+    skip_start: NodeIndex,
+    edges: &'a [Edge<E>],
+    next: [EdgeIndex; 2],
+}
+
+impl<'a, E> Iterator for Neighbors<'a, E> {
+    type Item = NodeIndex;
+
+    fn next(&mut self) -> Option<NodeIndex> {
+        // First any outgoing edges
+        match self.edges.get(self.next[0].index()) {
+            None => {}
+            Some(edge) => {
+                self.next[0] = edge.next[0];
+                return Some(edge.node[1]);
+            }
+        }
+        // Then incoming edges
+        // For an "undirected" iterator (traverse both incoming
+        // and outgoing edge lists), make sure we don't double
+        // count selfloops by skipping them in the incoming list.
+        while let Some(edge) = self.edges.get(self.next[1].index()) {
+            self.next[1] = edge.next[1];
+            if edge.node[0] != self.skip_start {
+                return Some(edge.node[0]);
+            }
+        }
+        None
+    }
+}
+
+struct EdgesWalkerMut<'a, E: 'a> {
+    edges: &'a mut [Edge<E>],
+    next: EdgeIndex,
+    dir: Direction,
+}
+
+fn edges_walker_mut<E>(
+    edges: &mut [Edge<E>],
+    next: EdgeIndex,
+    dir: Direction,
+) -> EdgesWalkerMut<E> {
+    EdgesWalkerMut { edges, next, dir }
+}
+
+impl<'a, E> EdgesWalkerMut<'a, E> {
+    fn next_edge(&mut self) -> Option<&mut Edge<E>> {
+        self.next().map(|t| t.1)
+    }
+
+    fn next(&mut self) -> Option<(EdgeIndex, &mut Edge<E>)> {
+        let this_index = self.next;
+        let k = self.dir as usize;
+        match self.edges.get_mut(self.next.index()) {
+            None => None,
+            Some(edge) => {
+                self.next = edge.next[k];
+                Some((this_index, edge))
+            }
+        }
+    }
+}
+
+/// Iterator over the edges of from or to a node
+pub struct Edges<'a, E: 'a> {
+    /// starting node to skip over
+    skip_start: NodeIndex,
+    edges: &'a [Edge<E>],
+
+    /// Next edge to visit.
+    next: [EdgeIndex; 2],
+
+    /// For directed graphs: the direction to iterate in
+    /// For undirected graphs: the direction of edges
+    direction: Direction,
+}
+
+impl<'a, E> Iterator for Edges<'a, E> {
+    type Item = EdgeReference<'a, E>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        //      type        direction    |    iterate over    reverse
+        //                               |
+        //    Directed      Outgoing     |      outgoing        no
+        //    Directed      Incoming     |      incoming        no
+        //   Undirected     Outgoing     |        both       incoming
+        //   Undirected     Incoming     |        both       outgoing
+
+        // For iterate_over, "both" is represented as None.
+        // For reverse, "no" is represented as None.
+        let (iterate_over, reverse) = (None, Some(self.direction.opposite()));
+
+        if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing {
+            let i = self.next[0].index();
+            if let Some(Edge { node, weight, next }) = self.edges.get(i) {
+                self.next[0] = next[0];
+                return Some(EdgeReference {
+                    index: EdgeIndex(i as u32),
+                    node: if reverse == Some(Direction::Outgoing) {
+                        swap_pair(*node)
+                    } else {
+                        *node
+                    },
+                    weight,
+                });
+            }
+        }
+
+        if iterate_over.unwrap_or(Direction::Incoming) == Direction::Incoming {
+            while let Some(Edge { node, weight, next }) = self.edges.get(self.next[1].index()) {
+                let edge_index = self.next[1];
+                self.next[1] = next[1];
+                // In any of the "both" situations, self-loops would be iterated over twice.
+                // Skip them here.
+                if iterate_over.is_none() && node[0] == self.skip_start {
+                    continue;
+                }
+
+                return Some(EdgeReference {
+                    index: edge_index,
+                    node: if reverse == Some(Direction::Incoming) {
+                        swap_pair(*node)
+                    } else {
+                        *node
+                    },
+                    weight,
+                });
+            }
+        }
+
+        None
+    }
+}
+
+fn swap_pair<T>(mut x: [T; 2]) -> [T; 2] {
+    x.swap(0, 1);
+    x
+}
+
+impl<'a, E> Clone for Edges<'a, E> {
+    fn clone(&self) -> Self {
+        Edges {
+            skip_start: self.skip_start,
+            edges: self.edges,
+            next: self.next,
+            direction: self.direction,
+        }
+    }
+}
+
+/// Index the `Graph` by `NodeIndex` to access node weights.
+///
+/// **Panics** if the node doesn't exist.
+impl<N, E> Index<NodeIndex> for Graph<N, E> {
+    type Output = N;
+    fn index(&self, index: NodeIndex) -> &N {
+        &self.nodes[index.index()].weight
+    }
+}
+
+/// Index the `Graph` by `NodeIndex` to access node weights.
+///
+/// **Panics** if the node doesn't exist.
+impl<N, E> IndexMut<NodeIndex> for Graph<N, E> {
+    fn index_mut(&mut self, index: NodeIndex) -> &mut N {
+        &mut self.nodes[index.index()].weight
+    }
+}
+
+/// Index the `Graph` by `EdgeIndex` to access edge weights.
+///
+/// **Panics** if the edge doesn't exist.
+impl<N, E> Index<EdgeIndex> for Graph<N, E> {
+    type Output = E;
+    fn index(&self, index: EdgeIndex) -> &E {
+        &self.edges[index.index()].weight
+    }
+}
+
+/// Index the `Graph` by `EdgeIndex` to access edge weights.
+///
+/// **Panics** if the edge doesn't exist.
+impl<N, E> IndexMut<EdgeIndex> for Graph<N, E> {
+    fn index_mut(&mut self, index: EdgeIndex) -> &mut E {
+        &mut self.edges[index.index()].weight
+    }
+}
+
+/// A “walker” object that can be used to step through the edge list of a node.
+///
+/// Created with [`.detach()`](struct.Neighbors.html#method.detach).
+///
+/// The walker does not borrow from the graph, so it lets you step through
+/// neighbors or incident edges while also mutating graph weights, as
+/// in the following example:
+pub struct WalkNeighbors {
+    skip_start: NodeIndex,
+    next: [EdgeIndex; 2],
+}
+
+impl Clone for WalkNeighbors {
+    fn clone(&self) -> Self {
+        WalkNeighbors {
+            skip_start: self.skip_start,
+            next: self.next,
+        }
+    }
+}
+
+/// Reference to a `Graph` edge.
+#[derive(Debug)]
+pub struct EdgeReference<'a, E: 'a> {
+    index: EdgeIndex,
+    node: [NodeIndex; 2],
+    weight: &'a E,
+}
+
+impl<'a, E: 'a> EdgeReference<'a, E> {
+    #[inline]
+    pub fn id(&self) -> EdgeIndex {
+        self.index
+    }
+
+    #[inline]
+    pub fn weight(&self) -> &'a E {
+        self.weight
+    }
+}
+
+impl<'a, E> Clone for EdgeReference<'a, E> {
+    fn clone(&self) -> Self {
+        *self
+    }
+}
+
+impl<'a, E> Copy for EdgeReference<'a, E> {}
+
+impl<'a, E> PartialEq for EdgeReference<'a, E>
+where
+    E: PartialEq,
+{
+    fn eq(&self, rhs: &Self) -> bool {
+        self.index == rhs.index && self.weight == rhs.weight
+    }
+}
+
+/// Iterator over all nodes of a graph.
+pub struct NodeReferences<'a, N: 'a> {
+    iter: std::iter::Enumerate<std::slice::Iter<'a, Node<N>>>,
+}
+
+impl<'a, N> Iterator for NodeReferences<'a, N> {
+    type Item = (NodeIndex, &'a N);
+
+    fn next(&mut self) -> Option<Self::Item> {
+        self.iter
+            .next()
+            .map(|(i, node)| (NodeIndex::new(i as u32), &node.weight))
+    }
+
+    fn size_hint(&self) -> (usize, Option<usize>) {
+        self.iter.size_hint()
+    }
+}
+
+impl<'a, N> DoubleEndedIterator for NodeReferences<'a, N> {
+    fn next_back(&mut self) -> Option<Self::Item> {
+        self.iter
+            .next_back()
+            .map(|(i, node)| (NodeIndex::new(i as u32), &node.weight))
+    }
+}
+
+impl<'a, N> ExactSizeIterator for NodeReferences<'a, N> {}

src/data/mod.rs

@@ -0,0 +1,4 @@
+//! Data structures modified with guaranteed deterministic behavior after deserialization.
+
+pub mod arena;
+pub(crate) mod graph;

src/dynamics/integration_parameters.rs

@@ -0,0 +1,207 @@
+/// Parameters for a time-step of the physics engine.
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct IntegrationParameters {
+    /// The timestep (default: `1.0 / 60.0`)
+    dt: f32,
+    /// The inverse of `dt`.
+    inv_dt: f32,
+    //    /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`).
+    //    ///
+    //    /// This parameter is ignored if rapier is not compiled with is `parallel` feature.
+    //    /// Refer to rayon's documentation regarding how to configure the number of threads with either
+    //    /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`.
+    //    /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower
+    //    /// simulation than setting `multithreading_enabled` to `false`.
+    //    pub multithreading_enabled: bool,
+    /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`).
+    /// This allows the user to take action during a timestep, in-between two CCD events.
+    pub return_after_ccd_substep: bool,
+    /// The Error Reduction Parameter in `[0, 1]` is the proportion of
+    /// the positional error to be corrected at each time step (default: `0.2`).
+    pub erp: f32,
+    /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of
+    /// the positional error to be corrected at each time step (default: `0.2`).
+    pub joint_erp: f32,
+    /// Each cached impulse are multiplied by this coefficient in `[0, 1]`
+    /// when they are re-used to initialize the solver (default `1.0`).
+    pub warmstart_coeff: f32,
+    /// Contacts at points where the involved bodies have a relative
+    /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`).
+    pub restitution_velocity_threshold: f32,
+    /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
+    pub allowed_linear_error: f32,
+    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
+    pub prediction_distance: f32,
+    /// Amount of angular drift of joint limits the engine wont
+    /// attempt to correct (default: `0.001rad`).
+    pub allowed_angular_error: f32,
+    /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`).
+    pub max_linear_correction: f32,
+    /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`).
+    pub max_angular_correction: f32,
+    /// Maximum nonlinear SOR-prox scaling parameter when the constraint
+    /// correction direction is close to the kernel of the involved multibody's
+    /// jacobian (default: `0.2`).
+    pub max_stabilization_multiplier: f32,
+    /// Maximum number of iterations performed by the velocity constraints solver (default: `4`).
+    pub max_velocity_iterations: usize,
+    /// Maximum number of iterations performed by the position-based constraints solver (default: `1`).
+    pub max_position_iterations: usize,
+    /// Minimum number of dynamic bodies in each active island (default: `128`).
+    pub min_island_size: usize,
+    /// Maximum number of iterations performed by the position-based constraints solver for CCD steps (default: `10`).
+    ///
+    /// This should be sufficiently high so all penetration get resolved. For example, if CCD cause your
+    /// objects to stutter, that may be because the number of CCD position iterations is too low, causing
+    /// them to remain stuck in a penetration configuration for a few frames.
+    ///
+    /// The highest this number, the highest its computational cost.
+    pub max_ccd_position_iterations: usize,
+    /// Maximum number of substeps performed by the  solver (default: `1`).
+    pub max_ccd_substeps: usize,
+    /// Controls the number of Proximity::Intersecting events generated by a trigger during CCD resolution (default: `false`).
+    ///
+    /// If false, triggers will only generate one Proximity::Intersecting event during a step, even
+    /// if another colliders repeatedly enters and leaves the triggers during multiple CCD substeps.
+    ///
+    /// If true, triggers will generate as many Proximity::Intersecting and Proximity::Disjoint/Proximity::WithinMargin
+    /// events as the number of times a collider repeatedly enters and leaves the triggers during multiple CCD substeps.
+    /// This is more computationally intensive.
+    pub multiple_ccd_substep_sensor_events_enabled: bool,
+    /// Whether penetration are taken into account in CCD resolution (default: `false`).
+    ///
+    /// If this is set to `false` two penetrating colliders will not be considered to have any time of impact
+    /// while they are penetrating. This may end up allowing some tunelling, but will avoid stuttering effect
+    /// when the constraints solver fails to completely separate two colliders after a CCD contact.
+    ///
+    /// If this is set to `true`, two penetrating colliders will be considered to have a time of impact
+    /// equal to 0 until the constraints solver manages to separate them. This will prevent tunnelling
+    /// almost completely, but may introduce stuttering effects when the constraints solver fails to completely
+    /// separate two colliders after a CCD contact.
+    // FIXME: this is a very binary way of handling penetration.
+    // We should provide a more flexible solution by letting the user choose some
+    // minimal amount of movement applied to an object that get stuck.
+    pub ccd_on_penetration_enabled: bool,
+}
+
+impl IntegrationParameters {
+    /// Creates a set of integration parameters with the given values.
+    pub fn new(
+        dt: f32,
+        //        multithreading_enabled: bool,
+        erp: f32,
+        joint_erp: f32,
+        warmstart_coeff: f32,
+        restitution_velocity_threshold: f32,
+        allowed_linear_error: f32,
+        allowed_angular_error: f32,
+        max_linear_correction: f32,
+        max_angular_correction: f32,
+        prediction_distance: f32,
+        max_stabilization_multiplier: f32,
+        max_velocity_iterations: usize,
+        max_position_iterations: usize,
+        max_ccd_position_iterations: usize,
+        max_ccd_substeps: usize,
+        return_after_ccd_substep: bool,
+        multiple_ccd_substep_sensor_events_enabled: bool,
+        ccd_on_penetration_enabled: bool,
+    ) -> Self {
+        IntegrationParameters {
+            dt,
+            inv_dt: if dt == 0.0 { 0.0 } else { 1.0 / dt },
+            //            multithreading_enabled,
+            erp,
+            joint_erp,
+            warmstart_coeff,
+            restitution_velocity_threshold,
+            allowed_linear_error,
+            allowed_angular_error,
+            max_linear_correction,
+            max_angular_correction,
+            prediction_distance,
+            max_stabilization_multiplier,
+            max_velocity_iterations,
+            max_position_iterations,
+            // FIXME: what is the optimal value for min_island_size?
+            // It should not be too big so that we don't end up with
+            // huge islands that don't fit in cache.
+            // However we don't want it to be too small and end up with
+            // tons of islands, reducing SIMD parallelism opportunities.
+            min_island_size: 128,
+            max_ccd_position_iterations,
+            max_ccd_substeps,
+            return_after_ccd_substep,
+            multiple_ccd_substep_sensor_events_enabled,
+            ccd_on_penetration_enabled,
+        }
+    }
+
+    /// The current time-stepping length.
+    #[inline(always)]
+    pub fn dt(&self) -> f32 {
+        self.dt
+    }
+
+    /// The inverse of the time-stepping length.
+    ///
+    /// This is zero if `self.dt` is zero.
+    #[inline(always)]
+    pub fn inv_dt(&self) -> f32 {
+        self.inv_dt
+    }
+
+    /// Sets the time-stepping length.
+    ///
+    /// This automatically recompute `self.inv_dt`.
+    #[inline]
+    pub fn set_dt(&mut self, dt: f32) {
+        assert!(dt >= 0.0, "The time-stepping length cannot be negative.");
+        self.dt = dt;
+        if dt == 0.0 {
+            self.inv_dt = 0.0
+        } else {
+            self.inv_dt = 1.0 / dt
+        }
+    }
+
+    /// Sets the inverse time-stepping length (i.e. the frequency).
+    ///
+    /// This automatically recompute `self.dt`.
+    #[inline]
+    pub fn set_inv_dt(&mut self, inv_dt: f32) {
+        self.inv_dt = inv_dt;
+        if inv_dt == 0.0 {
+            self.dt = 0.0
+        } else {
+            self.dt = 1.0 / inv_dt
+        }
+    }
+}
+
+impl Default for IntegrationParameters {
+    fn default() -> Self {
+        Self::new(
+            1.0 / 60.0,
+            //            true,
+            0.2,
+            0.2,
+            1.0,
+            1.0,
+            0.005,
+            0.001,
+            0.2,
+            0.2,
+            0.002,
+            0.2,
+            4,
+            1,
+            10,
+            1,
+            false,
+            false,
+            false,
+        )
+    }
+}

src/dynamics/joint/ball_joint.rs

@@ -0,0 +1,34 @@
+use crate::math::{Point, Vector};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that removes all relative linear motion between a pair of points on two bodies.
+pub struct BallJoint {
+    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
+    pub local_anchor1: Point<f32>,
+    /// Where the ball joint is attached on the first body, expressed in the first body local frame.
+    pub local_anchor2: Point<f32>,
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub impulse: Vector<f32>,
+}
+
+impl BallJoint {
+    /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies.
+    pub fn new(local_anchor1: Point<f32>, local_anchor2: Point<f32>) -> Self {
+        Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros())
+    }
+
+    pub(crate) fn with_impulse(
+        local_anchor1: Point<f32>,
+        local_anchor2: Point<f32>,
+        impulse: Vector<f32>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse,
+        }
+    }
+}

src/dynamics/joint/fixed_joint.rs

@@ -0,0 +1,33 @@
+use crate::math::{Isometry, SpacialVector};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that prevents all relative movement between two bodies.
+///
+/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
+pub struct FixedJoint {
+    /// The frame of reference for the first body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor1: Isometry<f32>,
+    /// The frame of reference for the second body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor2: Isometry<f32>,
+    /// The impulse applied to the first body affected by this joint.
+    ///
+    /// The impulse applied to the second body affected by this joint is given by `-impulse`.
+    /// This combines both linear and angular impulses:
+    /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
+    /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
+    pub impulse: SpacialVector<f32>,
+}
+
+impl FixedJoint {
+    /// Creates a new fixed joint from the frames of reference of both bodies.
+    pub fn new(local_anchor1: Isometry<f32>, local_anchor2: Isometry<f32>) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse: SpacialVector::zeros(),
+        }
+    }
+}

src/dynamics/joint/joint.rs

@@ -0,0 +1,112 @@
+#[cfg(feature = "dim3")]
+use crate::dynamics::RevoluteJoint;
+use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// An enum grouping all possible types of joints.
+pub enum JointParams {
+    /// A Ball joint that removes all relative linear degrees of freedom between the affected bodies.
+    BallJoint(BallJoint),
+    /// A fixed joint that removes all relative degrees of freedom between the affected bodies.
+    FixedJoint(FixedJoint),
+    /// A prismatic joint that removes all degrees of degrees of freedom between the affected
+    /// bodies except for the translation along one axis.
+    PrismaticJoint(PrismaticJoint),
+    #[cfg(feature = "dim3")]
+    /// A revolute joint that removes all degrees of degrees of freedom between the affected
+    /// bodies except for the translation along one axis.
+    RevoluteJoint(RevoluteJoint),
+}
+
+impl JointParams {
+    /// An integer identifier for each type of joint.
+    pub fn type_id(&self) -> usize {
+        match self {
+            JointParams::BallJoint(_) => 0,
+            JointParams::FixedJoint(_) => 1,
+            JointParams::PrismaticJoint(_) => 2,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => 3,
+        }
+    }
+
+    /// Gets a reference to the underlying ball joint, if `self` is one.
+    pub fn as_ball_joint(&self) -> Option<&BallJoint> {
+        if let JointParams::BallJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
+    /// Gets a reference to the underlying fixed joint, if `self` is one.
+    pub fn as_fixed_joint(&self) -> Option<&FixedJoint> {
+        if let JointParams::FixedJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
+    /// Gets a reference to the underlying prismatic joint, if `self` is one.
+    pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
+        if let JointParams::PrismaticJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
+    /// Gets a reference to the underlying revolute joint, if `self` is one.
+    #[cfg(feature = "dim3")]
+    pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> {
+        if let JointParams::RevoluteJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+}
+
+impl From<BallJoint> for JointParams {
+    fn from(j: BallJoint) -> Self {
+        JointParams::BallJoint(j)
+    }
+}
+
+impl From<FixedJoint> for JointParams {
+    fn from(j: FixedJoint) -> Self {
+        JointParams::FixedJoint(j)
+    }
+}
+
+#[cfg(feature = "dim3")]
+impl From<RevoluteJoint> for JointParams {
+    fn from(j: RevoluteJoint) -> Self {
+        JointParams::RevoluteJoint(j)
+    }
+}
+
+impl From<PrismaticJoint> for JointParams {
+    fn from(j: PrismaticJoint) -> Self {
+        JointParams::PrismaticJoint(j)
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint attached to two bodies.
+pub struct Joint {
+    /// Handle to the first body attached to this joint.
+    pub body1: RigidBodyHandle,
+    /// Handle to the second body attached to this joint.
+    pub body2: RigidBodyHandle,
+    // A joint needs to know its handle to simplify its removal.
+    pub(crate) handle: JointHandle,
+    #[cfg(feature = "parallel")]
+    pub(crate) constraint_index: usize,
+    #[cfg(feature = "parallel")]
+    pub(crate) position_constraint_index: usize,
+    /// The joint geometric parameters and impulse.
+    pub params: JointParams,
+}

src/dynamics/joint/joint_set.rs

@@ -0,0 +1,218 @@
+use super::Joint;
+use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex};
+
+use crate::data::arena::{Arena, Index};
+use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet};
+
+/// The unique identifier of a joint added to the joint set.
+pub type JointHandle = Index;
+pub(crate) type JointIndex = usize;
+pub(crate) type JointGraphEdge = crate::data::graph::Edge<Joint>;
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A set of joints that can be handled by a physics `World`.
+pub struct JointSet {
+    joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
+    joint_graph: InteractionGraph<Joint>,
+}
+
+impl JointSet {
+    /// Creates a new empty set of joints.
+    pub fn new() -> Self {
+        Self {
+            joint_ids: Arena::new(),
+            joint_graph: InteractionGraph::new(),
+        }
+    }
+
+    /// An always-invalid joint handle.
+    pub fn invalid_handle() -> JointHandle {
+        JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+    }
+
+    /// The number of joints on this set.
+    pub fn len(&self) -> usize {
+        self.joint_graph.graph.edges.len()
+    }
+
+    /// Retrieve the joint graph where edges are joints and nodes are rigid body handles.
+    pub fn joint_graph(&self) -> &InteractionGraph<Joint> {
+        &self.joint_graph
+    }
+
+    /// Is the given joint handle valid?
+    pub fn contains(&self, handle: JointHandle) -> bool {
+        self.joint_ids.contains(handle)
+    }
+
+    /// Gets the joint with the given handle.
+    pub fn get(&self, handle: JointHandle) -> Option<&Joint> {
+        let id = self.joint_ids.get(handle)?;
+        self.joint_graph.graph.edge_weight(*id)
+    }
+
+    /// Gets the joint with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the joint at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the joint position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// Using this is discouraged in favor of `self.get(handle)` which does not
+    /// suffer form the ABA problem.
+    pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> {
+        let (id, handle) = self.joint_ids.get_unknown_gen(i)?;
+        Some((self.joint_graph.graph.edge_weight(*id)?, handle))
+    }
+
+    /// Iterates through all the joint on this set.
+    pub fn iter(&self) -> impl Iterator<Item = &Joint> {
+        self.joint_graph.graph.edges.iter().map(|e| &e.weight)
+    }
+
+    /// Iterates mutably through all the joint on this set.
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = &mut Joint> {
+        self.joint_graph
+            .graph
+            .edges
+            .iter_mut()
+            .map(|e| &mut e.weight)
+    }
+
+    // /// The set of joints as an array.
+    // pub(crate) fn joints(&self) -> &[JointGraphEdge] {
+    //     // self.joint_graph
+    //     //     .graph
+    //     //     .edges
+    //     //     .iter_mut()
+    //     //     .map(|e| &mut e.weight)
+    // }
+
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] {
+        &mut self.joint_graph.graph.edges[..]
+    }
+
+    #[cfg(feature = "parallel")]
+    pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec<JointGraphEdge> {
+        &mut self.joint_graph.graph.edges
+    }
+
+    /// Inserts a new joint into this set and retrieve its handle.
+    pub fn insert<J>(
+        &mut self,
+        bodies: &mut RigidBodySet,
+        body1: RigidBodyHandle,
+        body2: RigidBodyHandle,
+        joint_params: J,
+    ) -> JointHandle
+    where
+        J: Into<JointParams>,
+    {
+        let handle = self.joint_ids.insert(0.into());
+        let joint = Joint {
+            body1,
+            body2,
+            handle,
+            #[cfg(feature = "parallel")]
+            constraint_index: 0,
+            #[cfg(feature = "parallel")]
+            position_constraint_index: 0,
+            params: joint_params.into(),
+        };
+
+        let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2);
+        let (rb1, rb2) = (
+            rb1.expect("Attempt to attach a joint to a non-existing body."),
+            rb2.expect("Attempt to attach a joint to a non-existing body."),
+        );
+
+        // NOTE: the body won't have a graph index if it does not
+        // have any joint attached.
+        if !InteractionGraph::<Joint>::is_graph_index_valid(rb1.joint_graph_index) {
+            rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1);
+        }
+
+        if !InteractionGraph::<Joint>::is_graph_index_valid(rb2.joint_graph_index) {
+            rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2);
+        }
+
+        let id = self
+            .joint_graph
+            .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint);
+
+        self.joint_ids[handle] = id;
+        handle
+    }
+
+    /// Retrieve all the joints happening between two active bodies.
+    // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions.
+    pub(crate) fn select_active_interactions(
+        &self,
+        bodies: &RigidBodySet,
+        out: &mut Vec<Vec<JointIndex>>,
+    ) {
+        for out_island in &mut out[..bodies.num_islands()] {
+            out_island.clear();
+        }
+
+        // FIXME: don't iterate through all the interactions.
+        for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() {
+            let joint = &edge.weight;
+            let rb1 = &bodies[joint.body1];
+            let rb2 = &bodies[joint.body2];
+
+            if (rb1.is_dynamic() || rb2.is_dynamic())
+                && (!rb1.is_dynamic() || !rb1.is_sleeping())
+                && (!rb2.is_dynamic() || !rb2.is_sleeping())
+            {
+                let island_index = if !rb1.is_dynamic() {
+                    rb2.active_island_id
+                } else {
+                    rb1.active_island_id
+                };
+
+                out[island_index].push(i);
+            }
+        }
+    }
+
+    pub(crate) fn remove_rigid_body(
+        &mut self,
+        deleted_id: RigidBodyGraphIndex,
+        bodies: &mut RigidBodySet,
+    ) {
+        if InteractionGraph::<()>::is_graph_index_valid(deleted_id) {
+            // We have to delete each joint one by one in order to:
+            // - Wake-up the attached bodies.
+            // - Update our Handle -> graph edge mapping.
+            // Delete the node.
+            let to_delete: Vec<_> = self
+                .joint_graph
+                .interactions_with(deleted_id)
+                .map(|e| (e.0, e.1, e.2.handle))
+                .collect();
+            for (h1, h2, to_delete_handle) in to_delete {
+                let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap();
+                self.joint_graph.graph.remove_edge(to_delete_edge_id);
+
+                // Update the id of the edge which took the place of the deleted one.
+                if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) {
+                    self.joint_ids[j.handle] = to_delete_edge_id;
+                }
+
+                // Wake up the attached bodies.
+                bodies.wake_up(h1);
+                bodies.wake_up(h2);
+            }
+
+            if let Some(other) = self.joint_graph.remove_node(deleted_id) {
+                // One rigid-body joint graph index may have been invalidated
+                // so we need to update it.
+                if let Some(replacement) = bodies.get_mut_internal(other) {
+                    replacement.joint_graph_index = deleted_id;
+                }
+            }
+        }
+    }
+}

src/dynamics/joint/mod.rs

@@ -0,0 +1,16 @@
+pub use self::ball_joint::BallJoint;
+pub use self::fixed_joint::FixedJoint;
+pub use self::joint::{Joint, JointParams};
+pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
+pub use self::joint_set::{JointHandle, JointSet};
+pub use self::prismatic_joint::PrismaticJoint;
+#[cfg(feature = "dim3")]
+pub use self::revolute_joint::RevoluteJoint;
+
+mod ball_joint;
+mod fixed_joint;
+mod joint;
+mod joint_set;
+mod prismatic_joint;
+#[cfg(feature = "dim3")]
+mod revolute_joint;

src/dynamics/joint/prismatic_joint.rs

@@ -0,0 +1,193 @@
+use crate::math::{Isometry, Point, Vector, DIM};
+use crate::utils::WBasis;
+use na::Unit;
+#[cfg(feature = "dim2")]
+use na::Vector2;
+#[cfg(feature = "dim3")]
+use na::Vector5;
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that removes all relative motion between two bodies, except for the translations along one axis.
+pub struct PrismaticJoint {
+    /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body.
+    pub local_anchor1: Point<f32>,
+    /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body.
+    pub local_anchor2: Point<f32>,
+    pub(crate) local_axis1: Unit<Vector<f32>>,
+    pub(crate) local_axis2: Unit<Vector<f32>>,
+    pub(crate) basis1: [Vector<f32>; DIM - 1],
+    pub(crate) basis2: [Vector<f32>; DIM - 1],
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    #[cfg(feature = "dim3")]
+    pub impulse: Vector5<f32>,
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    #[cfg(feature = "dim2")]
+    pub impulse: Vector2<f32>,
+    /// Whether or not this joint should enforce translational limits along its axis.
+    pub limits_enabled: bool,
+    /// The min an max relative position of the attached bodies along this joint's axis.
+    pub limits: [f32; 2],
+    /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub limits_impulse: f32,
+    // pub motor_enabled: bool,
+    // pub target_motor_vel: f32,
+    // pub max_motor_impulse: f32,
+    // pub motor_impulse: f32,
+}
+
+impl PrismaticJoint {
+    /// Creates a new prismatic joint with the given point of applications and axis, all expressed
+    /// in the local-space of the affected bodies.
+    #[cfg(feature = "dim2")]
+    pub fn new(
+        local_anchor1: Point<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1: local_axis1.orthonormal_basis(),
+            basis2: local_axis2.orthonormal_basis(),
+            impulse: na::zero(),
+            limits_enabled: false,
+            limits: [-f32::MAX, f32::MAX],
+            limits_impulse: 0.0,
+            // motor_enabled: false,
+            // target_motor_vel: 0.0,
+            // max_motor_impulse: f32::MAX,
+            // motor_impulse: 0.0,
+        }
+    }
+
+    /// Creates a new prismatic joint with the given point of applications and axis, all expressed
+    /// in the local-space of the affected bodies.
+    ///
+    /// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal
+    /// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically
+    /// computed arbitrarily.
+    #[cfg(feature = "dim3")]
+    pub fn new(
+        local_anchor1: Point<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_tangent1: Vector<f32>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+        local_tangent2: Vector<f32>,
+    ) -> Self {
+        let basis1 = if let Some(local_bitangent1) =
+            Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3)
+        {
+            [
+                local_bitangent1.into_inner(),
+                local_bitangent1.cross(&local_axis1),
+            ]
+        } else {
+            local_axis1.orthonormal_basis()
+        };
+
+        let basis2 = if let Some(local_bitangent2) =
+            Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3)
+        {
+            [
+                local_bitangent2.into_inner(),
+                local_bitangent2.cross(&local_axis2),
+            ]
+        } else {
+            local_axis2.orthonormal_basis()
+        };
+
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1,
+            basis2,
+            impulse: na::zero(),
+            limits_enabled: false,
+            limits: [-f32::MAX, f32::MAX],
+            limits_impulse: 0.0,
+            // motor_enabled: false,
+            // target_motor_vel: 0.0,
+            // max_motor_impulse: f32::MAX,
+            // motor_impulse: 0.0,
+        }
+    }
+
+    /// The local axis of this joint, expressed in the local-space of the first attached body.
+    pub fn local_axis1(&self) -> Unit<Vector<f32>> {
+        self.local_axis1
+    }
+
+    /// The local axis of this joint, expressed in the local-space of the second attached body.
+    pub fn local_axis2(&self) -> Unit<Vector<f32>> {
+        self.local_axis2
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim2")]
+    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+        use na::{Matrix2, Rotation2, UnitComplex};
+
+        let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]);
+        let rotmat = Rotation2::from_matrix_unchecked(mat);
+        let rotation = UnitComplex::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor1.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim2")]
+    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+        use na::{Matrix2, Rotation2, UnitComplex};
+
+        let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]);
+        let rotmat = Rotation2::from_matrix_unchecked(mat);
+        let rotation = UnitComplex::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor2.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim3")]
+    pub(crate) fn local_frame1(&self) -> Isometry<f32> {
+        use na::{Matrix3, Rotation3, UnitQuaternion};
+
+        let mat = Matrix3::from_columns(&[
+            self.local_axis1.into_inner(),
+            self.basis1[0],
+            self.basis1[1],
+        ]);
+        let rotmat = Rotation3::from_matrix_unchecked(mat);
+        let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor1.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+
+    // FIXME: precompute this?
+    #[cfg(feature = "dim3")]
+    pub(crate) fn local_frame2(&self) -> Isometry<f32> {
+        use na::{Matrix3, Rotation3, UnitQuaternion};
+
+        let mat = Matrix3::from_columns(&[
+            self.local_axis2.into_inner(),
+            self.basis2[0],
+            self.basis2[1],
+        ]);
+        let rotmat = Rotation3::from_matrix_unchecked(mat);
+        let rotation = UnitQuaternion::from_rotation_matrix(&rotmat);
+        let translation = self.local_anchor2.coords.into();
+        Isometry::from_parts(translation, rotation)
+    }
+}

src/dynamics/joint/revolute_joint.rs

@@ -0,0 +1,46 @@
+use crate::math::{Point, Vector};
+use crate::utils::WBasis;
+use na::{Unit, Vector5};
+
+#[derive(Copy, Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that removes all relative motion between two bodies, except for the rotations along one axis.
+pub struct RevoluteJoint {
+    /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body.
+    pub local_anchor1: Point<f32>,
+    /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body.
+    pub local_anchor2: Point<f32>,
+    /// The rotation axis of this revolute joint expressed in the local space of the first attached body.
+    pub local_axis1: Unit<Vector<f32>>,
+    /// The rotation axis of this revolute joint expressed in the local space of the second attached body.
+    pub local_axis2: Unit<Vector<f32>>,
+    /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body.
+    pub basis1: [Vector<f32>; 2],
+    /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body.
+    pub basis2: [Vector<f32>; 2],
+    /// The impulse applied by this joint on the first body.
+    ///
+    /// The impulse applied to the second body is given by `-impulse`.
+    pub impulse: Vector5<f32>,
+}
+
+impl RevoluteJoint {
+    /// Creates a new revolute joint with the given point of applications and axis, all expressed
+    /// in the local-space of the affected bodies.
+    pub fn new(
+        local_anchor1: Point<f32>,
+        local_axis1: Unit<Vector<f32>>,
+        local_anchor2: Point<f32>,
+        local_axis2: Unit<Vector<f32>>,
+    ) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            local_axis1,
+            local_axis2,
+            basis1: local_axis1.orthonormal_basis(),
+            basis2: local_axis2.orthonormal_basis(),
+            impulse: na::zero(),
+        }
+    }
+}

src/dynamics/mass_properties.rs

@@ -0,0 +1,206 @@
+use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::utils;
+use num::Zero;
+use std::ops::{Add, AddAssign};
+#[cfg(feature = "dim3")]
+use {na::Matrix3, std::ops::MulAssign};
+
+#[derive(Copy, Clone, Debug, PartialEq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The local mass properties of a rigid-body.
+pub struct MassProperties {
+    /// The center of mass of a rigid-body expressed in its local-space.
+    pub local_com: Point<f32>,
+    /// The inverse of the mass of a rigid-body.
+    ///
+    /// If this is zero, the rigid-body is assumed to have infinite mass.
+    pub inv_mass: f32,
+    /// The inverse of the principal angular inertia of the rigid-body.
+    ///
+    /// Components set to zero are assumed to be infinite along the corresponding principal axis.
+    pub inv_principal_inertia_sqrt: AngVector<f32>,
+    #[cfg(feature = "dim3")]
+    /// The principal vectors of the local angular inertia tensor of the rigid-body.
+    pub principal_inertia_local_frame: Rotation<f32>,
+}
+
+impl MassProperties {
+    #[cfg(feature = "dim2")]
+    pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
+        let inv_mass = utils::inv(mass);
+        let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt());
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: AngVector<f32>) -> Self {
+        Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity())
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn with_principal_inertia_frame(
+        local_com: Point<f32>,
+        mass: f32,
+        principal_inertia: AngVector<f32>,
+        principal_inertia_local_frame: Rotation<f32>,
+    ) -> Self {
+        let inv_mass = utils::inv(mass);
+        let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+            principal_inertia_local_frame,
+        }
+    }
+
+    /// The world-space center of mass of the rigid-body.
+    pub fn world_com(&self, pos: &Isometry<f32>) -> Point<f32> {
+        pos * self.local_com
+    }
+
+    #[cfg(feature = "dim2")]
+    /// The world-space inverse angular inertia tensor of the rigid-body.
+    pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation<f32>) -> AngularInertia<f32> {
+        self.inv_principal_inertia_sqrt
+    }
+
+    #[cfg(feature = "dim3")]
+    /// The world-space inverse angular inertia tensor of the rigid-body.
+    pub fn world_inv_inertia_sqrt(&self, rot: &Rotation<f32>) -> AngularInertia<f32> {
+        if !self.inv_principal_inertia_sqrt.is_zero() {
+            let mut lhs = (rot * self.principal_inertia_local_frame)
+                .to_rotation_matrix()
+                .into_inner();
+            let rhs = lhs.transpose();
+            lhs.column_mut(0)
+                .mul_assign(self.inv_principal_inertia_sqrt.x);
+            lhs.column_mut(1)
+                .mul_assign(self.inv_principal_inertia_sqrt.y);
+            lhs.column_mut(2)
+                .mul_assign(self.inv_principal_inertia_sqrt.z);
+            let inertia = lhs * rhs;
+            AngularInertia::from_sdp_matrix(inertia)
+        } else {
+            AngularInertia::zero()
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
+    pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
+        let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
+        self.principal_inertia_local_frame.to_rotation_matrix()
+            * Matrix3::from_diagonal(&principal_inertia)
+            * self
+                .principal_inertia_local_frame
+                .inverse()
+                .to_rotation_matrix()
+    }
+
+    #[cfg(feature = "dim2")]
+    pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> f32 {
+        if self.inv_mass != 0.0 {
+            let mass = 1.0 / self.inv_mass;
+            let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt);
+            i + shift.norm_squared() * mass
+        } else {
+            0.0
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector<f32>) -> Matrix3<f32> {
+        if self.inv_mass != 0.0 {
+            let mass = 1.0 / self.inv_mass;
+            let matrix = self.reconstruct_inertia_matrix();
+            let diag = shift.norm_squared();
+            let diagm = Matrix3::from_diagonal_element(diag);
+            matrix + (diagm + shift * shift.transpose()) * mass
+        } else {
+            Matrix3::zeros()
+        }
+    }
+}
+
+impl Zero for MassProperties {
+    fn zero() -> Self {
+        Self {
+            inv_mass: 0.0,
+            inv_principal_inertia_sqrt: na::zero(),
+            #[cfg(feature = "dim3")]
+            principal_inertia_local_frame: Rotation::identity(),
+            local_com: Point::origin(),
+        }
+    }
+
+    fn is_zero(&self) -> bool {
+        *self == Self::zero()
+    }
+}
+
+impl Add<MassProperties> for MassProperties {
+    type Output = Self;
+
+    #[cfg(feature = "dim2")]
+    fn add(self, other: MassProperties) -> Self {
+        if self.is_zero() {
+            return other;
+        } else if other.is_zero() {
+            return self;
+        }
+
+        let m1 = utils::inv(self.inv_mass);
+        let m2 = utils::inv(other.inv_mass);
+        let inv_mass = utils::inv(m1 + m2);
+        let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass;
+        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+        let inertia = i1 + i2;
+        let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt());
+
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    fn add(self, other: MassProperties) -> Self {
+        if self.is_zero() {
+            return other;
+        } else if other.is_zero() {
+            return self;
+        }
+
+        let m1 = utils::inv(self.inv_mass);
+        let m2 = utils::inv(other.inv_mass);
+        let inv_mass = utils::inv(m1 + m2);
+        let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass;
+        let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+        let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+        let inertia = i1 + i2;
+        let eigen = inertia.symmetric_eigen();
+        let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
+        let principal_inertia = eigen.eigenvalues;
+        let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
+
+        Self {
+            local_com,
+            inv_mass,
+            inv_principal_inertia_sqrt,
+            principal_inertia_local_frame,
+        }
+    }
+}
+
+impl AddAssign<MassProperties> for MassProperties {
+    fn add_assign(&mut self, rhs: MassProperties) {
+        *self = *self + rhs
+    }
+}

src/dynamics/mass_properties_ball.rs

@@ -0,0 +1,30 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::math::Vector;
+use crate::math::{Point, PrincipalAngularInertia};
+
+impl MassProperties {
+    pub(crate) fn ball_volume_unit_angular_inertia(
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            let volume = std::f32::consts::PI * radius * radius;
+            let i = radius * radius / 2.0;
+            (volume, i)
+        }
+        #[cfg(feature = "dim3")]
+        {
+            let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0;
+            let i = radius * radius * 2.0 / 5.0;
+
+            (volume, Vector::repeat(i))
+        }
+    }
+
+    pub(crate) fn from_ball(density: f32, radius: f32) -> Self {
+        let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius);
+        let mass = vol * density;
+        Self::new(Point::origin(), mass, unit_i * mass)
+    }
+}

src/dynamics/mass_properties_capsule.rs

@@ -0,0 +1,60 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::geometry::Capsule;
+use crate::math::{Point, PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    fn cylinder_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
+            let sq_radius = radius * radius;
+            let sq_height = half_height * half_height * 4.0;
+            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
+
+            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
+            (volume, inertia)
+        }
+    }
+
+    pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
+        let half_height = (b - a).norm() / 2.0;
+        let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
+        let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius);
+        let cap_vol = cyl_vol + ball_vol;
+        let cap_mass = cap_vol * density;
+        let mut cap_unit_i = cyl_unit_i + ball_unit_i;
+        let local_com = na::center(&a, &b);
+
+        #[cfg(feature = "dim2")]
+        {
+            let h = half_height * 2.0;
+            let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
+            cap_unit_i += extra;
+            Self::new(local_com, cap_mass, cap_unit_i * cap_mass)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let h = half_height * 2.0;
+            let extra = h * h * 0.5 + h * radius * 3.0 / 8.0;
+            cap_unit_i.x += extra;
+            cap_unit_i.z += extra;
+            let local_frame = Capsule::new(a, b, radius).rotation_wrt_y();
+            Self::with_principal_inertia_frame(
+                local_com,
+                cap_mass,
+                cap_unit_i * cap_mass,
+                local_frame,
+            )
+        }
+    }
+}

src/dynamics/mass_properties_cuboid.rs

@@ -0,0 +1,33 @@
+use crate::dynamics::MassProperties;
+use crate::math::{Point, PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    pub(crate) fn cuboid_volume_unit_inertia(
+        half_extents: Vector<f32>,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            let volume = half_extents.x * half_extents.y * 4.0;
+            let ix = (half_extents.x * half_extents.x) / 3.0;
+            let iy = (half_extents.y * half_extents.y) / 3.0;
+
+            (volume, ix + iy)
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_extents.x * half_extents.y * half_extents.z * 8.0;
+            let ix = (half_extents.x * half_extents.x) / 3.0;
+            let iy = (half_extents.y * half_extents.y) / 3.0;
+            let iz = (half_extents.z * half_extents.z) / 3.0;
+
+            (volume, Vector::new(iy + iz, ix + iz, ix + iy))
+        }
+    }
+
+    pub(crate) fn from_cuboid(density: f32, half_extents: Vector<f32>) -> Self {
+        let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents);
+        let mass = vol * density;
+        Self::new(Point::origin(), mass, unit_i * mass)
+    }
+}

src/dynamics/mass_properties_polygon.rs

@@ -0,0 +1,144 @@
+use crate::dynamics::MassProperties;
+use crate::math::Point;
+
+impl MassProperties {
+    pub(crate) fn from_polygon(density: f32, vertices: &[Point<f32>]) -> MassProperties {
+        let (area, com) = convex_polygon_area_and_center_of_mass(vertices);
+
+        if area == 0.0 {
+            return MassProperties::new(com, 0.0, 0.0);
+        }
+
+        let mut itot = 0.0;
+        let factor = 1.0 / 6.0;
+
+        let mut iterpeek = vertices.iter().peekable();
+        let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or
+        while let Some(elem) = iterpeek.next() {
+            let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement));
+
+            // algorithm adapted from Box2D
+            let e1 = *elem - com;
+            let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com;
+
+            let ex1 = e1[0];
+            let ey1 = e1[1];
+            let ex2 = e2[0];
+            let ey2 = e2[1];
+
+            let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
+            let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
+
+            let ipart = factor * (intx2 + inty2);
+
+            itot += ipart * area;
+        }
+
+        Self::new(com, area * density, itot * density)
+    }
+}
+
+fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point<f32>]) -> (f32, Point<f32>) {
+    let geometric_center = convex_polygon
+        .iter()
+        .fold(Point::origin(), |e1, e2| e1 + e2.coords)
+        / convex_polygon.len() as f32;
+    let mut res = Point::origin();
+    let mut areasum = 0.0;
+
+    let mut iterpeek = convex_polygon.iter().peekable();
+    let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or.
+    while let Some(elem) = iterpeek.next() {
+        let (a, b, c) = (
+            elem,
+            iterpeek.peek().unwrap_or(&firstelement),
+            &geometric_center,
+        );
+        let area = triangle_area(a, b, c);
+        let center = (a.coords + b.coords + c.coords) / 3.0;
+
+        res += center * area;
+        areasum += area;
+    }
+
+    if areasum == 0.0 {
+        (areasum, geometric_center)
+    } else {
+        (areasum, res / areasum)
+    }
+}
+
+pub fn triangle_area(pa: &Point<f32>, pb: &Point<f32>, pc: &Point<f32>) -> f32 {
+    // Kahan's formula.
+    let a = na::distance(pa, pb);
+    let b = na::distance(pb, pc);
+    let c = na::distance(pc, pa);
+
+    let (c, b, a) = sort3(&a, &b, &c);
+    let a = *a;
+    let b = *b;
+    let c = *c;
+
+    let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c));
+
+    sqr.sqrt() * 0.25
+}
+
+/// Sorts a set of three values in increasing order.
+#[inline]
+pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) {
+    let a_b = *a > *b;
+    let a_c = *a > *c;
+    let b_c = *b > *c;
+
+    let sa;
+    let sb;
+    let sc;
+
+    // Sort the three values.
+    // FIXME: move this to the utilities?
+    if a_b {
+        // a > b
+        if a_c {
+            // a > c
+            sc = a;
+
+            if b_c {
+                // b > c
+                sa = c;
+                sb = b;
+            } else {
+                // b <= c
+                sa = b;
+                sb = c;
+            }
+        } else {
+            // a <= c
+            sa = b;
+            sb = a;
+            sc = c;
+        }
+    } else {
+        // a < b
+        if !a_c {
+            // a <= c
+            sa = a;
+
+            if b_c {
+                // b > c
+                sb = c;
+                sc = b;
+            } else {
+                sb = b;
+                sc = c;
+            }
+        } else {
+            // a > c
+            sa = c;
+            sb = a;
+            sc = b;
+        }
+    }
+
+    (sa, sb, sc)
+}

src/dynamics/mod.rs

@@ -0,0 +1,30 @@
+//! Structures related to dynamics: bodies, joints, etc.
+
+pub use self::integration_parameters::IntegrationParameters;
+pub(crate) use self::joint::JointIndex;
+#[cfg(feature = "dim3")]
+pub use self::joint::RevoluteJoint;
+pub use self::joint::{
+    BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint,
+};
+pub use self::mass_properties::MassProperties;
+pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
+pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodyMut, RigidBodySet};
+// #[cfg(not(feature = "parallel"))]
+pub(crate) use self::joint::JointGraphEdge;
+#[cfg(not(feature = "parallel"))]
+pub(crate) use self::solver::IslandSolver;
+#[cfg(feature = "parallel")]
+pub(crate) use self::solver::ParallelIslandSolver;
+
+mod integration_parameters;
+mod joint;
+mod mass_properties;
+mod mass_properties_ball;
+mod mass_properties_capsule;
+mod mass_properties_cuboid;
+#[cfg(feature = "dim2")]
+mod mass_properties_polygon;
+mod rigid_body;
+mod rigid_body_set;
+mod solver;

src/dynamics/rigid_body.rs

@@ -0,0 +1,441 @@
+use crate::dynamics::MassProperties;
+use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
+use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
+use crate::utils::{WCross, WDot};
+use num::Zero;
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// The status of a body, governing the way it is affected by external forces.
+pub enum BodyStatus {
+    /// A `BodyStatus::Dynamic` body can be affected by all external forces.
+    Dynamic,
+    /// A `BodyStatus::Static` body cannot be affected by external forces.
+    Static,
+    /// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled
+    /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies.
+    ///
+    /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body
+    /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be
+    /// modified by the user and is independent from any contact or joint it is involved in.
+    Kinematic,
+    // Semikinematic, // A kinematic that performs automatic CCD with the static environment toi avoid traversing it?
+    // Disabled,
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A rigid body.
+///
+/// To create a new rigid-body, use the `RigidBodyBuilder` structure.
+#[derive(Debug)]
+pub struct RigidBody {
+    /// The world-space position of the rigid-body.
+    pub position: Isometry<f32>,
+    pub(crate) predicted_position: Isometry<f32>,
+    /// The local mass properties of the rigid-body.
+    pub mass_properties: MassProperties,
+    /// The world-space center of mass of the rigid-body.
+    pub world_com: Point<f32>,
+    /// The square-root of the inverse angular inertia tensor of the rigid-body.
+    pub world_inv_inertia_sqrt: AngularInertia<f32>,
+    /// The linear velocity of the rigid-body.
+    pub linvel: Vector<f32>,
+    /// The angular velocity of the rigid-body.
+    pub angvel: AngVector<f32>,
+    pub(crate) linacc: Vector<f32>,
+    pub(crate) angacc: AngVector<f32>,
+    pub(crate) colliders: Vec<ColliderHandle>,
+    /// Whether or not this rigid-body is sleeping.
+    pub activation: ActivationStatus,
+    pub(crate) joint_graph_index: RigidBodyGraphIndex,
+    pub(crate) active_island_id: usize,
+    pub(crate) active_set_id: usize,
+    pub(crate) active_set_offset: usize,
+    pub(crate) active_set_timestamp: u32,
+    /// The status of the body, governing how it is affected by external forces.
+    pub body_status: BodyStatus,
+}
+
+impl Clone for RigidBody {
+    fn clone(&self) -> Self {
+        Self {
+            colliders: Vec::new(),
+            joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32),
+            active_island_id: crate::INVALID_USIZE,
+            active_set_id: crate::INVALID_USIZE,
+            active_set_offset: crate::INVALID_USIZE,
+            active_set_timestamp: crate::INVALID_U32,
+            ..*self
+        }
+    }
+}
+
+impl RigidBody {
+    fn new() -> Self {
+        Self {
+            position: Isometry::identity(),
+            predicted_position: Isometry::identity(),
+            mass_properties: MassProperties::zero(),
+            world_com: Point::origin(),
+            world_inv_inertia_sqrt: AngularInertia::zero(),
+            linvel: Vector::zeros(),
+            angvel: na::zero(),
+            linacc: Vector::zeros(),
+            angacc: na::zero(),
+            colliders: Vec::new(),
+            activation: ActivationStatus::new_active(),
+            joint_graph_index: InteractionGraph::<()>::invalid_graph_index(),
+            active_island_id: 0,
+            active_set_id: 0,
+            active_set_offset: 0,
+            active_set_timestamp: 0,
+            body_status: BodyStatus::Dynamic,
+        }
+    }
+
+    pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector<f32>) {
+        if self.mass_properties.inv_mass != 0.0 {
+            self.linvel += (gravity + self.linacc) * dt;
+            self.angvel += self.angacc * dt;
+
+            // Reset the accelerations.
+            self.linacc = na::zero();
+            self.angacc = na::zero();
+        }
+    }
+
+    /// The handles of colliders attached to this rigid body.
+    pub fn colliders(&self) -> &[ColliderHandle] {
+        &self.colliders[..]
+    }
+
+    /// Is this rigid body dynamic?
+    ///
+    /// A dynamic body can move freely and is affected by forces.
+    pub fn is_dynamic(&self) -> bool {
+        self.body_status == BodyStatus::Dynamic
+    }
+
+    /// Is this rigid body kinematic?
+    ///
+    /// A kinematic body can move freely but is not affected by forces.
+    pub fn is_kinematic(&self) -> bool {
+        self.body_status == BodyStatus::Kinematic
+    }
+
+    /// Is this rigid body static?
+    ///
+    /// A static body cannot move and is not affected by forces.
+    pub fn is_static(&self) -> bool {
+        self.body_status == BodyStatus::Static
+    }
+
+    /// The mass of this rigid body.
+    ///
+    /// Returns zero if this rigid body has an infinite mass.
+    pub fn mass(&self) -> f32 {
+        crate::utils::inv(self.mass_properties.inv_mass)
+    }
+
+    /// Put this rigid body to sleep.
+    ///
+    /// A sleeping body no longer moves and is no longer simulated by the physics engine unless
+    /// it is waken up. It can be woken manually with `self.wake_up` or automatically due to
+    /// external forces like contacts.
+    pub fn sleep(&mut self) {
+        self.activation.energy = 0.0;
+        self.activation.sleeping = true;
+        self.linvel = na::zero();
+        self.angvel = na::zero();
+    }
+
+    /// Wakes up this rigid body if it is sleeping.
+    pub fn wake_up(&mut self) {
+        self.activation.sleeping = false;
+
+        if self.activation.energy == 0.0 && self.is_dynamic() {
+            self.activation.energy = self.activation.threshold.abs() * 2.0;
+        }
+    }
+
+    pub(crate) fn update_energy(&mut self) {
+        let mix_factor = 0.01;
+        let new_energy = (1.0 - mix_factor) * self.activation.energy
+            + mix_factor * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel));
+        self.activation.energy = new_energy.min(self.activation.threshold.abs() * 4.0);
+    }
+
+    /// Is this rigid body sleeping?
+    pub fn is_sleeping(&self) -> bool {
+        self.activation.sleeping
+    }
+
+    fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
+        let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
+        let shift = Translation::from(com.coords);
+        shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
+    }
+    pub(crate) fn integrate(&mut self, dt: f32) {
+        self.position = self.integrate_velocity(dt) * self.position;
+    }
+
+    /// Sets the position of this rigid body.
+    pub fn set_position(&mut self, pos: Isometry<f32>) {
+        self.position = pos;
+
+        // TODO: update the predicted position for dynamic bodies too?
+        if self.is_static() {
+            self.predicted_position = pos;
+        }
+    }
+
+    /// If this rigid body is kinematic, sets its future position after the next timestep integration.
+    pub fn set_next_kinematic_position(&mut self, pos: Isometry<f32>) {
+        if self.is_kinematic() {
+            self.predicted_position = pos;
+        }
+    }
+
+    pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) {
+        let dpos = self.predicted_position * self.position.inverse();
+        #[cfg(feature = "dim2")]
+        {
+            self.angvel = dpos.rotation.angle() * inv_dt;
+        }
+        #[cfg(feature = "dim3")]
+        {
+            self.angvel = dpos.rotation.scaled_axis() * inv_dt;
+        }
+        self.linvel = dpos.translation.vector * inv_dt;
+    }
+
+    pub(crate) fn update_predicted_position(&mut self, dt: f32) {
+        self.predicted_position = self.integrate_velocity(dt) * self.position;
+    }
+
+    pub(crate) fn update_world_mass_properties(&mut self) {
+        self.world_com = self.mass_properties.world_com(&self.position);
+        self.world_inv_inertia_sqrt = self
+            .mass_properties
+            .world_inv_inertia_sqrt(&self.position.rotation);
+    }
+
+    /*
+     * Application of forces/impulses.
+     */
+    /// Applies a force at the center-of-mass of this rigid-body.
+    pub fn apply_force(&mut self, force: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.linacc += force * self.mass_properties.inv_mass;
+        }
+    }
+
+    /// Applies an impulse at the center-of-mass of this rigid-body.
+    pub fn apply_impulse(&mut self, impulse: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.linvel += impulse * self.mass_properties.inv_mass;
+        }
+    }
+
+    /// Applies a torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim2")]
+    pub fn apply_torque(&mut self, torque: f32) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+        }
+    }
+
+    /// Applies a torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim3")]
+    pub fn apply_torque(&mut self, torque: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque);
+        }
+    }
+
+    /// Applies an impulsive torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim2")]
+    pub fn apply_torque_impulse(&mut self, torque_impulse: f32) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angvel +=
+                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+        }
+    }
+
+    /// Applies an impulsive torque at the center-of-mass of this rigid-body.
+    #[cfg(feature = "dim3")]
+    pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<f32>) {
+        if self.body_status == BodyStatus::Dynamic {
+            self.angvel +=
+                self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse);
+        }
+    }
+
+    /// Applies a force at the given world-space point of this rigid-body.
+    pub fn apply_force_at_point(&mut self, force: Vector<f32>, point: Point<f32>) {
+        let torque = (point - self.world_com).gcross(force);
+        self.apply_force(force);
+        self.apply_torque(torque);
+    }
+
+    /// Applies an impulse at the given world-space point of this rigid-body.
+    pub fn apply_impulse_at_point(&mut self, impulse: Vector<f32>, point: Point<f32>) {
+        let torque_impulse = (point - self.world_com).gcross(impulse);
+        self.apply_impulse(impulse);
+        self.apply_torque_impulse(torque_impulse);
+    }
+}
+
+/// A builder for rigid-bodies.
+pub struct RigidBodyBuilder {
+    position: Isometry<f32>,
+    linvel: Vector<f32>,
+    angvel: AngVector<f32>,
+    body_status: BodyStatus,
+    can_sleep: bool,
+}
+
+impl RigidBodyBuilder {
+    /// Initialize a new builder for a rigid body which is either static, dynamic, or kinematic.
+    pub fn new(body_status: BodyStatus) -> Self {
+        Self {
+            position: Isometry::identity(),
+            linvel: Vector::zeros(),
+            angvel: na::zero(),
+            body_status,
+            can_sleep: true,
+        }
+    }
+
+    /// Initializes the builder of a new static rigid body.
+    pub fn new_static() -> Self {
+        Self::new(BodyStatus::Static)
+    }
+
+    /// Initializes the builder of a new kinematic rigid body.
+    pub fn new_kinematic() -> Self {
+        Self::new(BodyStatus::Kinematic)
+    }
+
+    /// Initializes the builder of a new dynamic rigid body.
+    pub fn new_dynamic() -> Self {
+        Self::new(BodyStatus::Dynamic)
+    }
+
+    /// Sets the initial translation of the rigid-body to be created.
+    #[cfg(feature = "dim2")]
+    pub fn translation(mut self, x: f32, y: f32) -> Self {
+        self.position.translation.x = x;
+        self.position.translation.y = y;
+        self
+    }
+
+    /// Sets the initial translation of the rigid-body to be created.
+    #[cfg(feature = "dim3")]
+    pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
+        self.position.translation.x = x;
+        self.position.translation.y = y;
+        self.position.translation.z = z;
+        self
+    }
+
+    /// Sets the initial orientation of the rigid-body to be created.
+    pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
+        self.position.rotation = Rotation::new(angle);
+        self
+    }
+
+    /// Sets the initial position (translation and orientation) of the rigid-body to be created.
+    pub fn position(mut self, pos: Isometry<f32>) -> Self {
+        self.position = pos;
+        self
+    }
+
+    /// Sets the initial linear velocity of the rigid-body to be created.
+    #[cfg(feature = "dim2")]
+    pub fn linvel(mut self, x: f32, y: f32) -> Self {
+        self.linvel = Vector::new(x, y);
+        self
+    }
+
+    /// Sets the initial linear velocity of the rigid-body to be created.
+    #[cfg(feature = "dim3")]
+    pub fn linvel(mut self, x: f32, y: f32, z: f32) -> Self {
+        self.linvel = Vector::new(x, y, z);
+        self
+    }
+
+    /// Sets the initial angular velocity of the rigid-body to be created.
+    pub fn angvel(mut self, angvel: AngVector<f32>) -> Self {
+        self.angvel = angvel;
+        self
+    }
+
+    /// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium.
+    pub fn can_sleep(mut self, can_sleep: bool) -> Self {
+        self.can_sleep = can_sleep;
+        self
+    }
+
+    /// Build a new rigid-body with the parameters configured with this builder.
+    pub fn build(&self) -> RigidBody {
+        let mut rb = RigidBody::new();
+        rb.predicted_position = self.position; // FIXME: compute the correct value?
+        rb.set_position(self.position);
+        rb.linvel = self.linvel;
+        rb.angvel = self.angvel;
+        rb.body_status = self.body_status;
+
+        if !self.can_sleep {
+            rb.activation.threshold = -1.0;
+        }
+
+        rb
+    }
+}
+
+/// The activation status of a body.
+///
+/// This controls whether a body is sleeping or not.
+/// If the threshold is negative, the body never sleeps.
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ActivationStatus {
+    /// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
+    pub threshold: f32,
+    /// The current pseudo-kinetic energy of the body.
+    pub energy: f32,
+    /// Is this body already sleeping?
+    pub sleeping: bool,
+}
+
+impl ActivationStatus {
+    /// The default amount of energy bellow which a body can be put to sleep by nphysics.
+    pub fn default_threshold() -> f32 {
+        0.01
+    }
+
+    /// Create a new activation status initialised with the default activation threshold and is active.
+    pub fn new_active() -> Self {
+        ActivationStatus {
+            threshold: Self::default_threshold(),
+            energy: Self::default_threshold() * 4.0,
+            sleeping: false,
+        }
+    }
+
+    /// Create a new activation status initialised with the default activation threshold and is inactive.
+    pub fn new_inactive() -> Self {
+        ActivationStatus {
+            threshold: Self::default_threshold(),
+            energy: 0.0,
+            sleeping: true,
+        }
+    }
+
+    /// Returns `true` if the body is not asleep.
+    #[inline]
+    pub fn is_active(&self) -> bool {
+        self.energy != 0.0
+    }
+}

src/dynamics/rigid_body_set.rs

@@ -0,0 +1,518 @@
+#[cfg(feature = "parallel")]
+use rayon::prelude::*;
+
+use crate::data::arena::Arena;
+use crate::dynamics::{Joint, RigidBody};
+use crate::geometry::{ColliderSet, ContactPair, InteractionGraph};
+use crossbeam::channel::{Receiver, Sender};
+use std::ops::{Deref, DerefMut, Index, IndexMut};
+
+/// A mutable reference to a rigid-body.
+pub struct RigidBodyMut<'a> {
+    rb: &'a mut RigidBody,
+    was_sleeping: bool,
+    handle: RigidBodyHandle,
+    sender: &'a Sender<RigidBodyHandle>,
+}
+
+impl<'a> RigidBodyMut<'a> {
+    fn new(
+        handle: RigidBodyHandle,
+        rb: &'a mut RigidBody,
+        sender: &'a Sender<RigidBodyHandle>,
+    ) -> Self {
+        Self {
+            was_sleeping: rb.is_sleeping(),
+            handle,
+            sender,
+            rb,
+        }
+    }
+}
+
+impl<'a> Deref for RigidBodyMut<'a> {
+    type Target = RigidBody;
+    fn deref(&self) -> &RigidBody {
+        &*self.rb
+    }
+}
+
+impl<'a> DerefMut for RigidBodyMut<'a> {
+    fn deref_mut(&mut self) -> &mut RigidBody {
+        self.rb
+    }
+}
+
+impl<'a> Drop for RigidBodyMut<'a> {
+    fn drop(&mut self) {
+        if self.was_sleeping && !self.rb.is_sleeping() {
+            self.sender.send(self.handle).unwrap();
+        }
+    }
+}
+
+/// The unique handle of a rigid body added to a `RigidBodySet`.
+pub type RigidBodyHandle = crate::data::arena::Index;
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A pair of rigid body handles.
+pub struct BodyPair {
+    /// The first rigid body handle.
+    pub body1: RigidBodyHandle,
+    /// The second rigid body handle.
+    pub body2: RigidBodyHandle,
+}
+
+impl BodyPair {
+    pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self {
+        BodyPair { body1, body2 }
+    }
+
+    pub(crate) fn swap(self) -> Self {
+        Self::new(self.body2, self.body1)
+    }
+}
+
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A set of rigid bodies that can be handled by a physics pipeline.
+pub struct RigidBodySet {
+    // NOTE: the pub(crate) are needed by the broad phase
+    // to avoid borrowing issues. It is also needed for
+    // parallelism because the `Receiver` breaks the Sync impl.
+    // Could we avoid this?
+    pub(crate) bodies: Arena<RigidBody>,
+    pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
+    pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
+    // Set of inactive bodies which have been modified.
+    // This typically include static bodies which have been modified.
+    pub(crate) modified_inactive_set: Vec<RigidBodyHandle>,
+    pub(crate) active_islands: Vec<usize>,
+    active_set_timestamp: u32,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    can_sleep: Vec<RigidBodyHandle>, // Workspace.
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    stack: Vec<RigidBodyHandle>, // Workspace.
+    #[cfg_attr(
+        feature = "serde-serialize",
+        serde(skip, default = "crossbeam::channel::unbounded")
+    )]
+    activation_channel: (Sender<RigidBodyHandle>, Receiver<RigidBodyHandle>),
+}
+
+impl RigidBodySet {
+    /// Create a new empty set of rigid bodies.
+    pub fn new() -> Self {
+        RigidBodySet {
+            bodies: Arena::new(),
+            active_dynamic_set: Vec::new(),
+            active_kinematic_set: Vec::new(),
+            modified_inactive_set: Vec::new(),
+            active_islands: Vec::new(),
+            active_set_timestamp: 0,
+            can_sleep: Vec::new(),
+            stack: Vec::new(),
+            activation_channel: crossbeam::channel::unbounded(),
+        }
+    }
+
+    /// An always-invalid rigid-body handle.
+    pub fn invalid_handle() -> RigidBodyHandle {
+        RigidBodyHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64)
+    }
+
+    /// The number of rigid bodies on this set.
+    pub fn len(&self) -> usize {
+        self.bodies.len()
+    }
+
+    pub(crate) fn activate(&mut self, handle: RigidBodyHandle) {
+        let mut rb = &mut self.bodies[handle];
+        if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
+            rb.active_set_id = self.active_dynamic_set.len();
+            self.active_dynamic_set.push(handle);
+        }
+    }
+
+    /// Is the given body handle valid?
+    pub fn contains(&self, handle: RigidBodyHandle) -> bool {
+        self.bodies.contains(handle)
+    }
+
+    /// Insert a rigid body into this set and retrieve its handle.
+    pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle {
+        let handle = self.bodies.insert(rb);
+        let rb = &mut self.bodies[handle];
+        rb.active_set_id = self.active_dynamic_set.len();
+
+        if !rb.is_sleeping() && rb.is_dynamic() {
+            self.active_dynamic_set.push(handle);
+        }
+
+        if rb.is_kinematic() {
+            self.active_kinematic_set.push(handle);
+        }
+
+        if !rb.is_dynamic() {
+            self.modified_inactive_set.push(handle);
+        }
+
+        handle
+    }
+
+    pub(crate) fn num_islands(&self) -> usize {
+        self.active_islands.len() - 1
+    }
+
+    pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option<RigidBody> {
+        let rb = self.bodies.remove(handle)?;
+        // Remove this body from the active dynamic set.
+        if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) {
+            self.active_dynamic_set.swap_remove(rb.active_set_id);
+
+            if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) {
+                self.bodies[*replacement].active_set_id = rb.active_set_id;
+            }
+        }
+
+        Some(rb)
+    }
+
+    /// Forces the specified rigid-body to wake up if it is dynamic.
+    pub fn wake_up(&mut self, handle: RigidBodyHandle) {
+        if let Some(rb) = self.bodies.get_mut(handle) {
+            if rb.is_dynamic() {
+                rb.wake_up();
+
+                if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) {
+                    rb.active_set_id = self.active_dynamic_set.len();
+                    self.active_dynamic_set.push(handle);
+                }
+            }
+        }
+    }
+
+    /// Gets the rigid-body with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the rigid-body at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the rigid-body position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// Using this is discouraged in favor of `self.get(handle)` which does not
+    /// suffer form the ABA problem.
+    pub fn get_unknown_gen(&self, i: usize) -> Option<(&RigidBody, RigidBodyHandle)> {
+        self.bodies.get_unknown_gen(i)
+    }
+
+    /// Gets a mutable reference to the rigid-body with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the rigid-body at position `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the rigid-body position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// Using this is discouraged in favor of `self.get_mut(handle)` which does not
+    /// suffer form the ABA problem.
+    pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> {
+        self.bodies.get_unknown_gen_mut(i)
+    }
+
+    /// Gets the rigid-body with the given handle.
+    pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> {
+        self.bodies.get(handle)
+    }
+
+    /// Gets a mutable reference to the rigid-body with the given handle.
+    pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<RigidBodyMut> {
+        let sender = &self.activation_channel.0;
+        self.bodies
+            .get_mut(handle)
+            .map(|rb| RigidBodyMut::new(handle, rb, sender))
+    }
+
+    pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
+        self.bodies.get_mut(handle)
+    }
+
+    pub(crate) fn get2_mut_internal(
+        &mut self,
+        h1: RigidBodyHandle,
+        h2: RigidBodyHandle,
+    ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) {
+        self.bodies.get2_mut(h1, h2)
+    }
+
+    /// Iterates through all the rigid-bodies on this set.
+    pub fn iter(&self) -> impl Iterator<Item = (RigidBodyHandle, &RigidBody)> {
+        self.bodies.iter()
+    }
+
+    /// Iterates mutably through all the rigid-bodies on this set.
+    pub fn iter_mut(&mut self) -> impl Iterator<Item = (RigidBodyHandle, RigidBodyMut)> {
+        let sender = &self.activation_channel.0;
+        self.bodies
+            .iter_mut()
+            .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender)))
+    }
+
+    /// Iter through all the active dynamic rigid-bodies on this set.
+    pub fn iter_active_dynamic<'a>(
+        &'a self,
+    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
+        let bodies: &'a _ = &self.bodies;
+        self.active_dynamic_set
+            .iter()
+            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+    }
+
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn iter_active_island<'a>(
+        &'a self,
+        island_id: usize,
+    ) -> impl Iterator<Item = (RigidBodyHandle, &'a RigidBody)> {
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        let bodies: &'a _ = &self.bodies;
+        self.active_dynamic_set[island_range]
+            .iter()
+            .filter_map(move |h| Some((*h, bodies.get(*h)?)))
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_active_body_mut_internal(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_dynamic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+
+        for handle in &self.active_kinematic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_active_dynamic_body_mut_internal(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_dynamic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    pub(crate) fn foreach_active_kinematic_body_mut_internal(
+        &mut self,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        for handle in &self.active_kinematic_set {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[inline(always)]
+    #[cfg(not(feature = "parallel"))]
+    pub(crate) fn foreach_active_island_body_mut_internal(
+        &mut self,
+        island_id: usize,
+        mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
+    ) {
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        for handle in &self.active_dynamic_set[island_range] {
+            if let Some(rb) = self.bodies.get_mut(*handle) {
+                f(*handle, rb)
+            }
+        }
+    }
+
+    #[cfg(feature = "parallel")]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub(crate) fn foreach_active_island_body_mut_internal_parallel(
+        &mut self,
+        island_id: usize,
+        f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
+    ) {
+        use std::sync::atomic::Ordering;
+
+        let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
+        let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _);
+        self.active_dynamic_set[island_range]
+            .par_iter()
+            .for_each_init(
+                || bodies.load(Ordering::Relaxed),
+                |bodies, handle| {
+                    let bodies: &mut Arena<RigidBody> = unsafe { std::mem::transmute(*bodies) };
+                    if let Some(rb) = bodies.get_mut(*handle) {
+                        f(*handle, rb)
+                    }
+                },
+            );
+    }
+
+    // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] {
+    //     &self.active_dynamic_set
+    // }
+
+    pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
+        self.active_islands[island_id]..self.active_islands[island_id + 1]
+    }
+
+    pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
+        &self.active_dynamic_set[self.active_island_range(island_id)]
+    }
+
+    pub(crate) fn maintain_active_set(&mut self) {
+        for handle in self.activation_channel.1.try_iter() {
+            if let Some(rb) = self.bodies.get_mut(handle) {
+                // Push the body to the active set if it is not
+                // sleeping and if it is not already inside of the active set.
+                if !rb.is_sleeping() // May happen if the body was put to sleep manually.
+                    && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set.
+                    && self.active_dynamic_set.get(rb.active_set_id) != Some(&handle)
+                {
+                    rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
+                    self.active_dynamic_set.push(handle);
+                }
+            }
+        }
+    }
+
+    pub(crate) fn update_active_set_with_contacts(
+        &mut self,
+        colliders: &ColliderSet,
+        contact_graph: &InteractionGraph<ContactPair>,
+        joint_graph: &InteractionGraph<Joint>,
+        min_island_size: usize,
+    ) {
+        assert!(
+            min_island_size > 0,
+            "The minimum island size must be at least 1."
+        );
+
+        // Update the energy of every rigid body and
+        // keep only those that may not sleep.
+        //        let t = instant::now();
+        self.active_set_timestamp += 1;
+        self.stack.clear();
+        self.can_sleep.clear();
+
+        // NOTE: the `.rev()` is here so that two successive timesteps preserve
+        // the order of the bodies in the `active_dynamic_set` vec. This reversal
+        // does not seem to affect performances nor stability. However it makes
+        // debugging slightly nicer so we keep this rev.
+        for h in self.active_dynamic_set.drain(..).rev() {
+            let rb = &mut self.bodies[h];
+            rb.update_energy();
+            if rb.activation.energy <= rb.activation.threshold {
+                // Mark them as sleeping for now. This will
+                // be set to false during the graph traversal
+                // if it should not be put to sleep.
+                rb.activation.sleeping = true;
+                self.can_sleep.push(h);
+            } else {
+                self.stack.push(h);
+            }
+        }
+
+        //        println!("Selection: {}", instant::now() - t);
+
+        //        let t = instant::now();
+        // Propagation of awake state and awake island computation through the
+        // traversal of the interaction graph.
+        self.active_islands.clear();
+        self.active_islands.push(0);
+
+        // The max avoid underflow when the stack is empty.
+        let mut island_marker = self.stack.len().max(1) - 1;
+
+        while let Some(handle) = self.stack.pop() {
+            let rb = &mut self.bodies[handle];
+
+            if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
+                // We already visited this body and its neighbors.
+                // Also, we don't propagate awake state through static bodies.
+                continue;
+            } else if self.stack.len() < island_marker {
+                if self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
+                    >= min_island_size
+                {
+                    // We are starting a new island.
+                    self.active_islands.push(self.active_dynamic_set.len());
+                }
+
+                island_marker = self.stack.len();
+            }
+
+            rb.wake_up();
+            rb.active_island_id = self.active_islands.len() - 1;
+            rb.active_set_id = self.active_dynamic_set.len();
+            rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id];
+            rb.active_set_timestamp = self.active_set_timestamp;
+            self.active_dynamic_set.push(handle);
+
+            // Read all the contacts and push objects touching this one.
+            for collider_handle in &rb.colliders {
+                let collider = &colliders[*collider_handle];
+
+                for inter in contact_graph.interactions_with(collider.contact_graph_index) {
+                    for manifold in &inter.2.manifolds {
+                        if manifold.num_active_contacts() > 0 {
+                            let other =
+                                crate::utils::other_handle((inter.0, inter.1), *collider_handle);
+                            let other_body = colliders[other].parent;
+                            self.stack.push(other_body);
+                            break;
+                        }
+                    }
+                }
+            }
+
+            for inter in joint_graph.interactions_with(rb.joint_graph_index) {
+                let other = crate::utils::other_handle((inter.0, inter.1), handle);
+                self.stack.push(other);
+            }
+        }
+
+        self.active_islands.push(self.active_dynamic_set.len());
+        //        println!(
+        //            "Extraction: {}, num islands: {}",
+        //            instant::now() - t,
+        //            self.active_islands.len() - 1
+        //        );
+
+        // Actually put to sleep bodies which have not been detected as awake.
+        //        let t = instant::now();
+        for h in &self.can_sleep {
+            let b = &mut self.bodies[*h];
+            if b.activation.sleeping {
+                b.sleep();
+            }
+        }
+        //        println!("Activation: {}", instant::now() - t);
+    }
+}
+
+impl Index<RigidBodyHandle> for RigidBodySet {
+    type Output = RigidBody;
+
+    fn index(&self, index: RigidBodyHandle) -> &RigidBody {
+        &self.bodies[index]
+    }
+}
+
+impl IndexMut<RigidBodyHandle> for RigidBodySet {
+    fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody {
+        &mut self.bodies[index]
+    }
+}

src/dynamics/solver/categorization.rs

@@ -0,0 +1,70 @@
+use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory};
+
+pub(crate) fn categorize_position_contacts(
+    bodies: &RigidBodySet,
+    manifolds: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    out_point_point_ground: &mut Vec<ContactManifoldIndex>,
+    out_plane_point_ground: &mut Vec<ContactManifoldIndex>,
+    out_point_point: &mut Vec<ContactManifoldIndex>,
+    out_plane_point: &mut Vec<ContactManifoldIndex>,
+) {
+    for manifold_i in manifold_indices {
+        let manifold = &manifolds[*manifold_i];
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            match manifold.kinematics.category {
+                KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i),
+                KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i),
+            }
+        } else {
+            match manifold.kinematics.category {
+                KinematicsCategory::PointPoint => out_point_point.push(*manifold_i),
+                KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i),
+            }
+        }
+    }
+}
+
+pub(crate) fn categorize_velocity_contacts(
+    bodies: &RigidBodySet,
+    manifolds: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    out_ground: &mut Vec<ContactManifoldIndex>,
+    out_not_ground: &mut Vec<ContactManifoldIndex>,
+) {
+    for manifold_i in manifold_indices {
+        let manifold = &manifolds[*manifold_i];
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            out_ground.push(*manifold_i)
+        } else {
+            out_not_ground.push(*manifold_i)
+        }
+    }
+}
+
+pub(crate) fn categorize_joints(
+    bodies: &RigidBodySet,
+    joints: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    ground_joints: &mut Vec<JointIndex>,
+    nonground_joints: &mut Vec<JointIndex>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints[*joint_i].weight;
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        if !rb1.is_dynamic() || !rb2.is_dynamic() {
+            ground_joints.push(*joint_i);
+        } else {
+            nonground_joints.push(*joint_i);
+        }
+    }
+}

src/dynamics/solver/delta_vel.rs

@@ -0,0 +1,18 @@
+use crate::math::{AngVector, Vector};
+use na::{Scalar, SimdRealField};
+
+#[derive(Copy, Clone, Debug)]
+//#[repr(align(64))]
+pub(crate) struct DeltaVel<N: Scalar> {
+    pub linear: Vector<N>,
+    pub angular: AngVector<N>,
+}
+
+impl<N: SimdRealField> DeltaVel<N> {
+    pub fn zero() -> Self {
+        Self {
+            linear: na::zero(),
+            angular: na::zero(),
+        }
+    }
+}

src/dynamics/solver/interaction_groups.rs

@@ -0,0 +1,434 @@
+use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
+    vec_map::VecMap,
+};
+
+pub(crate) trait PairInteraction {
+    fn body_pair(&self) -> BodyPair;
+}
+
+impl<'a> PairInteraction for &'a mut ContactManifold {
+    fn body_pair(&self) -> BodyPair {
+        self.body_pair
+    }
+}
+
+impl<'a> PairInteraction for JointGraphEdge {
+    fn body_pair(&self) -> BodyPair {
+        BodyPair::new(self.weight.body1, self.weight.body2)
+    }
+}
+
+#[cfg(feature = "parallel")]
+pub(crate) struct ParallelInteractionGroups {
+    bodies_color: Vec<u128>,         // Workspace.
+    interaction_indices: Vec<usize>, // Workspace.
+    interaction_colors: Vec<usize>,  // Workspace.
+    sorted_interactions: Vec<usize>,
+    groups: Vec<usize>,
+}
+
+#[cfg(feature = "parallel")]
+impl ParallelInteractionGroups {
+    pub fn new() -> Self {
+        Self {
+            bodies_color: Vec::new(),
+            interaction_indices: Vec::new(),
+            interaction_colors: Vec::new(),
+            sorted_interactions: Vec::new(),
+            groups: Vec::new(),
+        }
+    }
+
+    pub fn group(&self, i: usize) -> &[usize] {
+        let range = self.groups[i]..self.groups[i + 1];
+        &self.sorted_interactions[range]
+    }
+    pub fn num_groups(&self) -> usize {
+        self.groups.len() - 1
+    }
+
+    pub fn group_interactions<Interaction: PairInteraction>(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[Interaction],
+        interaction_indices: &[usize],
+    ) {
+        let num_island_bodies = bodies.active_island(island_id).len();
+        self.bodies_color.clear();
+        self.interaction_indices.clear();
+        self.groups.clear();
+        self.sorted_interactions.clear();
+        self.interaction_colors.clear();
+
+        let mut color_len = [0; 128];
+        self.bodies_color.resize(num_island_bodies, 0u128);
+        self.interaction_indices
+            .extend_from_slice(interaction_indices);
+        self.interaction_colors.resize(interaction_indices.len(), 0);
+        let bcolors = &mut self.bodies_color;
+
+        for (interaction_id, color) in self
+            .interaction_indices
+            .iter()
+            .zip(self.interaction_colors.iter_mut())
+        {
+            let body_pair = interactions[*interaction_id].body_pair();
+            let rb1 = &bodies[body_pair.body1];
+            let rb2 = &bodies[body_pair.body2];
+
+            match (rb1.is_static(), rb2.is_static()) {
+                (false, false) => {
+                    let color_mask =
+                        bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                }
+                (true, false) => {
+                    let color_mask = bcolors[rb2.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb2.active_set_offset] |= 1 << *color;
+                }
+                (false, true) => {
+                    let color_mask = bcolors[rb1.active_set_offset];
+                    *color = (!color_mask).trailing_zeros() as usize;
+                    color_len[*color] += 1;
+                    bcolors[rb1.active_set_offset] |= 1 << *color;
+                }
+                (true, true) => unreachable!(),
+            }
+        }
+
+        let mut sort_offsets = [0; 128];
+        let mut last_offset = 0;
+
+        for i in 0..128 {
+            if color_len[i] == 0 {
+                break;
+            }
+
+            self.groups.push(last_offset);
+            sort_offsets[i] = last_offset;
+            last_offset += color_len[i];
+        }
+
+        self.sorted_interactions
+            .resize(interaction_indices.len(), 0);
+
+        for (interaction_id, color) in interaction_indices
+            .iter()
+            .zip(self.interaction_colors.iter())
+        {
+            self.sorted_interactions[sort_offsets[*color]] = *interaction_id;
+            sort_offsets[*color] += 1;
+        }
+
+        self.groups.push(self.sorted_interactions.len());
+    }
+}
+
+pub(crate) struct InteractionGroups {
+    #[cfg(feature = "simd-is-enabled")]
+    buckets: VecMap<([usize; SIMD_WIDTH], usize)>,
+    #[cfg(feature = "simd-is-enabled")]
+    body_masks: Vec<u128>,
+    #[cfg(feature = "simd-is-enabled")]
+    pub grouped_interactions: Vec<usize>,
+    pub nongrouped_interactions: Vec<usize>,
+}
+
+impl InteractionGroups {
+    pub fn new() -> Self {
+        Self {
+            #[cfg(feature = "simd-is-enabled")]
+            buckets: VecMap::new(),
+            #[cfg(feature = "simd-is-enabled")]
+            body_masks: Vec::new(),
+            #[cfg(feature = "simd-is-enabled")]
+            grouped_interactions: Vec::new(),
+            nongrouped_interactions: Vec::new(),
+        }
+    }
+
+    // FIXME: there is a lot of duplicated code with group_manifolds here.
+    // But we don't refactor just now because we may end up with distinct
+    // grouping strategies in the future.
+    #[cfg(not(feature = "simd-is-enabled"))]
+    pub fn group_joints(
+        &mut self,
+        _island_id: usize,
+        _bodies: &RigidBodySet,
+        _interactions: &[JointGraphEdge],
+        interaction_indices: &[JointIndex],
+    ) {
+        self.nongrouped_interactions
+            .extend_from_slice(interaction_indices);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn group_joints(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[JointGraphEdge],
+        interaction_indices: &[JointIndex],
+    ) {
+        // NOTE: in 3D we have up to 10 different joint types.
+        // In 2D we only have 5 joint types.
+        #[cfg(feature = "dim3")]
+        const NUM_JOINT_TYPES: usize = 10;
+        #[cfg(feature = "dim2")]
+        const NUM_JOINT_TYPES: usize = 5;
+
+        // The j-th bit of joint_type_conflicts[i] indicates that the
+        // j-th bucket contains a joint with a type different than `i`.
+        let mut joint_type_conflicts = [0u128; NUM_JOINT_TYPES];
+
+        // Note: each bit of a body mask indicates what bucket already contains
+        // a constraints involving this body.
+        // FIXME: currently, this is a bit overconservative because when a bucket
+        // is full, we don't clear the corresponding body mask bit. This may result
+        // in less grouped constraints.
+        self.body_masks
+            .resize(bodies.active_island(island_id).len(), 0u128);
+
+        // NOTE: each bit of the occupied mask indicates what bucket already
+        // contains at least one constraint.
+        let mut occupied_mask = 0u128;
+
+        for interaction_i in interaction_indices {
+            let interaction = &interactions[*interaction_i].weight;
+            let body1 = &bodies[interaction.body1];
+            let body2 = &bodies[interaction.body2];
+            let is_static1 = !body1.is_dynamic();
+            let is_static2 = !body2.is_dynamic();
+
+            if is_static1 && is_static2 {
+                continue;
+            }
+
+            let ijoint = interaction.params.type_id();
+            let i1 = body1.active_set_offset;
+            let i2 = body2.active_set_offset;
+            let conflicts =
+                self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
+            let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
+            let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
+
+            let target_index = if conflictfree_occupied_targets != 0 {
+                // Try to fill partial WContacts first.
+                conflictfree_occupied_targets.trailing_zeros()
+            } else {
+                conflictfree_targets.trailing_zeros()
+            };
+
+            if target_index == 128 {
+                // The interaction conflicts with every bucket we can manage.
+                // So push it in an nongrouped interaction list that won't be combined with
+                // any other interactions.
+                self.nongrouped_interactions.push(*interaction_i);
+                continue;
+            }
+
+            let target_mask_bit = 1 << target_index;
+
+            let bucket = self
+                .buckets
+                .entry(target_index as usize)
+                .or_insert_with(|| ([0; SIMD_WIDTH], 0));
+
+            if bucket.1 == SIMD_LAST_INDEX {
+                // We completed our group.
+                (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
+                self.grouped_interactions.extend_from_slice(&bucket.0);
+                bucket.1 = 0;
+                occupied_mask &= !target_mask_bit;
+
+                for k in 0..NUM_JOINT_TYPES {
+                    joint_type_conflicts[k] &= !target_mask_bit;
+                }
+            } else {
+                (bucket.0)[bucket.1] = *interaction_i;
+                bucket.1 += 1;
+                occupied_mask |= target_mask_bit;
+
+                for k in 0..ijoint {
+                    joint_type_conflicts[k] |= target_mask_bit;
+                }
+                for k in ijoint + 1..NUM_JOINT_TYPES {
+                    joint_type_conflicts[k] |= target_mask_bit;
+                }
+            }
+
+            // NOTE: static bodies don't transmit forces. Therefore they don't
+            // imply any interaction conflicts.
+            if !is_static1 {
+                self.body_masks[i1] |= target_mask_bit;
+            }
+
+            if !is_static2 {
+                self.body_masks[i2] |= target_mask_bit;
+            }
+        }
+
+        self.nongrouped_interactions.extend(
+            self.buckets
+                .values()
+                .flat_map(|e| e.0.iter().take(e.1).copied()),
+        );
+        self.buckets.clear();
+        self.body_masks.iter_mut().for_each(|e| *e = 0);
+
+        assert!(
+            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            "Invalid SIMD contact grouping."
+        );
+
+        //        println!(
+        //            "Num grouped interactions: {}, nongrouped: {}",
+        //            self.grouped_interactions.len(),
+        //            self.nongrouped_interactions.len()
+        //        );
+    }
+
+    pub fn clear_groups(&mut self) {
+        #[cfg(feature = "simd-is-enabled")]
+        self.grouped_interactions.clear();
+        self.nongrouped_interactions.clear();
+    }
+
+    #[cfg(not(feature = "simd-is-enabled"))]
+    pub fn group_manifolds(
+        &mut self,
+        _island_id: usize,
+        _bodies: &RigidBodySet,
+        _interactions: &[&mut ContactManifold],
+        interaction_indices: &[ContactManifoldIndex],
+    ) {
+        self.nongrouped_interactions
+            .extend_from_slice(interaction_indices);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn group_manifolds(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        interactions: &[&mut ContactManifold],
+        interaction_indices: &[ContactManifoldIndex],
+    ) {
+        // Note: each bit of a body mask indicates what bucket already contains
+        // a constraints involving this body.
+        // FIXME: currently, this is a bit overconservative because when a bucket
+        // is full, we don't clear the corresponding body mask bit. This may result
+        // in less grouped contacts.
+        // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
+        self.body_masks
+            .resize(bodies.active_island(island_id).len(), 0u128);
+
+        // NOTE: each bit of the occupied mask indicates what bucket already
+        // contains at least one constraint.
+        let mut occupied_mask = 0u128;
+        let max_interaction_points = interaction_indices
+            .iter()
+            .map(|i| interactions[*i].num_active_contacts())
+            .max()
+            .unwrap_or(1);
+
+        // FIXME: find a way to reduce the number of iteration.
+        // There must be a way to iterate just once on every interaction indices
+        // instead of MAX_MANIFOLD_POINTS times.
+        for k in 1..=max_interaction_points {
+            for interaction_i in interaction_indices {
+                let interaction = &interactions[*interaction_i];
+
+                // FIXME: how could we avoid iterating
+                // on each interaction at every iteration on k?
+                if interaction.num_active_contacts() != k {
+                    continue;
+                }
+
+                let body1 = &bodies[interaction.body_pair.body1];
+                let body2 = &bodies[interaction.body_pair.body2];
+                let is_static1 = !body1.is_dynamic();
+                let is_static2 = !body2.is_dynamic();
+
+                // FIXME: don't generate interactions between static bodies in the first place.
+                if is_static1 && is_static2 {
+                    continue;
+                }
+
+                let i1 = body1.active_set_offset;
+                let i2 = body2.active_set_offset;
+                let conflicts = self.body_masks[i1] | self.body_masks[i2];
+                let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
+                let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;
+
+                let target_index = if conflictfree_occupied_targets != 0 {
+                    // Try to fill partial WContacts first.
+                    conflictfree_occupied_targets.trailing_zeros()
+                } else {
+                    conflictfree_targets.trailing_zeros()
+                };
+
+                if target_index == 128 {
+                    // The interaction conflicts with every bucket we can manage.
+                    // So push it in an nongrouped interaction list that won't be combined with
+                    // any other interactions.
+                    self.nongrouped_interactions.push(*interaction_i);
+                    continue;
+                }
+
+                let target_mask_bit = 1 << target_index;
+
+                let bucket = self
+                    .buckets
+                    .entry(target_index as usize)
+                    .or_insert_with(|| ([0; SIMD_WIDTH], 0));
+
+                if bucket.1 == SIMD_LAST_INDEX {
+                    // We completed our group.
+                    (bucket.0)[SIMD_LAST_INDEX] = *interaction_i;
+                    self.grouped_interactions.extend_from_slice(&bucket.0);
+                    bucket.1 = 0;
+                    occupied_mask = occupied_mask & (!target_mask_bit);
+                } else {
+                    (bucket.0)[bucket.1] = *interaction_i;
+                    bucket.1 += 1;
+                    occupied_mask = occupied_mask | target_mask_bit;
+                }
+
+                // NOTE: static bodies don't transmit forces. Therefore they don't
+                // imply any interaction conflicts.
+                if !is_static1 {
+                    self.body_masks[i1] |= target_mask_bit;
+                }
+
+                if !is_static2 {
+                    self.body_masks[i2] |= target_mask_bit;
+                }
+            }
+
+            self.nongrouped_interactions.extend(
+                self.buckets
+                    .values()
+                    .flat_map(|e| e.0.iter().take(e.1).copied()),
+            );
+            self.buckets.clear();
+            self.body_masks.iter_mut().for_each(|e| *e = 0);
+            occupied_mask = 0u128;
+        }
+
+        assert!(
+            self.grouped_interactions.len() % SIMD_WIDTH == 0,
+            "Invalid SIMD contact grouping."
+        );
+    }
+}

src/dynamics/solver/island_solver.rs

@@ -0,0 +1,73 @@
+use super::{PositionSolver, VelocitySolver};
+use crate::counters::Counters;
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+
+pub struct IslandSolver {
+    velocity_solver: VelocitySolver,
+    position_solver: PositionSolver,
+}
+
+impl IslandSolver {
+    pub fn new() -> Self {
+        Self {
+            velocity_solver: VelocitySolver::new(),
+            position_solver: PositionSolver::new(),
+        }
+    }
+
+    pub fn solve_island(
+        &mut self,
+        island_id: usize,
+        counters: &mut Counters,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+        joints: &mut [JointGraphEdge],
+        joint_indices: &[JointIndex],
+    ) {
+        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+            counters.solver.velocity_assembly_time.resume();
+            self.velocity_solver.init_constraints(
+                island_id,
+                params,
+                bodies,
+                manifolds,
+                &manifold_indices,
+                joints,
+                &joint_indices,
+            );
+            counters.solver.velocity_assembly_time.pause();
+
+            counters.solver.velocity_resolution_time.resume();
+            self.velocity_solver
+                .solve_constraints(island_id, params, bodies, manifolds, joints);
+            counters.solver.velocity_resolution_time.pause();
+
+            counters.solver.position_assembly_time.resume();
+            self.position_solver.init_constraints(
+                island_id,
+                params,
+                bodies,
+                manifolds,
+                &manifold_indices,
+                joints,
+                &joint_indices,
+            );
+            counters.solver.position_assembly_time.pause();
+        }
+
+        counters.solver.velocity_update_time.resume();
+        bodies
+            .foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt()));
+        counters.solver.velocity_update_time.pause();
+
+        if manifold_indices.len() != 0 || joint_indices.len() != 0 {
+            counters.solver.position_resolution_time.resume();
+            self.position_solver
+                .solve_constraints(island_id, params, bodies);
+            counters.solver.position_resolution_time.pause();
+        }
+    }
+}

src/dynamics/solver/joint_constraint/ball_position_constraint.rs

@@ -0,0 +1,165 @@
+use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+#[cfg(feature = "dim2")]
+use crate::math::SdpMatrix;
+use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+
+#[derive(Debug)]
+pub(crate) struct BallPositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    local_com1: Point<f32>,
+    local_com2: Point<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    local_anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+}
+
+impl BallPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
+        Self {
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
+            im1: rb1.mass_properties.inv_mass,
+            im2: rb2.mass_properties.inv_mass,
+            ii1: rb1.world_inv_inertia_sqrt.squared(),
+            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            local_anchor1: cparams.local_anchor1,
+            local_anchor2: cparams.local_anchor2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+
+        let com1 = position1 * self.local_com1;
+        let com2 = position2 * self.local_com2;
+
+        let err = anchor1 - anchor2;
+
+        let centered_anchor1 = anchor1 - com1;
+        let centered_anchor2 = anchor2 - com2;
+
+        let cmat1 = centered_anchor1.gcross_matrix();
+        let cmat2 = centered_anchor2.gcross_matrix();
+
+        // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
+        // because it is anti-symmetric.
+        #[cfg(feature = "dim3")]
+        let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
+            + self.ii2.quadform(&cmat2).add_diagonal(self.im2);
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way. It is also
+        // faster because in 2D lhs will be symmetric.
+        #[cfg(feature = "dim2")]
+        let lhs = {
+            let m11 =
+                self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
+            let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
+            let m22 =
+                self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
+            SdpMatrix::new(m11, m12, m22)
+        };
+
+        let inv_lhs = lhs.inverse_unchecked();
+        let impulse = inv_lhs * -(err * params.joint_erp);
+
+        position1.translation.vector += self.im1 * impulse;
+        position2.translation.vector -= self.im2 * impulse;
+
+        let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse));
+        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
+
+        position1.rotation = Rotation::new(angle1) * position1.rotation;
+        position2.rotation = Rotation::new(angle2) * position2.rotation;
+
+        positions[self.position1 as usize] = position1;
+        positions[self.position2 as usize] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct BallPositionGroundConstraint {
+    position2: usize,
+    anchor1: Point<f32>,
+    im2: f32,
+    ii2: AngularInertia<f32>,
+    local_anchor2: Point<f32>,
+    local_com2: Point<f32>,
+}
+
+impl BallPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &BallJoint,
+        flipped: bool,
+    ) -> Self {
+        if flipped {
+            // Note the only thing that is flipped here
+            // are the local_anchors. The rb1 and rb2 have
+            // already been flipped by the caller.
+            Self {
+                anchor1: rb1.predicted_position * cparams.local_anchor2,
+                im2: rb2.mass_properties.inv_mass,
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                local_anchor2: cparams.local_anchor1,
+                position2: rb2.active_set_offset,
+                local_com2: rb2.mass_properties.local_com,
+            }
+        } else {
+            Self {
+                anchor1: rb1.predicted_position * cparams.local_anchor1,
+                im2: rb2.mass_properties.inv_mass,
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                local_anchor2: cparams.local_anchor2,
+                position2: rb2.active_set_offset,
+                local_com2: rb2.mass_properties.local_com,
+            }
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        let anchor2 = position2 * self.local_anchor2;
+        let com2 = position2 * self.local_com2;
+
+        let err = self.anchor1 - anchor2;
+        let centered_anchor2 = anchor2 - com2;
+        let cmat2 = centered_anchor2.gcross_matrix();
+
+        #[cfg(feature = "dim3")]
+        let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2);
+
+        #[cfg(feature = "dim2")]
+        let lhs = {
+            let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2;
+            let m12 = cmat2.x * cmat2.y * self.ii2;
+            let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2;
+            SdpMatrix::new(m11, m12, m22)
+        };
+
+        let inv_lhs = lhs.inverse_unchecked();
+        let impulse = inv_lhs * -(err * params.joint_erp);
+        position2.translation.vector -= self.im2 * impulse;
+
+        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
+        position2.rotation = Rotation::new(angle2) * position2.rotation;
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs

@@ -0,0 +1,199 @@
+use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
+#[cfg(feature = "dim2")]
+use crate::math::SdpMatrix;
+use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use simba::simd::SimdValue;
+
+#[derive(Debug)]
+pub(crate) struct WBallPositionConstraint {
+    position1: [usize; SIMD_WIDTH],
+    position2: [usize; SIMD_WIDTH],
+
+    local_com1: Point<SimdFloat>,
+    local_com2: Point<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1: AngularInertia<SimdFloat>,
+    ii2: AngularInertia<SimdFloat>,
+
+    local_anchor1: Point<SimdFloat>,
+    local_anchor2: Point<SimdFloat>,
+}
+
+impl WBallPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+    ) -> Self {
+        let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
+        let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1 = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        )
+        .squared();
+        let ii2 = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        )
+        .squared();
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        Self {
+            local_com1,
+            local_com2,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            local_anchor1,
+            local_anchor2,
+            position1,
+            position2,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
+        let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
+
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+
+        let com1 = position1 * self.local_com1;
+        let com2 = position2 * self.local_com2;
+
+        let err = anchor1 - anchor2;
+
+        let centered_anchor1 = anchor1 - com1;
+        let centered_anchor2 = anchor2 - com2;
+
+        let cmat1 = centered_anchor1.gcross_matrix();
+        let cmat2 = centered_anchor2.gcross_matrix();
+
+        // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose()
+        // because it is anti-symmetric.
+        #[cfg(feature = "dim3")]
+        let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1)
+            + self.ii2.quadform(&cmat2).add_diagonal(self.im2);
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        let lhs = {
+            let m11 =
+                self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2;
+            let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2;
+            let m22 =
+                self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2;
+            SdpMatrix::new(m11, m12, m22)
+        };
+
+        let inv_lhs = lhs.inverse_unchecked();
+        let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
+
+        position1.translation.vector += impulse * self.im1;
+        position2.translation.vector -= impulse * self.im2;
+
+        let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse));
+        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
+
+        position1.rotation = Rotation::new(angle1) * position1.rotation;
+        position2.rotation = Rotation::new(angle2) * position2.rotation;
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.position1[ii]] = position1.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            positions[self.position2[ii]] = position2.extract(ii);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WBallPositionGroundConstraint {
+    position2: [usize; SIMD_WIDTH],
+    anchor1: Point<SimdFloat>,
+    im2: SimdFloat,
+    ii2: AngularInertia<SimdFloat>,
+    local_anchor2: Point<SimdFloat>,
+    local_com2: Point<SimdFloat>,
+}
+
+impl WBallPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+        let anchor1 = position1
+            * Point::from(array![|ii| if flipped[ii] {
+                cparams[ii].local_anchor2
+            } else {
+                cparams[ii].local_anchor1
+            }; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2 = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        )
+        .squared();
+        let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
+            cparams[ii].local_anchor1
+        } else {
+            cparams[ii].local_anchor2
+        }; SIMD_WIDTH]);
+        let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
+
+        Self {
+            anchor1,
+            im2,
+            ii2,
+            local_anchor2,
+            position2,
+            local_com2,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
+
+        let anchor2 = position2 * self.local_anchor2;
+        let com2 = position2 * self.local_com2;
+
+        let err = self.anchor1 - anchor2;
+        let centered_anchor2 = anchor2 - com2;
+        let cmat2 = centered_anchor2.gcross_matrix();
+
+        #[cfg(feature = "dim3")]
+        let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2);
+
+        #[cfg(feature = "dim2")]
+        let lhs = {
+            let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2;
+            let m12 = cmat2.x * cmat2.y * self.ii2;
+            let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2;
+            SdpMatrix::new(m11, m12, m22)
+        };
+
+        let inv_lhs = lhs.inverse_unchecked();
+        let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp));
+        position2.translation.vector -= impulse * self.im2;
+
+        let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse));
+        position2.rotation = Rotation::new(angle2) * position2.rotation;
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.position2[ii]] = position2.extract(ii);
+        }
+    }
+}

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -0,0 +1,238 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{SdpMatrix, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+
+#[derive(Debug)]
+pub(crate) struct BallVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    rhs: Vector<f32>,
+    pub(crate) impulse: Vector<f32>,
+
+    gcross1: Vector<f32>,
+    gcross2: Vector<f32>,
+
+    inv_lhs: SdpMatrix<f32>,
+
+    im1: f32,
+    im2: f32,
+}
+
+impl BallVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &BallJoint,
+    ) -> Self {
+        let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com;
+        let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com;
+
+        let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
+        let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+
+        let rhs = -(vel1 - vel2);
+        let lhs;
+
+        let cmat1 = anchor1.gcross_matrix();
+        let cmat2 = anchor2.gcross_matrix();
+
+        #[cfg(feature = "dim3")]
+        {
+            lhs = rb2
+                .world_inv_inertia_sqrt
+                .squared()
+                .quadform(&cmat2)
+                .add_diagonal(im2)
+                + rb1
+                    .world_inv_inertia_sqrt
+                    .squared()
+                    .quadform(&cmat1)
+                    .add_diagonal(im1);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let ii1 = rb1.world_inv_inertia_sqrt.squared();
+            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
+            let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
+            let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
+            lhs = SdpMatrix::new(m11, m12, m22)
+        }
+
+        let gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1);
+        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
+
+        let inv_lhs = lhs.inverse_unchecked();
+
+        BallVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            im2,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            gcross1,
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        mj_lambda1.linear += self.im1 * self.impulse;
+        mj_lambda1.angular += self.gcross1.gcross(self.impulse);
+        mj_lambda2.linear -= self.im2 * self.impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let dvel = -vel1 + vel2 + self.rhs;
+
+        let impulse = self.inv_lhs * dvel;
+        self.impulse += impulse;
+
+        mj_lambda1.linear += self.im1 * impulse;
+        mj_lambda1.angular += self.gcross1.gcross(impulse);
+
+        mj_lambda2.linear -= self.im2 * impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::BallJoint(ball) = &mut joint.params {
+            ball.impulse = self.impulse
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct BallVelocityGroundConstraint {
+    mj_lambda2: usize,
+    joint_id: JointIndex,
+    rhs: Vector<f32>,
+    impulse: Vector<f32>,
+    gcross2: Vector<f32>,
+    inv_lhs: SdpMatrix<f32>,
+    im2: f32,
+}
+
+impl BallVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &BallJoint,
+        flipped: bool,
+    ) -> Self {
+        let (anchor1, anchor2) = if flipped {
+            (
+                rb1.position * cparams.local_anchor2 - rb1.world_com,
+                rb2.position * cparams.local_anchor1 - rb2.world_com,
+            )
+        } else {
+            (
+                rb1.position * cparams.local_anchor1 - rb1.world_com,
+                rb2.position * cparams.local_anchor2 - rb2.world_com,
+            )
+        };
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
+        let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
+        let rhs = vel2 - vel1;
+
+        let cmat2 = anchor2.gcross_matrix();
+        let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2);
+
+        let lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            lhs = rb2
+                .world_inv_inertia_sqrt
+                .squared()
+                .quadform(&cmat2)
+                .add_diagonal(im2);
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            let ii2 = rb2.world_inv_inertia_sqrt.squared();
+            let m11 = im2 + cmat2.x * cmat2.x * ii2;
+            let m12 = cmat2.x * cmat2.y * ii2;
+            let m22 = im2 + cmat2.y * cmat2.y * ii2;
+            lhs = SdpMatrix::new(m11, m12, m22)
+        }
+
+        let inv_lhs = lhs.inverse_unchecked();
+
+        BallVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+        mj_lambda2.linear -= self.im2 * self.impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let dvel = vel2 + self.rhs;
+
+        let impulse = self.inv_lhs * dvel;
+        self.impulse += impulse;
+
+        mj_lambda2.linear -= self.im2 * impulse;
+        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::BallJoint(ball) = &mut joint.params {
+            ball.impulse = self.impulse
+        }
+    }
+}

src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs

@@ -0,0 +1,329 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use simba::simd::SimdValue;
+
+#[derive(Debug)]
+pub(crate) struct WBallVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    rhs: Vector<SimdFloat>,
+    pub(crate) impulse: Vector<SimdFloat>,
+
+    gcross1: Vector<SimdFloat>,
+    gcross2: Vector<SimdFloat>,
+
+    inv_lhs: SdpMatrix<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+}
+
+impl WBallVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1 - world_com1;
+        let anchor2 = position2 * local_anchor2 - world_com2;
+
+        let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
+        let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
+        let rhs = -(vel1 - vel2);
+        let lhs;
+
+        let cmat1 = anchor1.gcross_matrix();
+        let cmat2 = anchor2.gcross_matrix();
+
+        #[cfg(feature = "dim3")]
+        {
+            lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2)
+                + ii1_sqrt.squared().quadform(&cmat1).add_diagonal(im1);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let ii1 = ii1_sqrt.squared();
+            let ii2 = ii2_sqrt.squared();
+            let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
+            let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
+            let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
+            lhs = SdpMatrix::new(m11, m12, m22)
+        }
+
+        let gcross1 = ii1_sqrt.transform_lin_vector(anchor1);
+        let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
+
+        let inv_lhs = lhs.inverse_unchecked();
+
+        WBallVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            im2,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            gcross1,
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        mj_lambda1.linear += self.impulse * self.im1;
+        mj_lambda1.angular += self.gcross1.gcross(self.impulse);
+        mj_lambda2.linear -= self.impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1);
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let dvel = -vel1 + vel2 + self.rhs;
+
+        let impulse = self.inv_lhs * dvel;
+        self.impulse += impulse;
+
+        mj_lambda1.linear += impulse * self.im1;
+        mj_lambda1.angular += self.gcross1.gcross(impulse);
+
+        mj_lambda2.linear -= impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::BallJoint(ball) = &mut joint.params {
+                ball.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WBallVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+    joint_id: [JointIndex; SIMD_WIDTH],
+    rhs: Vector<SimdFloat>,
+    pub(crate) impulse: Vector<SimdFloat>,
+    gcross2: Vector<SimdFloat>,
+    inv_lhs: SdpMatrix<SimdFloat>,
+    im2: SimdFloat,
+}
+
+impl WBallVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&BallJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let local_anchor1 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor2 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1 - world_com1;
+        let anchor2 = position2 * local_anchor2 - world_com2;
+
+        let vel1: Vector<SimdFloat> = linvel1 + angvel1.gcross(anchor1);
+        let vel2: Vector<SimdFloat> = linvel2 + angvel2.gcross(anchor2);
+        let rhs = vel2 - vel1;
+        let lhs;
+
+        let cmat2 = anchor2.gcross_matrix();
+        let gcross2 = ii2_sqrt.transform_lin_vector(anchor2);
+
+        #[cfg(feature = "dim3")]
+        {
+            lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let ii2 = ii2_sqrt.squared();
+            let m11 = im2 + cmat2.x * cmat2.x * ii2;
+            let m12 = cmat2.x * cmat2.y * ii2;
+            let m22 = im2 + cmat2.y * cmat2.y * ii2;
+            lhs = SdpMatrix::new(m11, m12, m22)
+        }
+
+        let inv_lhs = lhs.inverse_unchecked();
+
+        WBallVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            gcross2,
+            rhs,
+            inv_lhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        mj_lambda2.linear -= self.impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.gcross(self.impulse);
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2);
+        let dvel = vel2 + self.rhs;
+
+        let impulse = self.inv_lhs * dvel;
+        self.impulse += impulse;
+
+        mj_lambda2.linear -= impulse * self.im2;
+        mj_lambda2.angular -= self.gcross2.gcross(impulse);
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::BallJoint(ball) = &mut joint.params {
+                ball.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/fixed_position_constraint.rs

@@ -0,0 +1,139 @@
+use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
+use crate::math::{AngularInertia, Isometry, Point, Rotation};
+use crate::utils::WAngularInertia;
+
+#[derive(Debug)]
+pub(crate) struct FixedPositionConstraint {
+    position1: usize,
+    position2: usize,
+    local_anchor1: Isometry<f32>,
+    local_anchor2: Isometry<f32>,
+    local_com1: Point<f32>,
+    local_com2: Point<f32>,
+    im1: f32,
+    im2: f32,
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    lin_inv_lhs: f32,
+    ang_inv_lhs: AngularInertia<f32>,
+}
+
+impl FixedPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+        let lin_inv_lhs = 1.0 / (im1 + im2);
+        let ang_inv_lhs = (ii1 + ii2).inverse();
+
+        Self {
+            local_anchor1: cparams.local_anchor1,
+            local_anchor2: cparams.local_anchor2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
+            lin_inv_lhs,
+            ang_inv_lhs,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+        let ang_err = anchor2.rotation * anchor1.rotation.inverse();
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self
+            .ang_inv_lhs
+            .transform_vector(ang_err.scaled_axis() * params.joint_erp);
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self
+            .ang_inv_lhs
+            .transform_vector(ang_err.angle() * params.joint_erp);
+        position1.rotation =
+            Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
+        position2.rotation =
+            Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
+
+        // Linear correction.
+        let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector);
+        let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
+        let err = anchor2 - anchor1;
+        let impulse = err * (self.lin_inv_lhs * params.joint_erp);
+        position1.translation.vector += self.im1 * impulse;
+        position2.translation.vector -= self.im2 * impulse;
+
+        positions[self.position1 as usize] = position1;
+        positions[self.position2 as usize] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct FixedPositionGroundConstraint {
+    position2: usize,
+    anchor1: Isometry<f32>,
+    local_anchor2: Isometry<f32>,
+    local_com2: Point<f32>,
+    im2: f32,
+    ii2: AngularInertia<f32>,
+    impulse: f32,
+}
+
+impl FixedPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &FixedJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor1;
+        let local_anchor2;
+
+        if flipped {
+            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            local_anchor2 = cparams.local_anchor1;
+        } else {
+            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            local_anchor2 = cparams.local_anchor2;
+        };
+
+        Self {
+            anchor1,
+            local_anchor2,
+            position2: rb2.active_set_offset,
+            im2: rb2.mass_properties.inv_mass,
+            ii2: rb2.world_inv_inertia_sqrt.squared(),
+            local_com2: rb2.mass_properties.local_com,
+            impulse: 0.0,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let anchor2 = position2 * self.local_anchor2;
+        let ang_err = anchor2.rotation * self.anchor1.rotation.inverse();
+        position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
+
+        // Linear correction.
+        let anchor1 = Point::from(self.anchor1.translation.vector);
+        let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector);
+        let err = anchor2 - anchor1;
+        // NOTE: no need to divide by im2 just to multiply right after.
+        let impulse = err * params.joint_erp;
+        position2.translation.vector -= impulse;
+
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs

@@ -0,0 +1,370 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{AngularInertia, Dim, SpacialVector, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim2")]
+use na::{Matrix3, Vector3};
+#[cfg(feature = "dim3")]
+use na::{Matrix6, Vector6, U3};
+
+#[derive(Debug)]
+pub(crate) struct FixedVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    impulse: SpacialVector<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    ii1_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+
+    r1: Vector<f32>,
+    r2: Vector<f32>,
+}
+
+impl FixedVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &FixedJoint,
+    ) -> Self {
+        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor2 = rb2.position * cparams.local_anchor2;
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+        let rmat1 = r1.gcross_matrix();
+        let rmat2 = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let lhs00 =
+                ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
+            let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
+            let lhs11 = (ii1 + ii2).into_matrix();
+
+            // Note that Cholesky only reads the lower-triangular part of the matrix
+            // so we don't need to fill lhs01.
+            lhs = Matrix6::zeros();
+            lhs.fixed_slice_mut::<U3, U3>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
+            let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
+            let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
+            let m13 = rmat1.x * ii1 + rmat2.x * ii2;
+            let m23 = rmat1.y * ii1 + rmat2.y * ii2;
+            let m33 = ii1 + ii2;
+            lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
+        }
+
+        // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.try_inverse().expect("Singular system.");
+        #[cfg(feature = "dim3")]
+        let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
+
+        let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
+        let ang_dvel = -rb1.angvel + rb2.angvel;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
+        );
+
+        FixedVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
+            + mj_lambda2.linear
+            + ang_vel2.gcross(self.r2);
+        let dangvel = -ang_vel1 + ang_vel2;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::FixedJoint(fixed) = &mut joint.params {
+            fixed.impulse = self.impulse;
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct FixedVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    impulse: SpacialVector<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<f32>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<f32>,
+
+    im2: f32,
+    ii2: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+    r2: Vector<f32>,
+}
+
+impl FixedVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &FixedJoint,
+        flipped: bool,
+    ) -> Self {
+        let (anchor1, anchor2) = if flipped {
+            (
+                rb1.position * cparams.local_anchor2,
+                rb2.position * cparams.local_anchor1,
+            )
+        } else {
+            (
+                rb1.position * cparams.local_anchor1,
+                rb2.position * cparams.local_anchor2,
+            )
+        };
+
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+        let rmat2 = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
+            let lhs10 = ii2.transform_matrix(&rmat2);
+            let lhs11 = ii2.into_matrix();
+
+            // Note that Cholesky only reads the lower-triangular part of the matrix
+            // so we don't need to fill lhs01.
+            lhs = Matrix6::zeros();
+            lhs.fixed_slice_mut::<U3, U3>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let m11 = im2 + rmat2.x * rmat2.x * ii2;
+            let m12 = rmat2.x * rmat2.y * ii2;
+            let m22 = im2 + rmat2.y * rmat2.y * ii2;
+            let m13 = rmat2.x * ii2;
+            let m23 = rmat2.y * ii2;
+            let m33 = ii2;
+            lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33)
+        }
+
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.try_inverse().expect("Singular system.");
+        #[cfg(feature = "dim3")]
+        let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
+
+        let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let ang_dvel = rb2.angvel - rb1.angvel;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
+        );
+
+        FixedVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let dangvel = ang_vel2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let impulse = self.inv_lhs * rhs;
+
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::FixedJoint(fixed) = &mut joint.params {
+            fixed.impulse = self.impulse;
+        }
+    }
+}

src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs

@@ -0,0 +1,472 @@
+use simba::simd::SimdValue;
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix6, Vector6, U3};
+#[cfg(feature = "dim2")]
+use {
+    crate::utils::SdpMatrix3,
+    na::{Matrix3, Vector3},
+};
+
+#[derive(Debug)]
+pub(crate) struct WFixedVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1: AngularInertia<SimdFloat>,
+    ii2: AngularInertia<SimdFloat>,
+
+    ii1_sqrt: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+
+    r1: Vector<SimdFloat>,
+    r2: Vector<SimdFloat>,
+}
+
+impl WFixedVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&FixedJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let ii1 = ii1_sqrt.squared();
+        let ii2 = ii2_sqrt.squared();
+        let r1 = anchor1.translation.vector - world_com1.coords;
+        let r2 = anchor2.translation.vector - world_com2.coords;
+        let rmat1: CrossMatrix<_> = r1.gcross_matrix();
+        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let lhs00 =
+                ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2);
+            let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2);
+            let lhs11 = (ii1 + ii2).into_matrix();
+
+            // Note that Cholesky only reads the lower-triangular part of the matrix
+            // so we don't need to fill lhs01.
+            lhs = Matrix6::zeros();
+            lhs.fixed_slice_mut::<U3, U3>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2;
+            let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2;
+            let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2;
+            let m13 = rmat1.x * ii1 + rmat2.x * ii2;
+            let m23 = rmat1.y * ii1 + rmat2.y * ii2;
+            let m33 = ii1 + ii2;
+            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
+        }
+
+        // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix?
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2);
+        let ang_dvel = -angvel1 + angvel2;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
+        );
+
+        WFixedVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1)
+            + mj_lambda2.linear
+            + ang_vel2.gcross(self.r2);
+        let dangvel = -ang_vel1 + ang_vel2;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::FixedJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WFixedVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdFloat>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdFloat>,
+
+    im2: SimdFloat,
+    ii2: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+    r2: Vector<SimdFloat>,
+}
+
+impl WFixedVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&FixedJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Isometry::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Isometry::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let ii2 = ii2_sqrt.squared();
+        let r1 = anchor1.translation.vector - world_com1.coords;
+        let r2 = anchor2.translation.vector - world_com2.coords;
+        let rmat2: CrossMatrix<_> = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2);
+            let lhs10 = ii2.transform_matrix(&rmat2);
+            let lhs11 = ii2.into_matrix();
+
+            lhs = Matrix6::zeros();
+            lhs.fixed_slice_mut::<U3, U3>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U3>(3, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(3, 3).copy_from(&lhs11);
+        }
+
+        // In 2D we just unroll the computation because
+        // it's just easier that way.
+        #[cfg(feature = "dim2")]
+        {
+            let m11 = im2 + rmat2.x * rmat2.x * ii2;
+            let m12 = rmat2.x * rmat2.y * ii2;
+            let m22 = im2 + rmat2.y * rmat2.y * ii2;
+            let m13 = rmat2.x * ii2;
+            let m23 = rmat2.y * ii2;
+            let m33 = ii2;
+            lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33)
+        }
+
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix?
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let ang_dvel = angvel2 - angvel1;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z,
+        );
+
+        WFixedVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2: DeltaVel<SimdFloat> = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let dangvel = ang_vel2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let impulse = self.inv_lhs * rhs;
+
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::FixedJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -0,0 +1,340 @@
+use super::{
+    BallVelocityConstraint, BallVelocityGroundConstraint, FixedVelocityConstraint,
+    FixedVelocityGroundConstraint, PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+use super::{
+    WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
+    WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
+    WPrismaticVelocityGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+#[cfg(feature = "simd-is-enabled")]
+use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
+};
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+
+pub(crate) enum AnyJointVelocityConstraint {
+    BallConstraint(BallVelocityConstraint),
+    BallGroundConstraint(BallVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WBallConstraint(WBallVelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WBallGroundConstraint(WBallVelocityGroundConstraint),
+    FixedConstraint(FixedVelocityConstraint),
+    FixedGroundConstraint(FixedVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WFixedConstraint(WFixedVelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WFixedGroundConstraint(WFixedVelocityGroundConstraint),
+    PrismaticConstraint(PrismaticVelocityConstraint),
+    PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WPrismaticConstraint(WPrismaticVelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WPrismaticGroundConstraint(WPrismaticVelocityGroundConstraint),
+    #[cfg(feature = "dim3")]
+    RevoluteConstraint(RevoluteVelocityConstraint),
+    #[cfg(feature = "dim3")]
+    RevoluteGroundConstraint(RevoluteVelocityGroundConstraint),
+    #[cfg(feature = "dim3")]
+    #[cfg(feature = "simd-is-enabled")]
+    WRevoluteConstraint(WRevoluteVelocityConstraint),
+    #[cfg(feature = "dim3")]
+    #[cfg(feature = "simd-is-enabled")]
+    WRevoluteGroundConstraint(WRevoluteVelocityGroundConstraint),
+    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    Empty,
+}
+
+impl AnyJointVelocityConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(_: &Joint) -> usize {
+        1
+    }
+
+    pub fn from_joint(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        joint: &Joint,
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        match &joint.params {
+            JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
+                BallVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            ),
+            JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedConstraint(
+                FixedVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            ),
+            JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
+                PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            ),
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint(
+                RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
+            ),
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn from_wide_joint(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        joints: [&Joint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
+                    params, joint_id, rbs1, rbs2, joints,
+                ))
+            }
+            JointParams::FixedJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
+                    params, joint_id, rbs1, rbs2, joints,
+                ))
+            }
+            JointParams::PrismaticJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WPrismaticConstraint(
+                    WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WRevoluteConstraint(
+                    WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+                )
+            }
+        }
+    }
+
+    pub fn from_joint_ground(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        joint: &Joint,
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let mut rb1 = &bodies[joint.body1];
+        let mut rb2 = &bodies[joint.body2];
+        let flipped = !rb2.is_dynamic();
+
+        if flipped {
+            std::mem::swap(&mut rb1, &mut rb2);
+        }
+
+        match &joint.params {
+            JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
+                BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
+            ),
+            JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
+                FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
+            ),
+            JointParams::PrismaticJoint(p) => {
+                AnyJointVelocityConstraint::PrismaticGroundConstraint(
+                    PrismaticVelocityGroundConstraint::from_params(
+                        params, joint_id, rb1, rb2, p, flipped,
+                    ),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
+                RevoluteVelocityGroundConstraint::from_params(
+                    params, joint_id, rb1, rb2, p, flipped,
+                ),
+            ),
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn from_wide_joint_ground(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        joints: [&Joint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+    ) -> Self {
+        let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                flipped[ii] = true;
+            }
+        }
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WBallGroundConstraint(
+                    WBallVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+            JointParams::FixedJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WFixedGroundConstraint(
+                    WFixedVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+            JointParams::PrismaticJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WPrismaticGroundConstraint(
+                    WPrismaticVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => {
+                let joints =
+                    array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WRevoluteGroundConstraint(
+                    WRevoluteVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::FixedConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            AnyJointVelocityConstraint::RevoluteConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::FixedConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            AnyJointVelocityConstraint::RevoluteConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "dim3")]
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        match self {
+            AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all),
+
+            AnyJointVelocityConstraint::BallGroundConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallConstraint(c) => c.writeback_impulses(joints_all),
+
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WBallGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            AnyJointVelocityConstraint::FixedConstraint(c) => c.writeback_impulses(joints_all),
+            AnyJointVelocityConstraint::FixedGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
+            AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            #[cfg(feature = "dim3")]
+            AnyJointVelocityConstraint::RevoluteConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "dim3")]
+            AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            #[cfg(feature = "dim3")]
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "dim3")]
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            AnyJointVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -0,0 +1,169 @@
+use super::{
+    BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
+    FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+use super::{WBallPositionConstraint, WBallPositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+
+pub(crate) enum AnyJointPositionConstraint {
+    BallJoint(BallPositionConstraint),
+    BallGroundConstraint(BallPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WBallJoint(WBallPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WBallGroundConstraint(WBallPositionGroundConstraint),
+    FixedJoint(FixedPositionConstraint),
+    FixedGroundConstraint(FixedPositionGroundConstraint),
+    PrismaticJoint(PrismaticPositionConstraint),
+    PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
+    #[cfg(feature = "dim3")]
+    RevoluteJoint(RevolutePositionConstraint),
+    #[cfg(feature = "dim3")]
+    RevoluteGroundConstraint(RevolutePositionGroundConstraint),
+    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    Empty,
+}
+
+impl AnyJointPositionConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(joint: &Joint, grouped: bool) -> usize {
+        #[cfg(feature = "simd-is-enabled")]
+        if !grouped {
+            1
+        } else {
+            match &joint.params {
+                JointParams::BallJoint(_) => 1,
+                _ => SIMD_WIDTH, // For joints that don't support SIMD position constraints yet.
+            }
+        }
+
+        #[cfg(not(feature = "simd-is-enabled"))]
+        {
+            1
+        }
+    }
+
+    pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
+        let rb1 = &bodies[joint.body1];
+        let rb2 = &bodies[joint.body2];
+
+        match &joint.params {
+            JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
+                BallPositionConstraint::from_params(rb1, rb2, p),
+            ),
+            JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
+                FixedPositionConstraint::from_params(rb1, rb2, p),
+            ),
+            JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
+                PrismaticPositionConstraint::from_params(rb1, rb2, p),
+            ),
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteJoint(
+                RevolutePositionConstraint::from_params(rb1, rb2, p),
+            ),
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option<Self> {
+        let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                Some(AnyJointPositionConstraint::WBallJoint(
+                    WBallPositionConstraint::from_params(rbs1, rbs2, joints),
+                ))
+            }
+            JointParams::FixedJoint(_) => None,
+            JointParams::PrismaticJoint(_) => None,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => None,
+        }
+    }
+
+    pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
+        let mut rb1 = &bodies[joint.body1];
+        let mut rb2 = &bodies[joint.body2];
+        let flipped = !rb2.is_dynamic();
+
+        if flipped {
+            std::mem::swap(&mut rb1, &mut rb2);
+        }
+
+        match &joint.params {
+            JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
+                BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            ),
+            JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
+                FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            ),
+            JointParams::PrismaticJoint(p) => {
+                AnyJointPositionConstraint::PrismaticGroundConstraint(
+                    PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+                )
+            }
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteGroundConstraint(
+                RevolutePositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            ),
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    pub fn from_wide_joint_ground(
+        joints: [&Joint; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+    ) -> Option<Self> {
+        let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
+        let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                flipped[ii] = true;
+            }
+        }
+
+        match &joints[0].params {
+            JointParams::BallJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
+                Some(AnyJointPositionConstraint::WBallGroundConstraint(
+                    WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                ))
+            }
+            JointParams::FixedJoint(_) => None,
+            JointParams::PrismaticJoint(_) => None,
+            #[cfg(feature = "dim3")]
+            JointParams::RevoluteJoint(_) => None,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        match self {
+            AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WBallJoint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(feature = "dim3")]
+            AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions),
+            #[cfg(feature = "dim3")]
+            AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::Empty => unreachable!(),
+        }
+    }
+}

src/dynamics/solver/joint_constraint/mod.rs

@@ -0,0 +1,65 @@
+pub(self) use ball_position_constraint::{BallPositionConstraint, BallPositionGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use ball_position_constraint_wide::{
+    WBallPositionConstraint, WBallPositionGroundConstraint,
+};
+pub(self) use ball_velocity_constraint::{BallVelocityConstraint, BallVelocityGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use ball_velocity_constraint_wide::{
+    WBallVelocityConstraint, WBallVelocityGroundConstraint,
+};
+pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint};
+pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use fixed_velocity_constraint_wide::{
+    WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
+};
+pub(crate) use joint_constraint::AnyJointVelocityConstraint;
+pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
+pub(self) use prismatic_position_constraint::{
+    PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+};
+pub(self) use prismatic_velocity_constraint::{
+    PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use prismatic_velocity_constraint_wide::{
+    WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+pub(self) use revolute_position_constraint::{
+    RevolutePositionConstraint, RevolutePositionGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+pub(self) use revolute_velocity_constraint::{
+    RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint,
+};
+#[cfg(feature = "dim3")]
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use revolute_velocity_constraint_wide::{
+    WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint,
+};
+
+mod ball_position_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod ball_position_constraint_wide;
+mod ball_velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod ball_velocity_constraint_wide;
+mod fixed_position_constraint;
+mod fixed_velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod fixed_velocity_constraint_wide;
+mod joint_constraint;
+mod joint_position_constraint;
+mod prismatic_position_constraint;
+mod prismatic_velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod prismatic_velocity_constraint_wide;
+#[cfg(feature = "dim3")]
+mod revolute_position_constraint;
+#[cfg(feature = "dim3")]
+mod revolute_velocity_constraint;
+#[cfg(feature = "dim3")]
+#[cfg(feature = "simd-is-enabled")]
+mod revolute_velocity_constraint_wide;

src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs

@@ -0,0 +1,170 @@
+use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
+use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::utils::WAngularInertia;
+use na::Unit;
+
+#[derive(Debug)]
+pub(crate) struct PrismaticPositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    lin_inv_lhs: f32,
+    ang_inv_lhs: AngularInertia<f32>,
+
+    limits: [f32; 2],
+
+    local_frame1: Isometry<f32>,
+    local_frame2: Isometry<f32>,
+
+    local_axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+}
+
+impl PrismaticPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+        let lin_inv_lhs = 1.0 / (im1 + im2);
+        let ang_inv_lhs = (ii1 + ii2).inverse();
+
+        Self {
+            im1,
+            im2,
+            ii1,
+            ii2,
+            lin_inv_lhs,
+            ang_inv_lhs,
+            local_frame1: cparams.local_frame1(),
+            local_frame2: cparams.local_frame2(),
+            local_axis1: cparams.local_axis1,
+            local_axis2: cparams.local_axis2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+            limits: cparams.limits,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let frame1 = position1 * self.local_frame1;
+        let frame2 = position2 * self.local_frame2;
+        let ang_err = frame2.rotation * frame1.rotation.inverse();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self
+            .ang_inv_lhs
+            .transform_vector(ang_err.angle() * params.joint_erp);
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self
+            .ang_inv_lhs
+            .transform_vector(ang_err.scaled_axis() * params.joint_erp);
+        position1.rotation =
+            Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
+        position2.rotation =
+            Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
+
+        // Linear correction.
+        let anchor1 = position1 * Point::from(self.local_frame1.translation.vector);
+        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
+        let axis1 = position1 * self.local_axis1;
+        let dpos = anchor2 - anchor1;
+        let limit_err = dpos.dot(&axis1);
+        let mut err = dpos - *axis1 * limit_err;
+
+        if limit_err < self.limits[0] {
+            err += *axis1 * (limit_err - self.limits[0]);
+        } else if limit_err > self.limits[1] {
+            err += *axis1 * (limit_err - self.limits[1]);
+        }
+
+        let impulse = err * (self.lin_inv_lhs * params.joint_erp);
+        position1.translation.vector += self.im1 * impulse;
+        position2.translation.vector -= self.im2 * impulse;
+
+        positions[self.position1 as usize] = position1;
+        positions[self.position2 as usize] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct PrismaticPositionGroundConstraint {
+    position2: usize,
+    frame1: Isometry<f32>,
+    local_frame2: Isometry<f32>,
+    axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+    limits: [f32; 2],
+}
+
+impl PrismaticPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &PrismaticJoint,
+        flipped: bool,
+    ) -> Self {
+        let frame1;
+        let local_frame2;
+        let axis1;
+        let local_axis2;
+
+        if flipped {
+            frame1 = rb1.predicted_position * cparams.local_frame2();
+            local_frame2 = cparams.local_frame1();
+            axis1 = rb1.predicted_position * cparams.local_axis2;
+            local_axis2 = cparams.local_axis1;
+        } else {
+            frame1 = rb1.predicted_position * cparams.local_frame1();
+            local_frame2 = cparams.local_frame2();
+            axis1 = rb1.predicted_position * cparams.local_axis1;
+            local_axis2 = cparams.local_axis2;
+        };
+
+        Self {
+            frame1,
+            local_frame2,
+            axis1,
+            local_axis2,
+            position2: rb2.active_set_offset,
+            limits: cparams.limits,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        // Angular correction.
+        let frame2 = position2 * self.local_frame2;
+        let ang_err = frame2.rotation * self.frame1.rotation.inverse();
+        position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation;
+
+        // Linear correction.
+        let anchor1 = Point::from(self.frame1.translation.vector);
+        let anchor2 = position2 * Point::from(self.local_frame2.translation.vector);
+        let dpos = anchor2 - anchor1;
+        let limit_err = dpos.dot(&self.axis1);
+        let mut err = dpos - *self.axis1 * limit_err;
+
+        if limit_err < self.limits[0] {
+            err += *self.axis1 * (limit_err - self.limits[0]);
+        } else if limit_err > self.limits[1] {
+            err += *self.axis1 * (limit_err - self.limits[1]);
+        }
+
+        // NOTE: no need to divide by im2 just to multiply right after.
+        let impulse = err * params.joint_erp;
+        position2.translation.vector -= impulse;
+
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -0,0 +1,558 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
+};
+use crate::math::{AngularInertia, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+#[cfg(feature = "dim2")]
+use {
+    crate::utils::SdpMatrix2,
+    na::{Matrix2, Vector2},
+};
+
+#[cfg(feature = "dim2")]
+type LinImpulseDim = na::U1;
+#[cfg(feature = "dim3")]
+type LinImpulseDim = na::U2;
+
+#[derive(Debug)]
+pub(crate) struct PrismaticVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r1: Vector<f32>,
+    r2: Vector<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<f32>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<f32>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<f32>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<f32>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<f32>,
+
+    limits_impulse: f32,
+    limits_forcedirs: Option<(Vector<f32>, Vector<f32>)>,
+    limits_rhs: f32,
+
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<f32>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl PrismaticVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &PrismaticJoint,
+    ) -> Self {
+        // Linear part.
+        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor2 = rb2.position * cparams.local_anchor2;
+        let axis1 = rb1.position * cparams.local_axis1;
+        let axis2 = rb2.position * cparams.local_axis2;
+        #[cfg(feature = "dim2")]
+        let basis1 = rb1.position * cparams.basis1[0];
+        #[cfg(feature = "dim3")]
+        let basis1 = Matrix3x2::from_columns(&[
+            rb1.position * cparams.basis1[0],
+            rb1.position * cparams.basis1[1],
+        ]);
+
+        // #[cfg(feature = "dim2")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // #[cfg(feature = "dim3")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .unwrap_or(Rotation::identity())
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // let basis2 = r21 * basis1;
+        // NOTE: we use basis2 := basis1 for now is that allows
+        // simplifications of the computation without introducing
+        // much instabilities.
+
+        let im1 = rb1.mass_properties.inv_mass;
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r1_mat = r1.gcross_matrix();
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r2 = anchor2 - rb2.world_com;
+        let r2_mat = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let r1_mat_b1 = r1_mat * basis1;
+            let r2_mat_b1 = r2_mat * basis1;
+
+            lhs = Matrix5::zeros();
+            let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
+                + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
+            let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
+            let lhs11 = (ii1 + ii2).into_matrix();
+            lhs.fixed_slice_mut::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            let b1r1 = basis1.dot(&r1_mat);
+            let b2r2 = basis1.dot(&r2_mat);
+            let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
+            let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
+            let m22 = ii1 + ii2;
+            lhs = SdpMatrix2::new(m11, m12, m22);
+        }
+
+        let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
+        let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
+
+        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix();
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = rb2.angvel - rb1.angvel;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedirs = None;
+        let mut limits_rhs = 0.0;
+        let mut limits_impulse = 0.0;
+
+        if cparams.limits_enabled {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time, and allow predictive constraint activation.
+            if dist < cparams.limits[0] {
+                limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
+                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            } else if dist > cparams.limits[1] {
+                limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
+                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            }
+        }
+
+        PrismaticVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            limits_impulse: limits_impulse * params.warmstart_coeff,
+            limits_forcedirs,
+            limits_rhs,
+            basis1,
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+        }
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        /*
+         * Joint consraint.
+         */
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
+        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
+        let ang_dvel = ang_vel2 - ang_vel1;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        /*
+         * Joint limits.
+         */
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            // FIXME: the transformation by ii2_sqrt could be avoided by
+            // reusing some computations above.
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+                + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
+                + self.limits_rhs;
+            let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0);
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+            revolute.limits_impulse = self.limits_impulse;
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct PrismaticVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r2: Vector<f32>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<f32>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<f32>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<f32>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<f32>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<f32>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<f32>,
+
+    limits_impulse: f32,
+    limits_rhs: f32,
+
+    axis2: Vector<f32>,
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<f32>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<f32>,
+    limits_forcedir2: Option<Vector<f32>>,
+
+    im2: f32,
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl PrismaticVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &PrismaticJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor2;
+        let anchor1;
+        let axis2;
+        let axis1;
+        let basis1;
+
+        if flipped {
+            anchor2 = rb2.position * cparams.local_anchor1;
+            anchor1 = rb1.position * cparams.local_anchor2;
+            axis2 = rb2.position * cparams.local_axis1;
+            axis1 = rb1.position * cparams.local_axis2;
+            #[cfg(feature = "dim2")]
+            {
+                basis1 = rb1.position * cparams.basis2[0];
+            }
+            #[cfg(feature = "dim3")]
+            {
+                basis1 = Matrix3x2::from_columns(&[
+                    rb1.position * cparams.basis2[0],
+                    rb1.position * cparams.basis2[1],
+                ]);
+            }
+        } else {
+            anchor2 = rb2.position * cparams.local_anchor2;
+            anchor1 = rb1.position * cparams.local_anchor1;
+            axis2 = rb2.position * cparams.local_axis2;
+            axis1 = rb1.position * cparams.local_axis1;
+            #[cfg(feature = "dim2")]
+            {
+                basis1 = rb1.position * cparams.basis1[0];
+            }
+            #[cfg(feature = "dim3")]
+            {
+                basis1 = Matrix3x2::from_columns(&[
+                    rb1.position * cparams.basis1[0],
+                    rb1.position * cparams.basis1[1],
+                ]);
+            }
+        };
+
+        // #[cfg(feature = "dim2")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // #[cfg(feature = "dim3")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .unwrap_or(Rotation::identity())
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // let basis2 = r21 * basis1;
+        // NOTE: we use basis2 := basis1 for now is that allows
+        // simplifications of the computation without introducing
+        // much instabilities.
+
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r2 = anchor2 - rb2.world_com;
+        let r2_mat = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let r2_mat_b1 = r2_mat * basis1;
+
+            lhs = Matrix5::zeros();
+            let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
+            let lhs10 = ii2 * r2_mat_b1;
+            let lhs11 = ii2.into_matrix();
+            lhs.fixed_slice_mut::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            let b2r2 = basis1.dot(&r2_mat);
+            let m11 = im2 + b2r2 * ii2 * b2r2;
+            let m12 = basis1.dot(&r2_mat) * ii2;
+            let m22 = ii2;
+            lhs = SdpMatrix2::new(m11, m12, m22);
+        }
+
+        let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
+        let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
+
+        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix();
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = rb2.angvel - rb1.angvel;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedir2 = None;
+        let mut limits_rhs = 0.0;
+        let mut limits_impulse = 0.0;
+
+        if cparams.limits_enabled {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time.
+            // FIXME: allow predictive constraint activation.
+            if dist < cparams.limits[0] {
+                limits_forcedir2 = Some(axis2.into_inner());
+                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            } else if dist > cparams.limits[1] {
+                limits_forcedir2 = Some(-axis2.into_inner());
+                limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
+                limits_impulse = cparams.limits_impulse;
+            }
+        }
+
+        PrismaticVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            limits_impulse: limits_impulse * params.warmstart_coeff,
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+            axis2: axis2.into_inner(),
+            limits_forcedir2,
+            limits_rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+        }
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        /*
+         * Joint consraint.
+         */
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let lin_dvel = self.basis1.tr_mul(&lin_vel2);
+        let ang_dvel = ang_vel2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        /*
+         * Joint limits.
+         */
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            // FIXME: the transformation by ii2_sqrt could be avoided by
+            // reusing some computations above.
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+                + self.limits_rhs;
+            let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0);
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+            revolute.limits_impulse = self.limits_impulse;
+        }
+    }
+}

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -0,0 +1,687 @@
+use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SimdBool, SimdFloat, Vector, SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+#[cfg(feature = "dim2")]
+use {
+    crate::utils::SdpMatrix2,
+    na::{Matrix2, Vector2},
+};
+
+#[cfg(feature = "dim2")]
+type LinImpulseDim = na::U1;
+#[cfg(feature = "dim3")]
+type LinImpulseDim = na::U2;
+
+#[derive(Debug)]
+pub(crate) struct WPrismaticVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r1: Vector<SimdFloat>,
+    r2: Vector<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<SimdFloat>,
+
+    limits_impulse: SimdFloat,
+    limits_forcedirs: Option<(Vector<SimdFloat>, Vector<SimdFloat>)>,
+    limits_rhs: SimdFloat,
+
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1_sqrt: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WPrismaticVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&PrismaticJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
+        let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
+
+        #[cfg(feature = "dim2")]
+        let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
+        #[cfg(feature = "dim3")]
+        let local_basis1 = [
+            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+        ];
+
+        #[cfg(feature = "dim2")]
+        let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        #[cfg(feature = "dim3")]
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let axis1 = position1 * local_axis1;
+        let axis2 = position2 * local_axis2;
+
+        #[cfg(feature = "dim2")]
+        let basis1 = position1 * local_basis1[0];
+        #[cfg(feature = "dim3")]
+        let basis1 =
+            Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
+
+        // #[cfg(feature = "dim2")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // #[cfg(feature = "dim3")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .unwrap_or(Rotation::identity())
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // let basis2 = r21 * basis1;
+        // NOTE: we use basis2 := basis1 for now is that allows
+        // simplifications of the computation without introducing
+        // much instabilities.
+
+        let ii1 = ii1_sqrt.squared();
+        let r1 = anchor1 - world_com1;
+        let r1_mat = r1.gcross_matrix();
+
+        let ii2 = ii2_sqrt.squared();
+        let r2 = anchor2 - world_com2;
+        let r2_mat = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let r1_mat_b1 = r1_mat * basis1;
+            let r2_mat_b1 = r2_mat * basis1;
+
+            lhs = Matrix5::zeros();
+            let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1)
+                + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
+            let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1;
+            let lhs11 = (ii1 + ii2).into_matrix();
+            lhs.fixed_slice_mut::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            let b1r1 = basis1.dot(&r1_mat);
+            let b2r2 = basis1.dot(&r2_mat);
+            let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2;
+            let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2;
+            let m22 = ii1 + ii2;
+            lhs = SdpMatrix2::new(m11, m12, m22);
+        }
+
+        let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
+        let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
+
+        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix();
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = angvel2 - angvel1;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedirs = None;
+        let mut limits_rhs = na::zero();
+        let mut limits_impulse = na::zero();
+        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+
+        if limits_enabled.any() {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time + allow predictive constraint activation.
+            let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
+            let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
+            let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
+
+            let min_enabled = dist.simd_lt(min_limit);
+            let max_enabled = dist.simd_gt(max_limit);
+            let _0: SimdFloat = na::zero();
+            let _1: SimdFloat = na::one();
+            let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
+
+            if sign != _0 {
+                limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
+                limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
+                limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
+            }
+        }
+
+        WPrismaticVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            ii1_sqrt,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
+            limits_forcedirs,
+            limits_rhs,
+            basis1,
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        /*
+         * Joint consraint.
+         */
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
+        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1));
+        let ang_dvel = ang_vel2 - ang_vel1;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        /*
+         * Joint limits.
+         */
+        if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
+            // FIXME: the transformation by ii2_sqrt could be avoided by
+            // reusing some computations above.
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+                + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
+                + self.limits_rhs;
+            let new_impulse =
+                (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero());
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::PrismaticJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii);
+                rev.limits_impulse = self.limits_impulse.extract(ii);
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WPrismaticVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r2: Vector<SimdFloat>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector2<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    impulse: Vector2<SimdFloat>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    rhs: Vector5<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    impulse: Vector5<SimdFloat>,
+
+    limits_impulse: SimdFloat,
+    limits_rhs: SimdFloat,
+
+    axis2: Vector<SimdFloat>,
+    #[cfg(feature = "dim2")]
+    basis1: Vector2<SimdFloat>,
+    #[cfg(feature = "dim3")]
+    basis1: Matrix3x2<SimdFloat>,
+    limits_forcedir2: Option<Vector<SimdFloat>>,
+
+    im2: SimdFloat,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WPrismaticVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&PrismaticJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        #[cfg(feature = "dim2")]
+        let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        #[cfg(feature = "dim3")]
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let local_anchor1 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let local_axis1 = Vector::from(
+            array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
+        );
+        let local_axis2 = Vector::from(
+            array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
+        );
+
+        #[cfg(feature = "dim2")]
+        let basis1 = position1
+            * Vector::from(
+                array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+            );
+        #[cfg(feature = "dim3")]
+        let basis1 = Matrix3x2::from_columns(&[
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                ),
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                ),
+        ]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let axis1 = position1 * local_axis1;
+        let axis2 = position2 * local_axis2;
+
+        // #[cfg(feature = "dim2")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // #[cfg(feature = "dim3")]
+        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //     .unwrap_or(Rotation::identity())
+        //     .to_rotation_matrix()
+        //     .into_inner();
+        // let basis2 = r21 * basis1;
+        // NOTE: we use basis2 := basis1 for now is that allows
+        // simplifications of the computation without introducing
+        // much instabilities.
+        let ii2 = ii2_sqrt.squared();
+        let r1 = anchor1 - world_com1;
+        let r2 = anchor2 - world_com2;
+        let r2_mat = r2.gcross_matrix();
+
+        #[allow(unused_mut)] // For 2D.
+        let mut lhs;
+
+        #[cfg(feature = "dim3")]
+        {
+            let r2_mat_b1 = r2_mat * basis1;
+
+            lhs = Matrix5::zeros();
+            let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2);
+            let lhs10 = ii2 * r2_mat_b1;
+            let lhs11 = ii2.into_matrix();
+            lhs.fixed_slice_mut::<U2, U2>(0, 0)
+                .copy_from(&lhs00.into_matrix());
+            lhs.fixed_slice_mut::<U3, U2>(2, 0).copy_from(&lhs10);
+            lhs.fixed_slice_mut::<U3, U3>(2, 2).copy_from(&lhs11);
+        }
+
+        #[cfg(feature = "dim2")]
+        {
+            let b2r2 = basis1.dot(&r2_mat);
+            let m11 = im2 + b2r2 * ii2 * b2r2;
+            let m12 = basis1.dot(&r2_mat) * ii2;
+            let m22 = ii2;
+            lhs = SdpMatrix2::new(m11, m12, m22);
+        }
+
+        let anchor_linvel1 = linvel1 + angvel1.gcross(r1);
+        let anchor_linvel2 = linvel2 + angvel2.gcross(r2);
+
+        // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
+        // for which a textbook inverse is still efficient.
+        #[cfg(feature = "dim2")]
+        let inv_lhs = lhs.inverse_unchecked().into_matrix();
+        #[cfg(feature = "dim3")]
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
+        let ang_rhs = angvel2 - angvel1;
+
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_rhs.x, ang_rhs);
+        #[cfg(feature = "dim3")]
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
+
+        // Setup limit constraint.
+        let mut limits_forcedir2 = None;
+        let mut limits_rhs = na::zero();
+        let mut limits_impulse = na::zero();
+        let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
+
+        if limits_enabled.any() {
+            let danchor = anchor2 - anchor1;
+            let dist = danchor.dot(&axis1);
+
+            // FIXME: we should allow both limits to be active at
+            // the same time + allow predictive constraint activation.
+            let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
+            let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
+            let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]);
+
+            let use_min = dist.simd_lt(min_limit);
+            let use_max = dist.simd_gt(max_limit);
+            let _0: SimdFloat = na::zero();
+            let _1: SimdFloat = na::one();
+            let sign = _1.select(use_min, (-_1).select(use_max, _0));
+
+            if sign != _0 {
+                limits_forcedir2 = Some(axis2 * sign);
+                limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign;
+                limits_impulse = lim_impulse.select(use_min | use_max, _0);
+            }
+        }
+
+        WPrismaticVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff),
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+            axis2,
+            limits_forcedir2,
+            limits_rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        /*
+         * Joint consraint.
+         */
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let lin_dvel = self.basis1.tr_mul(&lin_vel2);
+        let ang_dvel = ang_vel2;
+        #[cfg(feature = "dim2")]
+        let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs;
+        #[cfg(feature = "dim3")]
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = self.basis1 * impulse.fixed_rows::<LinImpulseDim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = impulse.y;
+        #[cfg(feature = "dim3")]
+        let ang_impulse = impulse.fixed_rows::<U3>(2).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        /*
+         * Joint limits.
+         */
+        if let Some(limits_forcedir2) = self.limits_forcedir2 {
+            // FIXME: the transformation by ii2_sqrt could be avoided by
+            // reusing some computations above.
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
+                + self.limits_rhs;
+            let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero());
+            let dimpulse = new_impulse - self.limits_impulse;
+            self.limits_impulse = new_impulse;
+
+            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::PrismaticJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii);
+                rev.limits_impulse = self.limits_impulse.extract(ii);
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -0,0 +1,142 @@
+use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
+use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector};
+use crate::utils::WAngularInertia;
+use na::Unit;
+
+#[derive(Debug)]
+pub(crate) struct RevolutePositionConstraint {
+    position1: usize,
+    position2: usize,
+
+    im1: f32,
+    im2: f32,
+
+    ii1: AngularInertia<f32>,
+    ii2: AngularInertia<f32>,
+
+    lin_inv_lhs: f32,
+    ang_inv_lhs: AngularInertia<f32>,
+
+    local_anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+
+    local_axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+}
+
+impl RevolutePositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+        let lin_inv_lhs = 1.0 / (im1 + im2);
+        let ang_inv_lhs = (ii1 + ii2).inverse();
+
+        Self {
+            im1,
+            im2,
+            ii1,
+            ii2,
+            lin_inv_lhs,
+            ang_inv_lhs,
+            local_anchor1: cparams.local_anchor1,
+            local_anchor2: cparams.local_anchor2,
+            local_axis1: cparams.local_axis1,
+            local_axis2: cparams.local_axis2,
+            position1: rb1.active_set_offset,
+            position2: rb2.active_set_offset,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position1 = positions[self.position1 as usize];
+        let mut position2 = positions[self.position2 as usize];
+
+        let axis1 = position1 * self.local_axis1;
+        let axis2 = position2 * self.local_axis2;
+        let delta_rot =
+            Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or(Rotation::identity());
+        let ang_error = delta_rot.scaled_axis() * params.joint_erp;
+        let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error);
+
+        position1.rotation =
+            Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
+        position2.rotation =
+            Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
+
+        let anchor1 = position1 * self.local_anchor1;
+        let anchor2 = position2 * self.local_anchor2;
+
+        let delta_tra = anchor2 - anchor1;
+        let lin_error = delta_tra * params.joint_erp;
+        let lin_impulse = self.lin_inv_lhs * lin_error;
+
+        position1.translation.vector += self.im1 * lin_impulse;
+        position2.translation.vector -= self.im2 * lin_impulse;
+
+        positions[self.position1 as usize] = position1;
+        positions[self.position2 as usize] = position2;
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct RevolutePositionGroundConstraint {
+    position2: usize,
+    anchor1: Point<f32>,
+    local_anchor2: Point<f32>,
+    axis1: Unit<Vector<f32>>,
+    local_axis2: Unit<Vector<f32>>,
+}
+
+impl RevolutePositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &RevoluteJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor1;
+        let local_anchor2;
+        let axis1;
+        let local_axis2;
+
+        if flipped {
+            anchor1 = rb1.predicted_position * cparams.local_anchor2;
+            local_anchor2 = cparams.local_anchor1;
+            axis1 = rb1.predicted_position * cparams.local_axis2;
+            local_axis2 = cparams.local_axis1;
+        } else {
+            anchor1 = rb1.predicted_position * cparams.local_anchor1;
+            local_anchor2 = cparams.local_anchor2;
+            axis1 = rb1.predicted_position * cparams.local_axis1;
+            local_axis2 = cparams.local_axis2;
+        };
+
+        Self {
+            anchor1,
+            local_anchor2,
+            axis1,
+            local_axis2,
+            position2: rb2.active_set_offset,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        let axis2 = position2 * self.local_axis2;
+
+        let delta_rot =
+            Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp)
+                .unwrap_or(Rotation::identity());
+        position2.rotation = delta_rot * position2.rotation;
+
+        let anchor2 = position2 * self.local_anchor2;
+        let delta_tra = anchor2 - self.anchor1;
+        let lin_error = delta_tra * params.joint_erp;
+        position2.translation.vector -= lin_error;
+
+        positions[self.position2 as usize] = position2;
+    }
+}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -0,0 +1,294 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
+};
+use crate::math::{AngularInertia, Vector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+
+#[derive(Debug)]
+pub(crate) struct RevoluteVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r1: Vector<f32>,
+    r2: Vector<f32>,
+
+    inv_lhs: Matrix5<f32>,
+    rhs: Vector5<f32>,
+    impulse: Vector5<f32>,
+
+    basis1: Matrix3x2<f32>,
+
+    im1: f32,
+    im2: f32,
+
+    ii1_sqrt: AngularInertia<f32>,
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl RevoluteVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &RevoluteJoint,
+    ) -> Self {
+        // Linear part.
+        let anchor1 = rb1.position * cparams.local_anchor1;
+        let anchor2 = rb2.position * cparams.local_anchor2;
+        let basis1 = Matrix3x2::from_columns(&[
+            rb1.position * cparams.basis1[0],
+            rb1.position * cparams.basis1[1],
+        ]);
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = r21 * basis1;
+        // NOTE: to simplify, we use basis2 = basis1.
+        // Though we may want to test if that does not introduce any instability.
+        let im1 = rb1.mass_properties.inv_mass;
+        let im2 = rb2.mass_properties.inv_mass;
+
+        let ii1 = rb1.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r1_mat = r1.gcross_matrix();
+
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r2 = anchor2 - rb2.world_com;
+        let r2_mat = r2.gcross_matrix();
+
+        let mut lhs = Matrix5::zeros();
+        let lhs00 =
+            ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
+        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+
+        // Note that cholesky won't read the upper-right part
+        // of lhs so we don't have to fill it.
+        lhs.fixed_slice_mut::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        RevoluteVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            ii1_sqrt: rb1.world_inv_inertia_sqrt,
+            basis1,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
+            - mj_lambda1.linear
+            - ang_vel1.gcross(self.r1);
+        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct RevoluteVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    r2: Vector<f32>,
+
+    inv_lhs: Matrix5<f32>,
+    rhs: Vector5<f32>,
+    impulse: Vector5<f32>,
+
+    basis1: Matrix3x2<f32>,
+
+    im2: f32,
+
+    ii2_sqrt: AngularInertia<f32>,
+}
+
+impl RevoluteVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        cparams: &RevoluteJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor2;
+        let anchor1;
+        let basis1;
+
+        if flipped {
+            anchor1 = rb1.position * cparams.local_anchor2;
+            anchor2 = rb2.position * cparams.local_anchor1;
+            basis1 = Matrix3x2::from_columns(&[
+                rb1.position * cparams.basis2[0],
+                rb1.position * cparams.basis2[1],
+            ]);
+        } else {
+            anchor1 = rb1.position * cparams.local_anchor1;
+            anchor2 = rb2.position * cparams.local_anchor2;
+            basis1 = Matrix3x2::from_columns(&[
+                rb1.position * cparams.basis1[0],
+                rb1.position * cparams.basis1[1],
+            ]);
+        };
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = /*r21 * */ basis1;
+        let im2 = rb2.mass_properties.inv_mass;
+        let ii2 = rb2.world_inv_inertia_sqrt.squared();
+        let r1 = anchor1 - rb1.world_com;
+        let r2 = anchor2 - rb2.world_com;
+        let r2_mat = r2.gcross_matrix();
+
+        let mut lhs = Matrix5::zeros();
+        let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+
+        // Note that cholesky won't read the upper-right part
+        // of lhs so we don't have to fill it.
+        lhs.fixed_slice_mut::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        RevoluteVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2_sqrt: rb2.world_inv_inertia_sqrt,
+            impulse: cparams.impulse * params.warmstart_coeff,
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let joint = &mut joints_all[self.joint_id].weight;
+        if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
+            revolute.impulse = self.impulse;
+        }
+    }
+}

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -0,0 +1,397 @@
+use simba::simd::SimdValue;
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
+};
+use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, SIMD_WIDTH};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
+
+#[derive(Debug)]
+pub(crate) struct WRevoluteVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r1: Vector<SimdFloat>,
+    r2: Vector<SimdFloat>,
+
+    inv_lhs: Matrix5<SimdFloat>,
+    rhs: Vector5<SimdFloat>,
+    impulse: Vector5<SimdFloat>,
+
+    basis1: Matrix3x2<SimdFloat>,
+
+    im1: SimdFloat,
+    im2: SimdFloat,
+
+    ii1_sqrt: AngularInertia<SimdFloat>,
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WRevoluteVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&RevoluteJoint; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_basis1 = [
+            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+        ];
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+        let basis1 =
+            Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = r21 * basis1;
+        // NOTE: to simplify, we use basis2 = basis1.
+        // Though we may want to test if that does not introduce any instability.
+        let ii1 = ii1_sqrt.squared();
+        let r1 = anchor1 - world_com1;
+        let r1_mat = r1.gcross_matrix();
+
+        let ii2 = ii2_sqrt.squared();
+        let r2 = anchor2 - world_com2;
+        let r2_mat = r2.gcross_matrix();
+
+        let mut lhs = Matrix5::zeros();
+        let lhs00 =
+            ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
+        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+
+        // Note that cholesky won't read the upper-right part
+        // of lhs so we don't have to fill it.
+        lhs.fixed_slice_mut::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        WRevoluteVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            ii1_sqrt,
+            basis1,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            inv_lhs,
+            rhs,
+            r1,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
+            - mj_lambda1.linear
+            - ang_vel1.gcross(self.r1);
+        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::RevoluteJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WRevoluteVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    r2: Vector<SimdFloat>,
+
+    inv_lhs: Matrix5<SimdFloat>,
+    rhs: Vector5<SimdFloat>,
+    impulse: Vector5<SimdFloat>,
+
+    basis1: Matrix3x2<SimdFloat>,
+
+    im2: SimdFloat,
+
+    ii2_sqrt: AngularInertia<SimdFloat>,
+}
+
+impl WRevoluteVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdFloat>::from(
+            array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH],
+        );
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+
+        let local_anchor1 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+        );
+        let local_anchor2 = Point::from(
+            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+        );
+        let basis1 = Matrix3x2::from_columns(&[
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                ),
+            position1
+                * Vector::from(
+                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                ),
+        ]);
+
+        let anchor1 = position1 * local_anchor1;
+        let anchor2 = position2 * local_anchor2;
+
+        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
+        //            .unwrap_or(Rotation::identity())
+        //            .to_rotation_matrix()
+        //            .into_inner();
+        //        let basis2 = /*r21 * */ basis1;
+        let ii2 = ii2_sqrt.squared();
+        let r1 = anchor1 - world_com1;
+        let r2 = anchor2 - world_com2;
+        let r2_mat = r2.gcross_matrix();
+
+        let mut lhs = Matrix5::zeros();
+        let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+
+        // Note that cholesky won't read the upper-right part
+        // of lhs so we don't have to fill it.
+        lhs.fixed_slice_mut::<U3, U3>(0, 0)
+            .copy_from(&lhs00.into_matrix());
+        lhs.fixed_slice_mut::<U2, U3>(3, 0).copy_from(&lhs10);
+        lhs.fixed_slice_mut::<U2, U2>(3, 3).copy_from(&lhs11);
+
+        let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
+
+        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
+        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
+
+        WRevoluteVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2_sqrt,
+            impulse: impulse * SimdFloat::splat(params.warmstart_coeff),
+            basis1,
+            inv_lhs,
+            rhs,
+            r2,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
+        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    // FIXME: duplicated code with the non-ground constraint.
+    pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        for ii in 0..SIMD_WIDTH {
+            let joint = &mut joints_all[self.joint_id[ii]].weight;
+            if let JointParams::RevoluteJoint(rev) = &mut joint.params {
+                rev.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/mod.rs

@@ -0,0 +1,56 @@
+#[cfg(not(feature = "parallel"))]
+pub(crate) use self::island_solver::IslandSolver;
+#[cfg(feature = "parallel")]
+pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext};
+#[cfg(feature = "parallel")]
+pub(self) use self::parallel_position_solver::ParallelPositionSolver;
+#[cfg(feature = "parallel")]
+pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
+#[cfg(not(feature = "parallel"))]
+pub(self) use self::position_solver::PositionSolver;
+#[cfg(not(feature = "parallel"))]
+pub(self) use self::velocity_solver::VelocitySolver;
+pub(self) use delta_vel::DeltaVel;
+pub(self) use interaction_groups::*;
+pub(self) use joint_constraint::*;
+pub(self) use position_constraint::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use position_constraint_wide::*;
+pub(self) use position_ground_constraint::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use position_ground_constraint_wide::*;
+pub(self) use velocity_constraint::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use velocity_constraint_wide::*;
+pub(self) use velocity_ground_constraint::*;
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use velocity_ground_constraint_wide::*;
+
+mod categorization;
+mod delta_vel;
+mod interaction_groups;
+#[cfg(not(feature = "parallel"))]
+mod island_solver;
+mod joint_constraint;
+#[cfg(feature = "parallel")]
+mod parallel_island_solver;
+#[cfg(feature = "parallel")]
+mod parallel_position_solver;
+#[cfg(feature = "parallel")]
+mod parallel_velocity_solver;
+mod position_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod position_constraint_wide;
+mod position_ground_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod position_ground_constraint_wide;
+#[cfg(not(feature = "parallel"))]
+mod position_solver;
+mod velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod velocity_constraint_wide;
+mod velocity_ground_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod velocity_ground_constraint_wide;
+#[cfg(not(feature = "parallel"))]
+mod velocity_solver;

src/dynamics/solver/parallel_island_solver.rs

@@ -0,0 +1,259 @@
+use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
+use crate::dynamics::solver::ParallelPositionSolver;
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::Isometry;
+use crate::utils::WAngularInertia;
+use rayon::Scope;
+use std::sync::atomic::{AtomicUsize, Ordering};
+
+#[macro_export]
+#[doc(hidden)]
+macro_rules! concurrent_loop {
+    (let batch_size = $batch_size: expr;
+     for $elt: ident in $array: ident[$index_stream:expr,$index_count:expr] $f: expr) => {
+        let max_index = $array.len();
+
+        if max_index > 0 {
+            loop {
+                let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
+                if start_index > max_index {
+                    break;
+                }
+
+                let end_index = (start_index + $batch_size).min(max_index);
+                for $elt in &$array[start_index..end_index] {
+                    $f
+                }
+
+                $index_count.fetch_add(end_index - start_index, Ordering::SeqCst);
+            }
+        }
+    };
+
+    (let batch_size = $batch_size: expr;
+     for $elt: ident in $array: ident[$index_stream:expr] $f: expr) => {
+        let max_index = $array.len();
+
+        if max_index > 0 {
+            loop {
+                let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
+                if start_index > max_index {
+                    break;
+                }
+
+                let end_index = (start_index + $batch_size).min(max_index);
+                for $elt in &$array[start_index..end_index] {
+                    $f
+                }
+            }
+        }
+    };
+}
+
+pub(crate) struct ThreadContext {
+    pub batch_size: usize,
+    // Velocity solver.
+    pub constraint_initialization_index: AtomicUsize,
+    pub num_initialized_constraints: AtomicUsize,
+    pub joint_constraint_initialization_index: AtomicUsize,
+    pub num_initialized_joint_constraints: AtomicUsize,
+    pub warmstart_contact_index: AtomicUsize,
+    pub num_warmstarted_contacts: AtomicUsize,
+    pub warmstart_joint_index: AtomicUsize,
+    pub num_warmstarted_joints: AtomicUsize,
+    pub solve_interaction_index: AtomicUsize,
+    pub num_solved_interactions: AtomicUsize,
+    pub impulse_writeback_index: AtomicUsize,
+    pub joint_writeback_index: AtomicUsize,
+    pub body_integration_index: AtomicUsize,
+    pub num_integrated_bodies: AtomicUsize,
+    // Position solver.
+    pub position_constraint_initialization_index: AtomicUsize,
+    pub num_initialized_position_constraints: AtomicUsize,
+    pub position_joint_constraint_initialization_index: AtomicUsize,
+    pub num_initialized_position_joint_constraints: AtomicUsize,
+    pub solve_position_interaction_index: AtomicUsize,
+    pub num_solved_position_interactions: AtomicUsize,
+    pub position_writeback_index: AtomicUsize,
+}
+
+impl ThreadContext {
+    pub fn new(batch_size: usize) -> Self {
+        ThreadContext {
+            batch_size, // TODO perhaps there is some optimal value we can compute depending on the island size?
+            constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_constraints: AtomicUsize::new(0),
+            joint_constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_joint_constraints: AtomicUsize::new(0),
+            num_warmstarted_contacts: AtomicUsize::new(0),
+            warmstart_contact_index: AtomicUsize::new(0),
+            num_warmstarted_joints: AtomicUsize::new(0),
+            warmstart_joint_index: AtomicUsize::new(0),
+            solve_interaction_index: AtomicUsize::new(0),
+            num_solved_interactions: AtomicUsize::new(0),
+            impulse_writeback_index: AtomicUsize::new(0),
+            joint_writeback_index: AtomicUsize::new(0),
+            body_integration_index: AtomicUsize::new(0),
+            num_integrated_bodies: AtomicUsize::new(0),
+            position_constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_position_constraints: AtomicUsize::new(0),
+            position_joint_constraint_initialization_index: AtomicUsize::new(0),
+            num_initialized_position_joint_constraints: AtomicUsize::new(0),
+            solve_position_interaction_index: AtomicUsize::new(0),
+            num_solved_position_interactions: AtomicUsize::new(0),
+            position_writeback_index: AtomicUsize::new(0),
+        }
+    }
+
+    pub fn lock_until_ge(val: &AtomicUsize, target: usize) {
+        if target > 0 {
+            //        let backoff = crossbeam::utils::Backoff::new();
+            std::sync::atomic::fence(Ordering::SeqCst);
+            while val.load(Ordering::Relaxed) < target {
+                //  backoff.spin();
+                // std::thread::yield_now();
+            }
+        }
+    }
+}
+
+pub struct ParallelIslandSolver {
+    mj_lambdas: Vec<DeltaVel<f32>>,
+    positions: Vec<Isometry<f32>>,
+    parallel_groups: ParallelInteractionGroups,
+    parallel_joint_groups: ParallelInteractionGroups,
+    parallel_velocity_solver: ParallelVelocitySolver,
+    parallel_position_solver: ParallelPositionSolver,
+    thread: ThreadContext,
+}
+
+impl ParallelIslandSolver {
+    pub fn new() -> Self {
+        Self {
+            mj_lambdas: Vec::new(),
+            positions: Vec::new(),
+            parallel_groups: ParallelInteractionGroups::new(),
+            parallel_joint_groups: ParallelInteractionGroups::new(),
+            parallel_velocity_solver: ParallelVelocitySolver::new(),
+            parallel_position_solver: ParallelPositionSolver::new(),
+            thread: ThreadContext::new(8),
+        }
+    }
+
+    pub fn solve_island<'s>(
+        &'s mut self,
+        scope: &Scope<'s>,
+        island_id: usize,
+        params: &'s IntegrationParameters,
+        bodies: &'s mut RigidBodySet,
+        manifolds: &'s mut Vec<&'s mut ContactManifold>,
+        manifold_indices: &'s [ContactManifoldIndex],
+        joints: &'s mut Vec<JointGraphEdge>,
+        joint_indices: &[JointIndex],
+    ) {
+        let num_threads = rayon::current_num_threads();
+        let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
+        self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
+        self.parallel_groups
+            .group_interactions(island_id, bodies, manifolds, manifold_indices);
+        self.parallel_joint_groups
+            .group_interactions(island_id, bodies, joints, joint_indices);
+        self.parallel_velocity_solver.init_constraint_groups(
+            island_id,
+            bodies,
+            manifolds,
+            &self.parallel_groups,
+            joints,
+            &self.parallel_joint_groups,
+        );
+        self.parallel_position_solver.init_constraint_groups(
+            island_id,
+            bodies,
+            manifolds,
+            &self.parallel_groups,
+            joints,
+            &self.parallel_joint_groups,
+        );
+
+        self.mj_lambdas.clear();
+        self.mj_lambdas
+            .resize(bodies.active_island(island_id).len(), DeltaVel::zero());
+        self.positions.clear();
+        self.positions
+            .resize(bodies.active_island(island_id).len(), Isometry::identity());
+
+        for _ in 0..num_task_per_island {
+            // We use AtomicPtr because it is Send+Sync while *mut is not.
+            // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818
+            let thread = &self.thread;
+            let mj_lambdas = std::sync::atomic::AtomicPtr::new(&mut self.mj_lambdas as *mut _);
+            let positions = std::sync::atomic::AtomicPtr::new(&mut self.positions as *mut _);
+            let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
+            let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _);
+            let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _);
+            let parallel_velocity_solver =
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _);
+            let parallel_position_solver =
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _);
+
+            scope.spawn(move |_| {
+                // Transmute *mut -> &mut
+                let mj_lambdas: &mut Vec<DeltaVel<f32>> =
+                    unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
+                let positions: &mut Vec<Isometry<f32>> =
+                    unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
+                let bodies: &mut RigidBodySet =
+                    unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
+                let manifolds: &mut Vec<&mut ContactManifold> =
+                    unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
+                let joints: &mut Vec<JointGraphEdge> =
+                    unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) };
+                let parallel_velocity_solver: &mut ParallelVelocitySolver = unsafe {
+                    std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed))
+                };
+                let parallel_position_solver: &mut ParallelPositionSolver = unsafe {
+                    std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed))
+                };
+
+                enable_flush_to_zero!(); // Ensure this is enabled on each thread.
+                parallel_velocity_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
+                parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
+                parallel_velocity_solver
+                    .solve_constraints(&thread, params, manifolds, joints, mj_lambdas);
+
+                // Write results back to rigid bodies and integrate velocities.
+                let island_range = bodies.active_island_range(island_id);
+                let active_bodies = &bodies.active_dynamic_set[island_range];
+                let bodies = &mut bodies.bodies;
+
+                concurrent_loop! {
+                    let batch_size = thread.batch_size;
+                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
+                        let rb = &mut bodies[*handle];
+                        let dvel = mj_lambdas[rb.active_set_offset];
+                        rb.linvel += dvel.linear;
+                        rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
+                        rb.integrate(params.dt());
+                        positions[rb.active_set_offset] = rb.position;
+                    }
+                }
+
+                // We need to way for every body to be integrated because it also
+                // initialized `positions` to the updated values.
+                ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len());
+
+                parallel_position_solver.solve_constraints(&thread, params, positions);
+
+                // Write results back to rigid bodies.
+                concurrent_loop! {
+                    let batch_size = thread.batch_size;
+                    for handle in active_bodies[thread.position_writeback_index] {
+                        let rb = &mut bodies[*handle];
+                        rb.set_position(positions[rb.active_set_offset]);
+                    }
+                }
+            })
+        }
+    }
+}

src/dynamics/solver/parallel_position_solver.rs

@@ -0,0 +1,582 @@
+use super::ParallelInteractionGroups;
+use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
+use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
+use crate::geometry::ContactManifold;
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::{
+    dynamics::solver::{WPositionConstraint, WPositionGroundConstraint},
+    simd::SIMD_WIDTH,
+};
+use std::sync::atomic::Ordering;
+
+pub(crate) enum PositionConstraintDesc {
+    NongroundNongrouped(usize),
+    GroundNongrouped(usize),
+    #[cfg(feature = "simd-is-enabled")]
+    NongroundGrouped([usize; SIMD_WIDTH]),
+    #[cfg(feature = "simd-is-enabled")]
+    GroundGrouped([usize; SIMD_WIDTH]),
+}
+
+pub(crate) struct ParallelPositionSolverContactPart {
+    pub point_point: Vec<usize>,
+    pub plane_point: Vec<usize>,
+    pub ground_point_point: Vec<usize>,
+    pub ground_plane_point: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<AnyPositionConstraint>,
+    pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+pub(crate) struct ParallelPositionSolverJointPart {
+    pub not_ground_interactions: Vec<usize>,
+    pub ground_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<AnyJointPositionConstraint>,
+    pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+impl ParallelPositionSolverContactPart {
+    pub fn new() -> Self {
+        Self {
+            point_point: Vec::new(),
+            plane_point: Vec::new(),
+            ground_point_point: Vec::new(),
+            ground_plane_point: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+            constraint_descs: Vec::new(),
+            parallel_desc_groups: Vec::new(),
+        }
+    }
+}
+impl ParallelPositionSolverJointPart {
+    pub fn new() -> Self {
+        Self {
+            not_ground_interactions: Vec::new(),
+            ground_interactions: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+            constraint_descs: Vec::new(),
+            parallel_desc_groups: Vec::new(),
+        }
+    }
+}
+
+impl ParallelPositionSolverJointPart {
+    pub fn init_constraints_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        joints: &mut [JointGraphEdge],
+        joint_groups: &ParallelInteractionGroups,
+    ) {
+        let mut total_num_constraints = 0;
+        let num_groups = joint_groups.num_groups();
+
+        self.interaction_groups.clear_groups();
+        self.ground_interaction_groups.clear_groups();
+        self.parallel_desc_groups.clear();
+        self.constraint_descs.clear();
+        self.parallel_desc_groups.push(0);
+
+        for i in 0..num_groups {
+            let group = joint_groups.group(i);
+
+            self.not_ground_interactions.clear();
+            self.ground_interactions.clear();
+            categorize_joints(
+                bodies,
+                joints,
+                group,
+                &mut self.ground_interactions,
+                &mut self.not_ground_interactions,
+            );
+
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped = self.interaction_groups.grouped_interactions.len();
+            let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
+            let start_nongrouped_ground =
+                self.ground_interaction_groups.nongrouped_interactions.len();
+
+            self.interaction_groups.group_joints(
+                island_id,
+                bodies,
+                joints,
+                &self.not_ground_interactions,
+            );
+            self.ground_interaction_groups.group_joints(
+                island_id,
+                bodies,
+                joints,
+                &self.ground_interactions,
+            );
+
+            // Compute constraint indices.
+            for interaction_i in
+                &self.interaction_groups.nongrouped_interactions[start_nongrouped..]
+            {
+                let joint = &mut joints[*interaction_i].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, false);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in
+                self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+            {
+                let joint = &mut joints[interaction_i[0]].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, true);
+            }
+
+            for interaction_i in
+                &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+            {
+                let joint = &mut joints[*interaction_i].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, false);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in self.ground_interaction_groups.grouped_interactions
+                [start_grouped_ground..]
+                .chunks(SIMD_WIDTH)
+            {
+                let joint = &mut joints[interaction_i[0]].weight;
+                joint.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints +=
+                    AnyJointPositionConstraint::num_active_constraints(joint, true);
+            }
+
+            self.parallel_desc_groups.push(self.constraint_descs.len());
+        }
+
+        // Resize the constraints set.
+        self.constraints.clear();
+        self.constraints
+            .resize_with(total_num_constraints, || AnyJointPositionConstraint::Empty)
+    }
+
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.position_joint_constraint_initialization_index, thread.num_initialized_position_joint_constraints] {
+                match &desc.1 {
+                    PositionConstraintDesc::NongroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointPositionConstraint::from_joint(
+                            joint,
+                            bodies,
+                        );
+                        self.constraints[joint.position_constraint_index] = constraint;
+                    }
+                    PositionConstraintDesc::GroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointPositionConstraint::from_joint_ground(
+                            joint,
+                            bodies,
+                        );
+                        self.constraints[joint.position_constraint_index] = constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::NongroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint(
+                            joints, bodies,
+                        ) {
+                            self.constraints[joints[0].position_constraint_index] = constraint
+                        } else {
+                            for ii in 0..SIMD_WIDTH {
+                                let constraint = AnyJointPositionConstraint::from_joint(joints[ii], bodies);
+                                self.constraints[joints[0].position_constraint_index + ii] = constraint;
+                            }
+                        }
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::GroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint_ground(
+                            joints, bodies,
+                        ) {
+                            self.constraints[joints[0].position_constraint_index] = constraint
+                        } else {
+                            for ii in 0..SIMD_WIDTH {
+                                let constraint = AnyJointPositionConstraint::from_joint_ground(joints[ii], bodies);
+                                self.constraints[joints[0].position_constraint_index + ii] = constraint;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+impl ParallelPositionSolverContactPart {
+    pub fn init_constraints_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_groups: &ParallelInteractionGroups,
+    ) {
+        let mut total_num_constraints = 0;
+        let num_groups = manifold_groups.num_groups();
+
+        self.interaction_groups.clear_groups();
+        self.ground_interaction_groups.clear_groups();
+        self.parallel_desc_groups.clear();
+        self.constraint_descs.clear();
+        self.parallel_desc_groups.push(0);
+
+        for i in 0..num_groups {
+            let group = manifold_groups.group(i);
+
+            self.ground_point_point.clear();
+            self.ground_plane_point.clear();
+            self.point_point.clear();
+            self.plane_point.clear();
+            categorize_position_contacts(
+                bodies,
+                manifolds,
+                group,
+                &mut self.ground_point_point,
+                &mut self.ground_plane_point,
+                &mut self.point_point,
+                &mut self.plane_point,
+            );
+
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped = self.interaction_groups.grouped_interactions.len();
+            let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+            #[cfg(feature = "simd-is-enabled")]
+            let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
+            let start_nongrouped_ground =
+                self.ground_interaction_groups.nongrouped_interactions.len();
+
+            self.interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.point_point,
+            );
+            self.interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.plane_point,
+            );
+            self.ground_interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.ground_point_point,
+            );
+            self.ground_interaction_groups.group_manifolds(
+                island_id,
+                bodies,
+                manifolds,
+                &self.ground_plane_point,
+            );
+
+            // Compute constraint indices.
+            for interaction_i in
+                &self.interaction_groups.nongrouped_interactions[start_nongrouped..]
+            {
+                let manifold = &mut *manifolds[*interaction_i];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in
+                self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+            {
+                let manifold = &mut *manifolds[interaction_i[0]];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::NongroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            for interaction_i in
+                &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+            {
+                let manifold = &mut *manifolds[*interaction_i];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundNongrouped(*interaction_i),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            #[cfg(feature = "simd-is-enabled")]
+            for interaction_i in self.ground_interaction_groups.grouped_interactions
+                [start_grouped_ground..]
+                .chunks(SIMD_WIDTH)
+            {
+                let manifold = &mut *manifolds[interaction_i[0]];
+                manifold.position_constraint_index = total_num_constraints;
+                self.constraint_descs.push((
+                    total_num_constraints,
+                    PositionConstraintDesc::GroundGrouped(
+                        array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                    ),
+                ));
+                total_num_constraints += PositionConstraint::num_active_constraints(manifold);
+            }
+
+            self.parallel_desc_groups.push(self.constraint_descs.len());
+        }
+
+        // Resize the constraints set.
+        self.constraints.clear();
+        self.constraints
+            .resize_with(total_num_constraints, || AnyPositionConstraint::Empty)
+    }
+
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.position_constraint_initialization_index, thread.num_initialized_position_constraints] {
+                match &desc.1 {
+                    PositionConstraintDesc::NongroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        PositionConstraint::generate(
+                            params,
+                            manifold,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                    PositionConstraintDesc::GroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        PositionGroundConstraint::generate(
+                            params,
+                            manifold,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::NongroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WPositionConstraint::generate(
+                            params,
+                            manifolds,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    PositionConstraintDesc::GroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WPositionGroundConstraint::generate(
+                            params,
+                            manifolds,
+                            bodies,
+                            &mut self.constraints,
+                            false,
+                        );
+                    }
+                }
+            }
+        }
+    }
+}
+
+pub(crate) struct ParallelPositionSolver {
+    part: ParallelPositionSolverContactPart,
+    joint_part: ParallelPositionSolverJointPart,
+}
+
+impl ParallelPositionSolver {
+    pub fn new() -> Self {
+        Self {
+            part: ParallelPositionSolverContactPart::new(),
+            joint_part: ParallelPositionSolverJointPart::new(),
+        }
+    }
+
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_groups: &ParallelInteractionGroups,
+        joints: &mut [JointGraphEdge],
+        joint_groups: &ParallelInteractionGroups,
+    ) {
+        self.part
+            .init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
+        self.joint_part
+            .init_constraints_groups(island_id, bodies, joints, joint_groups);
+    }
+
+    pub fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        joints: &[JointGraphEdge],
+    ) {
+        self.part
+            .fill_constraints(thread, params, bodies, manifolds);
+        self.joint_part.fill_constraints(thread, bodies, joints);
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_position_constraints,
+            self.part.constraint_descs.len(),
+        );
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_position_joint_constraints,
+            self.joint_part.constraint_descs.len(),
+        );
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        if self.part.constraint_descs.len() == 0 {
+            return;
+        }
+
+        /*
+         * Solve constraints.
+         */
+        {
+            // Each thread will concurrently grab thread.batch_size constraint desc to
+            // solve. If the batch size is large enough for to cross the boundary of
+            // a palallel_desc_group, we have to wait util the current group is finished
+            // before starting the next one.
+            let mut start_index = thread
+                .solve_position_interaction_index
+                .fetch_add(thread.batch_size, Ordering::SeqCst);
+            let mut batch_size = thread.batch_size;
+            let contact_descs = &self.part.constraint_descs[..];
+            let joint_descs = &self.joint_part.constraint_descs[..];
+            let mut target_num_desc = 0;
+            let mut shift = 0;
+
+            for _ in 0..params.max_position_iterations {
+                macro_rules! solve {
+                    ($part: expr) => {
+                        // Joint groups.
+                        for group in $part.parallel_desc_groups.windows(2) {
+                            let num_descs_in_group = group[1] - group[0];
+                            target_num_desc += num_descs_in_group;
+
+                            while start_index < group[1] {
+                                let end_index = (start_index + batch_size).min(group[1]);
+
+                                let constraints = if end_index == $part.constraint_descs.len() {
+                                    &mut $part.constraints[$part.constraint_descs[start_index].0..]
+                                } else {
+                                    &mut $part.constraints[$part.constraint_descs[start_index].0
+                                        ..$part.constraint_descs[end_index].0]
+                                };
+
+                                for constraint in constraints {
+                                    constraint.solve(params, positions);
+                                }
+
+                                let num_solved = end_index - start_index;
+                                batch_size -= num_solved;
+
+                                thread
+                                    .num_solved_position_interactions
+                                    .fetch_add(num_solved, Ordering::SeqCst);
+
+                                if batch_size == 0 {
+                                    start_index = thread
+                                        .solve_position_interaction_index
+                                        .fetch_add(thread.batch_size, Ordering::SeqCst);
+                                    start_index -= shift;
+                                    batch_size = thread.batch_size;
+                                } else {
+                                    start_index += num_solved;
+                                }
+                            }
+                            ThreadContext::lock_until_ge(
+                                &thread.num_solved_position_interactions,
+                                target_num_desc,
+                            );
+                        }
+                    };
+                }
+
+                solve!(self.joint_part);
+                shift += joint_descs.len();
+                start_index -= joint_descs.len();
+                solve!(self.part);
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+            }
+        }
+    }
+}

src/dynamics/solver/parallel_velocity_solver.rs

@@ -0,0 +1,485 @@
+use super::ParallelInteractionGroups;
+use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts};
+use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
+use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
+use crate::geometry::ContactManifold;
+#[cfg(feature = "simd-is-enabled")]
+use crate::{
+    dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
+    simd::SIMD_WIDTH,
+};
+use std::sync::atomic::Ordering;
+
+pub(crate) enum VelocityConstraintDesc {
+    NongroundNongrouped(usize),
+    GroundNongrouped(usize),
+    #[cfg(feature = "simd-is-enabled")]
+    NongroundGrouped([usize; SIMD_WIDTH]),
+    #[cfg(feature = "simd-is-enabled")]
+    GroundGrouped([usize; SIMD_WIDTH]),
+}
+
+pub(crate) struct ParallelVelocitySolverPart<Constraint> {
+    pub not_ground_interactions: Vec<usize>,
+    pub ground_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<Constraint>,
+    pub constraint_descs: Vec<(usize, VelocityConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+impl<Constraint> ParallelVelocitySolverPart<Constraint> {
+    pub fn new() -> Self {
+        Self {
+            not_ground_interactions: Vec::new(),
+            ground_interactions: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+            constraint_descs: Vec::new(),
+            parallel_desc_groups: Vec::new(),
+        }
+    }
+}
+
+macro_rules! impl_init_constraints_group {
+    ($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
+        impl ParallelVelocitySolverPart<$Constraint> {
+            pub fn init_constraints_groups(
+                &mut self,
+                island_id: usize,
+                bodies: &RigidBodySet,
+                interactions: &mut [$Interaction],
+                interaction_groups: &ParallelInteractionGroups,
+            ) {
+                let mut total_num_constraints = 0;
+                let num_groups = interaction_groups.num_groups();
+
+                self.interaction_groups.clear_groups();
+                self.ground_interaction_groups.clear_groups();
+                self.parallel_desc_groups.clear();
+                self.constraint_descs.clear();
+                self.parallel_desc_groups.push(0);
+
+                for i in 0..num_groups {
+                    let group = interaction_groups.group(i);
+
+                    self.not_ground_interactions.clear();
+                    self.ground_interactions.clear();
+                    $categorize(
+                        bodies,
+                        interactions,
+                        group,
+                        &mut self.ground_interactions,
+                        &mut self.not_ground_interactions,
+                    );
+
+                    #[cfg(feature = "simd-is-enabled")]
+                    let start_grouped = self.interaction_groups.grouped_interactions.len();
+                    let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+                    #[cfg(feature = "simd-is-enabled")]
+                    let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
+                    let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
+
+                    self.interaction_groups.$group(
+                        island_id,
+                        bodies,
+                        interactions,
+                        &self.not_ground_interactions,
+                    );
+                    self.ground_interaction_groups.$group(
+                        island_id,
+                        bodies,
+                        interactions,
+                        &self.ground_interactions,
+                    );
+
+                    // Compute constraint indices.
+                    for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] {
+                        let interaction = &mut interactions[*interaction_i]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::NongroundNongrouped(*interaction_i),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    #[cfg(feature = "simd-is-enabled")]
+                    for interaction_i in
+                        self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
+                    {
+                        let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::NongroundGrouped(
+                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                            ),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    for interaction_i in
+                        &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
+                    {
+                        let interaction = &mut interactions[*interaction_i]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::GroundNongrouped(*interaction_i),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    #[cfg(feature = "simd-is-enabled")]
+                    for interaction_i in self.ground_interaction_groups.grouped_interactions
+                        [start_grouped_ground..]
+                        .chunks(SIMD_WIDTH)
+                    {
+                        let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
+                        interaction.constraint_index = total_num_constraints;
+                        self.constraint_descs.push((
+                            total_num_constraints,
+                            VelocityConstraintDesc::GroundGrouped(
+                                array![|ii| interaction_i[ii]; SIMD_WIDTH],
+                            ),
+                        ));
+                        total_num_constraints += $num_active_constraints(interaction);
+                    }
+
+                    self.parallel_desc_groups.push(self.constraint_descs.len());
+                }
+
+                // Resize the constraints set.
+                self.constraints.clear();
+                self.constraints
+                    .resize_with(total_num_constraints, || $empty_constraint)
+            }
+        }
+    }
+}
+
+impl_init_constraints_group!(
+    AnyVelocityConstraint,
+    &mut ContactManifold,
+    categorize_velocity_contacts,
+    group_manifolds,
+    VelocityConstraint::num_active_constraints,
+    AnyVelocityConstraint::Empty
+);
+
+impl_init_constraints_group!(
+    AnyJointVelocityConstraint,
+    JointGraphEdge,
+    categorize_joints,
+    group_joints,
+    AnyJointVelocityConstraint::num_active_constraints,
+    AnyJointVelocityConstraint::Empty,
+    weight
+);
+
+impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] {
+                match &desc.1 {
+                    VelocityConstraintDesc::NongroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
+                    }
+                    VelocityConstraintDesc::GroundNongrouped(manifold_id) => {
+                        let manifold = &*manifolds_all[*manifold_id];
+                        VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::NongroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::GroundGrouped(manifold_id) => {
+                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
+                        WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
+                    }
+                }
+            }
+        }
+    }
+}
+
+impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
+    fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
+                match &desc.1 {
+                    VelocityConstraintDesc::NongroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
+                        self.constraints[joint.constraint_index] = constraint;
+                    }
+                    VelocityConstraintDesc::GroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
+                        self.constraints[joint.constraint_index] = constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::NongroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
+                        self.constraints[joints[0].constraint_index] = constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    VelocityConstraintDesc::GroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
+                        self.constraints[joints[0].constraint_index] = constraint;
+                    }
+                }
+            }
+        }
+    }
+}
+
+pub(crate) struct ParallelVelocitySolver {
+    part: ParallelVelocitySolverPart<AnyVelocityConstraint>,
+    joint_part: ParallelVelocitySolverPart<AnyJointVelocityConstraint>,
+}
+
+impl ParallelVelocitySolver {
+    pub fn new() -> Self {
+        Self {
+            part: ParallelVelocitySolverPart::new(),
+            joint_part: ParallelVelocitySolverPart::new(),
+        }
+    }
+
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &mut [&mut ContactManifold],
+        manifold_groups: &ParallelInteractionGroups,
+        joints: &mut [JointGraphEdge],
+        joint_groups: &ParallelInteractionGroups,
+    ) {
+        self.part
+            .init_constraints_groups(island_id, bodies, manifolds, manifold_groups);
+        self.joint_part
+            .init_constraints_groups(island_id, bodies, joints, joint_groups);
+    }
+
+    pub fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        joints: &[JointGraphEdge],
+    ) {
+        self.part
+            .fill_constraints(thread, params, bodies, manifolds);
+        self.joint_part
+            .fill_constraints(thread, params, bodies, joints);
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_constraints,
+            self.part.constraint_descs.len(),
+        );
+        ThreadContext::lock_until_ge(
+            &thread.num_initialized_joint_constraints,
+            self.joint_part.constraint_descs.len(),
+        );
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        manifolds_all: &mut [&mut ContactManifold],
+        joints_all: &mut [JointGraphEdge],
+        mj_lambdas: &mut [DeltaVel<f32>],
+    ) {
+        if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
+            return;
+        }
+
+        /*
+         * Warmstart constraints.
+         */
+        {
+            // Each thread will concurrently grab thread.batch_size constraint desc to
+            // solve. If the batch size is large enough for to cross the boundary of
+            // a parallel_desc_group, we have to wait util the current group is finished
+            // before starting the next one.
+            let mut target_num_desc = 0;
+            let mut start_index = thread
+                .warmstart_contact_index
+                .fetch_add(thread.batch_size, Ordering::SeqCst);
+            let mut batch_size = thread.batch_size;
+            let mut shift = 0;
+
+            macro_rules! warmstart(
+                ($part: expr) => {
+                    for group in $part.parallel_desc_groups.windows(2) {
+                        let num_descs_in_group = group[1] - group[0];
+                        target_num_desc += num_descs_in_group;
+
+                        while start_index < group[1] {
+                            let end_index = (start_index + batch_size).min(group[1]);
+
+                            let constraints = if end_index == $part.constraint_descs.len() {
+                                &mut $part.constraints[$part.constraint_descs[start_index].0..]
+                            } else {
+                                &mut $part.constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
+                            };
+
+                            for constraint in constraints {
+                                constraint.warmstart(mj_lambdas);
+                            }
+
+                            let num_solved = end_index - start_index;
+                            batch_size -= num_solved;
+
+                            thread
+                                .num_warmstarted_contacts
+                                .fetch_add(num_solved, Ordering::SeqCst);
+
+                            if batch_size == 0 {
+                                start_index = thread
+                                    .warmstart_contact_index
+                                    .fetch_add(thread.batch_size, Ordering::SeqCst);
+                                start_index -= shift;
+                                batch_size = thread.batch_size;
+                            } else {
+                                start_index += num_solved;
+                            }
+                        }
+
+                        ThreadContext::lock_until_ge(&thread.num_warmstarted_contacts, target_num_desc);
+                    }
+                }
+            );
+
+            warmstart!(self.joint_part);
+            shift = self.joint_part.constraint_descs.len();
+            start_index -= shift;
+            warmstart!(self.part);
+        }
+
+        /*
+         * Solve constraints.
+         */
+        {
+            // Each thread will concurrently grab thread.batch_size constraint desc to
+            // solve. If the batch size is large enough for to cross the boundary of
+            // a parallel_desc_group, we have to wait util the current group is finished
+            // before starting the next one.
+            let mut start_index = thread
+                .solve_interaction_index
+                .fetch_add(thread.batch_size, Ordering::SeqCst);
+            let mut batch_size = thread.batch_size;
+            let contact_descs = &self.part.constraint_descs[..];
+            let joint_descs = &self.joint_part.constraint_descs[..];
+            let mut target_num_desc = 0;
+            let mut shift = 0;
+
+            for _ in 0..params.max_velocity_iterations {
+                macro_rules! solve {
+                    ($part: expr) => {
+                        // Joint groups.
+                        for group in $part.parallel_desc_groups.windows(2) {
+                            let num_descs_in_group = group[1] - group[0];
+
+                            target_num_desc += num_descs_in_group;
+
+                            while start_index < group[1] {
+                                let end_index = (start_index + batch_size).min(group[1]);
+
+                                let constraints = if end_index == $part.constraint_descs.len() {
+                                    &mut $part.constraints[$part.constraint_descs[start_index].0..]
+                                } else {
+                                    &mut $part.constraints[$part.constraint_descs[start_index].0
+                                        ..$part.constraint_descs[end_index].0]
+                                };
+
+                                //                                println!(
+                                //                                    "Solving a constraint {:?}.",
+                                //                                    rayon::current_thread_index()
+                                //                                );
+                                for constraint in constraints {
+                                    constraint.solve(mj_lambdas);
+                                }
+
+                                let num_solved = end_index - start_index;
+                                batch_size -= num_solved;
+
+                                thread
+                                    .num_solved_interactions
+                                    .fetch_add(num_solved, Ordering::SeqCst);
+
+                                if batch_size == 0 {
+                                    start_index = thread
+                                        .solve_interaction_index
+                                        .fetch_add(thread.batch_size, Ordering::SeqCst);
+                                    start_index -= shift;
+                                    batch_size = thread.batch_size;
+                                } else {
+                                    start_index += num_solved;
+                                }
+                            }
+                            ThreadContext::lock_until_ge(
+                                &thread.num_solved_interactions,
+                                target_num_desc,
+                            );
+                        }
+                    };
+                }
+
+                solve!(self.joint_part);
+                shift += joint_descs.len();
+                start_index -= joint_descs.len();
+                solve!(self.part);
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+            }
+        }
+
+        /*
+         * Writeback impulses.
+         */
+        let joint_constraints = &self.joint_part.constraints;
+        let contact_constraints = &self.part.constraints;
+        crate::concurrent_loop! {
+             let batch_size = thread.batch_size;
+             for constraint in joint_constraints[thread.joint_writeback_index] {
+                 constraint.writeback_impulses(joints_all);
+             }
+        }
+        crate::concurrent_loop! {
+             let batch_size = thread.batch_size;
+             for constraint in contact_constraints[thread.impulse_writeback_index] {
+                 constraint.writeback_impulses(manifolds_all);
+             }
+        }
+    }
+}

src/dynamics/solver/position_constraint.rs

@@ -0,0 +1,246 @@
+use crate::dynamics::solver::PositionGroundConstraint;
+#[cfg(feature = "simd-is-enabled")]
+use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+pub(crate) enum AnyPositionConstraint {
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPointPointGround(WPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPlanePointGround(WPositionGroundConstraint),
+    NongroupedPointPointGround(PositionGroundConstraint),
+    NongroupedPlanePointGround(PositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPointPoint(WPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedPlanePoint(WPositionConstraint),
+    NongroupedPointPoint(PositionConstraint),
+    NongroupedPlanePoint(PositionConstraint),
+    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    Empty,
+}
+
+impl AnyPositionConstraint {
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<f32>]) {
+        match self {
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPointPointGround(c) => {
+                c.solve_point_point(params, positions)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPlanePointGround(c) => {
+                c.solve_plane_point(params, positions)
+            }
+            AnyPositionConstraint::NongroupedPointPointGround(c) => {
+                c.solve_point_point(params, positions)
+            }
+            AnyPositionConstraint::NongroupedPlanePointGround(c) => {
+                c.solve_plane_point(params, positions)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPointPoint(c) => c.solve_point_point(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyPositionConstraint::GroupedPlanePoint(c) => c.solve_plane_point(params, positions),
+            AnyPositionConstraint::NongroupedPointPoint(c) => {
+                c.solve_point_point(params, positions)
+            }
+            AnyPositionConstraint::NongroupedPlanePoint(c) => {
+                c.solve_plane_point(params, positions)
+            }
+            AnyPositionConstraint::Empty => unreachable!(),
+        }
+    }
+}
+
+pub(crate) struct PositionConstraint {
+    pub rb1: usize,
+    pub rb2: usize,
+    // NOTE: the points are relative to the center of masses.
+    pub local_p1: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub local_n1: Vector<f32>,
+    pub num_contacts: u8,
+    pub radius: f32,
+    pub im1: f32,
+    pub im2: f32,
+    pub ii1: AngularInertia<f32>,
+    pub ii2: AngularInertia<f32>,
+    pub erp: f32,
+    pub max_linear_correction: f32,
+}
+
+impl PositionConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
+        let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
+        manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
+    }
+
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+        let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
+        let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
+        let radius =
+            manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
+            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
+
+            for l in 0..manifold_points.len() {
+                local_p1[l] = manifold_points[l].local_p1 + shift1;
+                local_p2[l] = manifold_points[l].local_p2 + shift2;
+            }
+
+            let constraint = PositionConstraint {
+                rb1: rb1.active_set_offset,
+                rb2: rb2.active_set_offset,
+                local_p1,
+                local_p2,
+                local_n1: manifold.local_n1,
+                radius,
+                im1: rb1.mass_properties.inv_mass,
+                im2: rb2.mass_properties.inv_mass,
+                ii1: rb1.world_inv_inertia_sqrt.squared(),
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                num_contacts: manifold_points.len() as u8,
+                erp: params.erp,
+                max_linear_correction: params.max_linear_correction,
+            };
+
+            if push {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPointPoint(constraint));
+                } else {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPlanePoint(constraint));
+                }
+            } else {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPointPoint(constraint);
+                } else {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPlanePoint(constraint);
+                }
+            }
+        }
+    }
+
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos1 = positions[self.rb1];
+        let mut pos2 = positions[self.rb2];
+        let allowed_err = params.allowed_linear_error;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+
+            let sqdist = dpos.norm_squared();
+
+            // NOTE: only works for the point-point case.
+            if sqdist < target_dist * target_dist {
+                let dist = sqdist.sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n);
+                let gcross2 = -dp2.gcross(n);
+                let ii_gcross1 = self.ii1.transform_vector(gcross1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r =
+                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                let tra1 = Translation::from(n * (impulse * self.im1));
+                let tra2 = Translation::from(n * (-impulse * self.im2));
+                let rot1 = Rotation::new(ii_gcross1 * impulse);
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+
+                pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb1] = pos1;
+        positions[self.rb2] = pos2;
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos1 = positions[self.rb1];
+        let mut pos2 = positions[self.rb2];
+        let allowed_err = params.allowed_linear_error;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let n1 = pos1 * self.local_n1;
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+            let dist = dpos.dot(&n1);
+
+            if dist < target_dist {
+                let p1 = p2 - n1 * dist;
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n1);
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross1 = self.ii1.transform_vector(gcross1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r =
+                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                let tra1 = Translation::from(n1 * (impulse * self.im1));
+                let tra2 = Translation::from(n1 * (-impulse * self.im2));
+                let rot1 = Rotation::new(ii_gcross1 * impulse);
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+
+                pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb1] = pos1;
+        positions[self.rb2] = pos2;
+    }
+}

src/dynamics/solver/position_constraint_wide.rs

@@ -0,0 +1,217 @@
+use super::AnyPositionConstraint;
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+use num::Zero;
+use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
+
+pub(crate) struct WPositionConstraint {
+    pub rb1: [usize; SIMD_WIDTH],
+    pub rb2: [usize; SIMD_WIDTH],
+    // NOTE: the points are relative to the center of masses.
+    pub local_p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub local_n1: Vector<SimdFloat>,
+    pub radius: SimdFloat,
+    pub im1: SimdFloat,
+    pub im2: SimdFloat,
+    pub ii1: AngularInertia<SimdFloat>,
+    pub ii2: AngularInertia<SimdFloat>,
+    pub erp: SimdFloat,
+    pub max_linear_correction: SimdFloat,
+    pub num_contacts: u8,
+}
+
+impl WPositionConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
+        let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let sqrt_ii1: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let sqrt_ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
+        let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
+
+        let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
+        let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
+
+        let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
+
+            let mut constraint = WPositionConstraint {
+                rb1,
+                rb2,
+                local_p1: [Point::origin(); MAX_MANIFOLD_POINTS],
+                local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
+                local_n1,
+                radius,
+                im1,
+                im2,
+                ii1: sqrt_ii1.squared(),
+                ii2: sqrt_ii2.squared(),
+                erp: SimdFloat::splat(params.erp),
+                max_linear_correction: SimdFloat::splat(params.max_linear_correction),
+                num_contacts: num_points as u8,
+            };
+
+            let shift1 = local_n1 * -radius1;
+            let shift2 = local_n2 * -radius2;
+
+            for i in 0..num_points {
+                let local_p1 =
+                    Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]);
+                let local_p2 =
+                    Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]);
+
+                constraint.local_p1[i] = local_p1 + shift1;
+                constraint.local_p2[i] = local_p2 + shift2;
+            }
+
+            if push {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints.push(AnyPositionConstraint::GroupedPointPoint(constraint));
+                } else {
+                    out_constraints.push(AnyPositionConstraint::GroupedPlanePoint(constraint));
+                }
+            } else {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPointPoint(constraint);
+                } else {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPlanePoint(constraint);
+                }
+            }
+        }
+    }
+
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+
+            let dpos = p2 - p1;
+            let sqdist = dpos.norm_squared();
+
+            if sqdist.simd_lt(target_dist * target_dist).any() {
+                let dist = sqdist.simd_sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp)
+                    .simd_clamp(-self.max_linear_correction, SimdFloat::zero());
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n);
+                let gcross2 = -dp2.gcross(n);
+                let ii_gcross1 = self.ii1.transform_vector(gcross1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r =
+                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                pos1.translation = Translation::from(n * (impulse * self.im1)) * pos1.translation;
+                pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
+                pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation;
+                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb1[ii]] = pos1.extract(ii);
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb2[ii]] = pos2.extract(ii);
+        }
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let n1 = pos1 * self.local_n1;
+            let p1 = pos1 * self.local_p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+            let dist = dpos.dot(&n1);
+
+            // NOTE: this condition does not seem to be useful perfomancewise?
+            if dist.simd_lt(target_dist).any() {
+                // NOTE: only works for the point-point case.
+                let p1 = p2 - n1 * dist;
+                let err = ((dist - target_dist) * self.erp)
+                    .simd_clamp(-self.max_linear_correction, SimdFloat::zero());
+                let dp1 = p1.coords - pos1.translation.vector;
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross1 = dp1.gcross(n1);
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross1 = self.ii1.transform_vector(gcross1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r =
+                    self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                pos1.translation = Translation::from(n1 * (impulse * self.im1)) * pos1.translation;
+                pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation;
+                pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
+                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb1[ii]] = pos1.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb2[ii]] = pos2.extract(ii);
+        }
+    }
+}

src/dynamics/solver/position_ground_constraint.rs

@@ -0,0 +1,189 @@
+use super::AnyPositionConstraint;
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+pub(crate) struct PositionGroundConstraint {
+    pub rb2: usize,
+    // NOTE: the points are relative to the center of masses.
+    pub p1: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
+    pub n1: Vector<f32>,
+    pub num_contacts: u8,
+    pub radius: f32,
+    pub im2: f32,
+    pub ii2: AngularInertia<f32>,
+    pub erp: f32,
+    pub max_linear_correction: f32,
+}
+
+impl PositionGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let mut rb1 = &bodies[manifold.body_pair.body1];
+        let mut rb2 = &bodies[manifold.body_pair.body2];
+        let flip = !rb2.is_dynamic();
+
+        let local_n1;
+        let local_n2;
+
+        if flip {
+            std::mem::swap(&mut rb1, &mut rb2);
+            local_n1 = manifold.local_n2;
+            local_n2 = manifold.local_n1;
+        } else {
+            local_n1 = manifold.local_n1;
+            local_n2 = manifold.local_n2;
+        };
+
+        let shift1 = local_n1 * -manifold.kinematics.radius1;
+        let shift2 = local_n2 * -manifold.kinematics.radius2;
+        let radius =
+            manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
+            let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
+
+            if flip {
+                // Don't forget that we already swapped rb1 and rb2 above.
+                // So if we flip, only manifold_points[k].{local_p1,local_p2} have to
+                // be swapped.
+                for k in 0..manifold_points.len() {
+                    p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1);
+                    local_p2[k] = manifold_points[k].local_p1 + shift2;
+                }
+            } else {
+                for k in 0..manifold_points.len() {
+                    p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1);
+                    local_p2[k] = manifold_points[k].local_p2 + shift2;
+                }
+            }
+
+            let constraint = PositionGroundConstraint {
+                rb2: rb2.active_set_offset,
+                p1,
+                local_p2,
+                n1: rb1.predicted_position * local_n1,
+                radius,
+                im2: rb2.mass_properties.inv_mass,
+                ii2: rb2.world_inv_inertia_sqrt.squared(),
+                num_contacts: manifold_points.len() as u8,
+                erp: params.erp,
+                max_linear_correction: params.max_linear_correction,
+            };
+
+            if push {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPointPointGround(
+                        constraint,
+                    ));
+                } else {
+                    out_constraints.push(AnyPositionConstraint::NongroupedPlanePointGround(
+                        constraint,
+                    ));
+                }
+            } else {
+                if manifold.kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPointPointGround(constraint);
+                } else {
+                    out_constraints[manifold.constraint_index + l] =
+                        AnyPositionConstraint::NongroupedPlanePointGround(constraint);
+                }
+            }
+        }
+    }
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos2 = positions[self.rb2];
+        let allowed_err = params.allowed_linear_error;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+
+            let sqdist = dpos.norm_squared();
+
+            // NOTE: only works for the point-point case.
+            if sqdist < target_dist * target_dist {
+                let dist = sqdist.sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross2 = -dp2.gcross(n);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                let tra2 = Translation::from(n * (-impulse * self.im2));
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb2] = pos2;
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos2 = positions[self.rb2];
+        let allowed_err = params.allowed_linear_error;
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let n1 = self.n1;
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+            let dist = dpos.dot(&n1);
+
+            if dist < target_dist {
+                let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction);
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                let tra2 = Translation::from(n1 * (-impulse * self.im2));
+                let rot2 = Rotation::new(ii_gcross2 * impulse);
+                pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation);
+            }
+        }
+
+        positions[self.rb2] = pos2;
+    }
+}

src/dynamics/solver/position_ground_constraint_wide.rs

@@ -0,0 +1,199 @@
+use super::AnyPositionConstraint;
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, KinematicsCategory};
+use crate::math::{
+    AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WDot};
+
+use num::Zero;
+use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue};
+
+pub(crate) struct WPositionGroundConstraint {
+    pub rb2: [usize; SIMD_WIDTH],
+    // NOTE: the points are relative to the center of masses.
+    pub p1: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub local_p2: [Point<SimdFloat>; MAX_MANIFOLD_POINTS],
+    pub n1: Vector<SimdFloat>,
+    pub radius: SimdFloat,
+    pub im2: SimdFloat,
+    pub ii2: AngularInertia<SimdFloat>,
+    pub erp: SimdFloat,
+    pub max_linear_correction: SimdFloat,
+    pub num_contacts: u8,
+}
+
+impl WPositionGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyPositionConstraint>,
+        push: bool,
+    ) {
+        let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
+        let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                flipped[ii] = true;
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+            }
+        }
+
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let sqrt_ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let local_n1 = Vector::from(
+            array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
+        );
+        let local_n2 = Vector::from(
+            array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
+        );
+
+        let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
+        let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
+
+        let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+
+        let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
+
+        let n1 = position1 * local_n1;
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
+
+            let mut constraint = WPositionGroundConstraint {
+                rb2,
+                p1: [Point::origin(); MAX_MANIFOLD_POINTS],
+                local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
+                n1,
+                radius,
+                im2,
+                ii2: sqrt_ii2.squared(),
+                erp: SimdFloat::splat(params.erp),
+                max_linear_correction: SimdFloat::splat(params.max_linear_correction),
+                num_contacts: num_points as u8,
+            };
+
+            for i in 0..num_points {
+                let local_p1 = Point::from(
+                    array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH],
+                );
+                let local_p2 = Point::from(
+                    array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH],
+                );
+
+                constraint.p1[i] = position1 * local_p1 - n1 * radius1;
+                constraint.local_p2[i] = local_p2 - local_n2 * radius2;
+            }
+
+            if push {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints
+                        .push(AnyPositionConstraint::GroupedPointPointGround(constraint));
+                } else {
+                    out_constraints
+                        .push(AnyPositionConstraint::GroupedPlanePointGround(constraint));
+                }
+            } else {
+                if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPointPointGround(constraint);
+                } else {
+                    out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                        AnyPositionConstraint::GroupedPlanePointGround(constraint);
+                }
+            }
+        }
+    }
+    pub fn solve_point_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+
+            let dpos = p2 - p1;
+            let sqdist = dpos.norm_squared();
+
+            if sqdist.simd_lt(target_dist * target_dist).any() {
+                let dist = sqdist.simd_sqrt();
+                let n = dpos / dist;
+                let err = ((dist - target_dist) * self.erp)
+                    .simd_clamp(-self.max_linear_correction, SimdFloat::zero());
+                let dp2 = p2.coords - pos2.translation.vector;
+                let gcross2 = -dp2.gcross(n);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation;
+                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb2[ii]] = pos2.extract(ii);
+        }
+    }
+
+    pub fn solve_plane_point(
+        &self,
+        params: &IntegrationParameters,
+        positions: &mut [Isometry<f32>],
+    ) {
+        // FIXME: can we avoid most of the multiplications by pos1/pos2?
+        // Compute jacobians.
+        let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
+        let allowed_err = SimdFloat::splat(params.allowed_linear_error);
+        let target_dist = self.radius - allowed_err;
+
+        for k in 0..self.num_contacts as usize {
+            let n1 = self.n1;
+            let p1 = self.p1[k];
+            let p2 = pos2 * self.local_p2[k];
+            let dpos = p2 - p1;
+            let dist = dpos.dot(&n1);
+
+            // NOTE: this condition does not seem to be useful perfomancewise?
+            if dist.simd_lt(target_dist).any() {
+                let err = ((dist - target_dist) * self.erp)
+                    .simd_clamp(-self.max_linear_correction, SimdFloat::zero());
+                let dp2 = p2.coords - pos2.translation.vector;
+
+                let gcross2 = -dp2.gcross(n1);
+                let ii_gcross2 = self.ii2.transform_vector(gcross2);
+
+                // Compute impulse.
+                let inv_r = self.im2 + gcross2.gdot(ii_gcross2);
+                let impulse = err / inv_r;
+
+                // Apply impulse.
+                pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation;
+                pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            positions[self.rb2[ii]] = pos2.extract(ii);
+        }
+    }
+}

src/dynamics/solver/position_solver.rs

@@ -0,0 +1,451 @@
+use super::{
+    AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+use super::{WPositionConstraint, WPositionGroundConstraint};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts};
+use crate::dynamics::{
+    solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::Isometry;
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+
+pub(crate) struct PositionSolverJointPart {
+    pub nonground_joints: Vec<JointIndex>,
+    pub ground_joints: Vec<JointIndex>,
+    pub nonground_joint_groups: InteractionGroups,
+    pub ground_joint_groups: InteractionGroups,
+    pub constraints: Vec<AnyJointPositionConstraint>,
+}
+
+impl PositionSolverJointPart {
+    pub fn new() -> Self {
+        Self {
+            nonground_joints: Vec::new(),
+            ground_joints: Vec::new(),
+            nonground_joint_groups: InteractionGroups::new(),
+            ground_joint_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+        }
+    }
+}
+
+pub(crate) struct PositionSolverPart {
+    pub point_point_manifolds: Vec<ContactManifoldIndex>,
+    pub plane_point_manifolds: Vec<ContactManifoldIndex>,
+    pub point_point_ground_manifolds: Vec<ContactManifoldIndex>,
+    pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>,
+    pub point_point_groups: InteractionGroups,
+    pub plane_point_groups: InteractionGroups,
+    pub point_point_ground_groups: InteractionGroups,
+    pub plane_point_ground_groups: InteractionGroups,
+    pub constraints: Vec<AnyPositionConstraint>,
+}
+
+impl PositionSolverPart {
+    pub fn new() -> Self {
+        Self {
+            point_point_manifolds: Vec::new(),
+            plane_point_manifolds: Vec::new(),
+            point_point_ground_manifolds: Vec::new(),
+            plane_point_ground_manifolds: Vec::new(),
+            point_point_groups: InteractionGroups::new(),
+            plane_point_groups: InteractionGroups::new(),
+            point_point_ground_groups: InteractionGroups::new(),
+            plane_point_ground_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+        }
+    }
+}
+
+pub(crate) struct PositionSolver {
+    positions: Vec<Isometry<f32>>,
+    part: PositionSolverPart,
+    joint_part: PositionSolverJointPart,
+}
+
+impl PositionSolver {
+    pub fn new() -> Self {
+        Self {
+            positions: Vec::new(),
+            part: PositionSolverPart::new(),
+            joint_part: PositionSolverJointPart::new(),
+        }
+    }
+
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        self.part
+            .init_constraints(island_id, params, bodies, manifolds, manifold_indices);
+        self.joint_part.init_constraints(
+            island_id,
+            params,
+            bodies,
+            joints,
+            joint_constraint_indices,
+        );
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+    ) {
+        self.positions.clear();
+        self.positions.extend(
+            bodies
+                .iter_active_island(island_id)
+                .map(|(_, b)| b.position),
+        );
+
+        for _ in 0..params.max_position_iterations {
+            for constraint in &self.joint_part.constraints {
+                constraint.solve(params, &mut self.positions)
+            }
+
+            for constraint in &self.part.constraints {
+                constraint.solve(params, &mut self.positions)
+            }
+        }
+
+        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+            rb.set_position(self.positions[rb.active_set_offset])
+        });
+    }
+}
+
+impl PositionSolverPart {
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.point_point_ground_manifolds.clear();
+        self.plane_point_ground_manifolds.clear();
+        self.point_point_manifolds.clear();
+        self.plane_point_manifolds.clear();
+        categorize_position_contacts(
+            bodies,
+            manifolds_all,
+            manifold_indices,
+            &mut self.point_point_ground_manifolds,
+            &mut self.plane_point_ground_manifolds,
+            &mut self.point_point_manifolds,
+            &mut self.plane_point_manifolds,
+        );
+
+        self.point_point_groups.clear_groups();
+        self.point_point_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.point_point_manifolds,
+        );
+        self.plane_point_groups.clear_groups();
+        self.plane_point_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.plane_point_manifolds,
+        );
+
+        self.point_point_ground_groups.clear_groups();
+        self.point_point_ground_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.point_point_ground_manifolds,
+        );
+        self.plane_point_ground_groups.clear_groups();
+        self.plane_point_ground_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds_all,
+            &self.plane_point_ground_manifolds,
+        );
+
+        self.constraints.clear();
+
+        /*
+         * Init non-ground contact constraints.
+         */
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.point_point_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+            compute_grouped_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.plane_point_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+        compute_nongrouped_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.point_point_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+        compute_nongrouped_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.plane_point_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+
+        /*
+         * Init ground contact constraints.
+         */
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_ground_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.point_point_ground_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+            compute_grouped_ground_constraints(
+                params,
+                bodies,
+                manifolds_all,
+                &self.plane_point_ground_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+        compute_nongrouped_ground_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.point_point_ground_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+        compute_nongrouped_ground_constraints(
+            params,
+            bodies,
+            manifolds_all,
+            &self.plane_point_ground_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+    }
+}
+
+impl PositionSolverJointPart {
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        self.ground_joints.clear();
+        self.nonground_joints.clear();
+        categorize_joints(
+            bodies,
+            joints,
+            joint_constraint_indices,
+            &mut self.ground_joints,
+            &mut self.nonground_joints,
+        );
+
+        self.nonground_joint_groups.clear_groups();
+        self.nonground_joint_groups
+            .group_joints(island_id, bodies, joints, &self.nonground_joints);
+
+        self.ground_joint_groups.clear_groups();
+        self.ground_joint_groups
+            .group_joints(island_id, bodies, joints, &self.ground_joints);
+
+        /*
+         * Init ground joint constraints.
+         */
+        self.constraints.clear();
+        compute_nongrouped_joint_ground_constraints(
+            params,
+            bodies,
+            joints,
+            &self.ground_joint_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_joint_ground_constraints(
+                params,
+                bodies,
+                joints,
+                &self.ground_joint_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+
+        /*
+         * Init non-ground joint constraints.
+         */
+        compute_nongrouped_joint_constraints(
+            params,
+            bodies,
+            joints,
+            &self.nonground_joint_groups.nongrouped_interactions,
+            &mut self.constraints,
+        );
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            compute_grouped_joint_constraints(
+                params,
+                bodies,
+                joints,
+                &self.nonground_joint_groups.grouped_interactions,
+                &mut self.constraints,
+            );
+        }
+    }
+}
+
+fn compute_nongrouped_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
+        PositionConstraint::generate(params, manifold, bodies, output, true)
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
+        let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+        WPositionConstraint::generate(params, manifolds, bodies, output, true)
+    }
+}
+
+fn compute_nongrouped_ground_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
+        PositionGroundConstraint::generate(params, manifold, bodies, output, true)
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_ground_constraints(
+    params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    manifolds_all: &[&mut ContactManifold],
+    manifold_indices: &[ContactManifoldIndex],
+    output: &mut Vec<AnyPositionConstraint>,
+) {
+    for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
+        let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+        WPositionGroundConstraint::generate(params, manifolds, bodies, output, true);
+    }
+}
+
+fn compute_nongrouped_joint_ground_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints_all[*joint_i].weight;
+        let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
+        output.push(pos_constraint);
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_joint_ground_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
+        let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
+        if let Some(pos_constraint) =
+            AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
+        {
+            output.push(pos_constraint);
+        } else {
+            for joint in joints.iter() {
+                output.push(AnyJointPositionConstraint::from_joint_ground(
+                    *joint, bodies,
+                ))
+            }
+        }
+    }
+}
+
+fn compute_nongrouped_joint_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices {
+        let joint = &joints_all[*joint_i];
+        let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies);
+        output.push(pos_constraint);
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+fn compute_grouped_joint_constraints(
+    _params: &IntegrationParameters,
+    bodies: &RigidBodySet,
+    joints_all: &[JointGraphEdge],
+    joint_indices: &[JointIndex],
+    output: &mut Vec<AnyJointPositionConstraint>,
+) {
+    for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
+        let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
+        if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) {
+            output.push(pos_constraint);
+        } else {
+            for joint in joints.iter() {
+                output.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_constraint.rs

@@ -0,0 +1,401 @@
+use super::DeltaVel;
+use crate::dynamics::solver::VelocityGroundConstraint;
+#[cfg(feature = "simd-is-enabled")]
+use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use simba::simd::SimdPartialOrd;
+
+//#[repr(align(64))]
+#[derive(Copy, Clone, Debug)]
+pub(crate) enum AnyVelocityConstraint {
+    NongroupedGround(VelocityGroundConstraint),
+    Nongrouped(VelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    GroupedGround(WVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    Grouped(WVelocityConstraint),
+    #[allow(dead_code)] // The Empty variant is only used with parallel code.
+    Empty,
+}
+
+impl AnyVelocityConstraint {
+    #[cfg(target_arch = "wasm32")]
+    pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> {
+        if let AnyVelocityConstraint::Nongrouped(c) = self {
+            Some(c)
+        } else {
+            None
+        }
+    }
+
+    #[cfg(target_arch = "wasm32")]
+    pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> {
+        if let AnyVelocityConstraint::NongroupedGround(c) = self {
+            Some(c)
+        } else {
+            None
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas),
+            AnyVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        match self {
+            AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas),
+            AnyVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+
+    pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) {
+        match self {
+            AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::Empty => unreachable!(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityConstraintElementPart {
+    pub gcross1: AngVector<f32>,
+    pub gcross2: AngVector<f32>,
+    pub rhs: f32,
+    pub impulse: f32,
+    pub r: f32,
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityConstraintElementPart {
+    fn zero() -> Self {
+        Self {
+            gcross1: na::zero(),
+            gcross2: na::zero(),
+            rhs: 0.0,
+            impulse: 0.0,
+            r: 0.0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityConstraintElement {
+    pub normal_part: VelocityConstraintElementPart,
+    pub tangent_part: [VelocityConstraintElementPart; DIM - 1],
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: VelocityConstraintElementPart::zero(),
+            tangent_part: [VelocityConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityConstraint {
+    pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
+    pub im1: f32,
+    pub im2: f32,
+    pub limit: f32,
+    pub mj_lambda1: usize,
+    pub mj_lambda2: usize,
+    pub manifold_id: ContactManifoldIndex,
+    pub manifold_contact_id: usize,
+    pub num_contacts: u8,
+    pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
+}
+
+impl VelocityConstraint {
+    #[cfg(feature = "parallel")]
+    pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
+        let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
+        manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
+    }
+
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let rb1 = &bodies[manifold.body_pair.body1];
+        let rb2 = &bodies[manifold.body_pair.body2];
+        let mj_lambda1 = rb1.active_set_offset;
+        let mj_lambda2 = rb2.active_set_offset;
+        let force_dir1 = rb1.position * (-manifold.local_n1);
+        let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            #[cfg(not(target_arch = "wasm32"))]
+            let mut constraint = VelocityConstraint {
+                dir1: force_dir1,
+                elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im1: rb1.mass_properties.inv_mass,
+                im2: rb2.mass_properties.inv_mass,
+                limit: manifold.friction,
+                mj_lambda1,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                num_contacts: manifold_points.len() as u8,
+            };
+
+            // TODO: this is a WIP optimization for WASM platforms.
+            // For some reasons, the compiler does not inline the `Vec::push` method
+            // in this method. This generates two memset and one memcpy which are both very
+            // expansive.
+            // This would likely be solved by some kind of "placement-push" (like emplace in C++).
+            // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
+            // avoid spurious copying.
+            // Is this optimization beneficial when targeting non-WASM platforms?
+            //
+            // NOTE: joints have the same problem, but it is not easy to refactor the code that way
+            // for the moment.
+            #[cfg(target_arch = "wasm32")]
+            let constraint = if push {
+                let new_len = out_constraints.len() + 1;
+                unsafe {
+                    out_constraints.resize_with(new_len, || {
+                        AnyVelocityConstraint::Nongrouped(
+                            std::mem::MaybeUninit::uninit().assume_init(),
+                        )
+                    });
+                }
+                out_constraints
+                    .last_mut()
+                    .unwrap()
+                    .as_nongrouped_mut()
+                    .unwrap()
+            } else {
+                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
+            };
+
+            #[cfg(target_arch = "wasm32")]
+            {
+                constraint.dir1 = force_dir1;
+                constraint.im1 = rb1.mass_properties.inv_mass;
+                constraint.im2 = rb2.mass_properties.inv_mass;
+                constraint.limit = manifold.friction;
+                constraint.mj_lambda1 = mj_lambda1;
+                constraint.mj_lambda2 = mj_lambda2;
+                constraint.manifold_id = manifold_id;
+                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.num_contacts = manifold_points.len() as u8;
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let dp1 = (rb1.position * manifold_point.local_p1).coords
+                    - rb1.position.translation.vector;
+                let dp2 = (rb2.position * manifold_point.local_p2).coords
+                    - rb2.position.translation.vector;
+
+                let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
+                let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross1 = rb1
+                        .world_inv_inertia_sqrt
+                        .transform_vector(dp1.gcross(force_dir1));
+                    let gcross2 = rb2
+                        .world_inv_inertia_sqrt
+                        .transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = 1.0
+                        / (rb1.mass_properties.inv_mass
+                            + rb2.mass_properties.inv_mass
+                            + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2));
+
+                    let rhs = (vel1 - vel2).dot(&force_dir1)
+                        + manifold_point.dist.max(0.0) * params.inv_dt();
+
+                    let impulse = manifold_points[k].impulse * warmstart_coeff;
+
+                    constraint.elements[k].normal_part = VelocityConstraintElementPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse,
+                        r,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    let tangents1 = force_dir1.orthonormal_basis();
+
+                    for j in 0..DIM - 1 {
+                        let gcross1 = rb1
+                            .world_inv_inertia_sqrt
+                            .transform_vector(dp1.gcross(tangents1[j]));
+                        let gcross2 = rb2
+                            .world_inv_inertia_sqrt
+                            .transform_vector(dp2.gcross(-tangents1[j]));
+                        let r = 1.0
+                            / (rb1.mass_properties.inv_mass
+                                + rb2.mass_properties.inv_mass
+                                + gcross1.gdot(gcross1)
+                                + gcross2.gdot(gcross2));
+                        let rhs = (vel1 - vel2).dot(&tangents1[j]);
+                        #[cfg(feature = "dim2")]
+                        let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
+                        #[cfg(feature = "dim3")]
+                        let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
+
+                        constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
+                            gcross1,
+                            gcross2,
+                            rhs,
+                            impulse,
+                            r,
+                        };
+                    }
+                }
+            }
+
+            #[cfg(not(target_arch = "wasm32"))]
+            if push {
+                out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
+            } else {
+                out_constraints[manifold.constraint_index + l] =
+                    AnyVelocityConstraint::Nongrouped(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel::zero();
+        let mut mj_lambda2 = DeltaVel::zero();
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
+            mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            // FIXME: move this out of the for loop?
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_part[j];
+                mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
+                mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        mj_lambdas[self.mj_lambda1 as usize].linear += mj_lambda1.linear;
+        mj_lambdas[self.mj_lambda1 as usize].angular += mj_lambda1.angular;
+        mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear;
+        mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        // Solve friction.
+        for i in 0..self.num_contacts as usize {
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let normal_elt = &self.elements[i].normal_part;
+                let elt = &mut self.elements[i].tangent_part[j];
+                let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
+                    + elt.gcross1.gdot(mj_lambda1.angular)
+                    - tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda);
+                mj_lambda1.angular += elt.gcross1 * dlambda;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve penetration.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)
+                - self.dir1.dot(&mj_lambda2.linear)
+                + elt.gcross2.gdot(mj_lambda2.angular)
+                + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda1.linear += self.dir1 * (self.im1 * dlambda);
+            mj_lambda1.angular += elt.gcross1 * dlambda;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        let manifold = &mut manifolds_all[self.manifold_id];
+        let k_base = self.manifold_contact_id;
+
+        for k in 0..self.num_contacts as usize {
+            let active_contacts = manifold.active_contacts_mut();
+            active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
+            #[cfg(feature = "dim2")]
+            {
+                active_contacts[k_base + k].tangent_impulse =
+                    self.elements[k].tangent_part[0].impulse;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                active_contacts[k_base + k].tangent_impulse = [
+                    self.elements[k].tangent_part[0].impulse,
+                    self.elements[k].tangent_part[1].impulse,
+                ];
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_constraint_wide.rs

@@ -0,0 +1,344 @@
+use super::{AnyVelocityConstraint, DeltaVel};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use num::Zero;
+use simba::simd::{SimdPartialOrd, SimdValue};
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraintElementPart {
+    pub gcross1: AngVector<SimdFloat>,
+    pub gcross2: AngVector<SimdFloat>,
+    pub rhs: SimdFloat,
+    pub impulse: SimdFloat,
+    pub r: SimdFloat,
+}
+
+impl WVelocityConstraintElementPart {
+    pub fn zero() -> Self {
+        Self {
+            gcross1: AngVector::zero(),
+            gcross2: AngVector::zero(),
+            rhs: SimdFloat::zero(),
+            impulse: SimdFloat::zero(),
+            r: SimdFloat::zero(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraintElement {
+    pub normal_part: WVelocityConstraintElementPart,
+    pub tangent_parts: [WVelocityConstraintElementPart; DIM - 1],
+}
+
+impl WVelocityConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: WVelocityConstraintElementPart::zero(),
+            tangent_parts: [WVelocityConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityConstraint {
+    pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
+    pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+    pub im1: SimdFloat,
+    pub im2: SimdFloat,
+    pub limit: SimdFloat,
+    pub mj_lambda1: [usize; SIMD_WIDTH],
+    pub mj_lambda2: [usize; SIMD_WIDTH],
+    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+    pub manifold_contact_id: usize,
+}
+
+impl WVelocityConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let inv_dt = SimdFloat::splat(params.inv_dt());
+        let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
+        let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
+
+        let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii1: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+
+        let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
+
+        let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+        let warmstart_multiplier =
+            SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
+        let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
+
+            let mut constraint = WVelocityConstraint {
+                dir1: force_dir1,
+                elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im1,
+                im2,
+                limit: friction,
+                mj_lambda1,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l,
+                num_contacts: num_points as u8,
+            };
+
+            for k in 0..num_points {
+                // FIXME: can we avoid the multiplications by position1/position2 here?
+                // By working as much as possible in local-space.
+                let p1 = position1
+                    * Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]);
+                let p2 = position2
+                    * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
+
+                let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+
+                let impulse =
+                    SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
+
+                let dp1 = p1.coords - position1.translation.vector;
+                let dp2 = p2.coords - position2.translation.vector;
+
+                let vel1 = linvel1 + angvel1.gcross(dp1);
+                let vel2 = linvel2 + angvel2.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = SimdFloat::splat(1.0)
+                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                    let rhs =
+                        (vel1 - vel2).dot(&force_dir1) + dist.simd_max(SimdFloat::zero()) * inv_dt;
+
+                    constraint.elements[k].normal_part = WVelocityConstraintElementPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse: impulse * warmstart_coeff,
+                        r,
+                    };
+                }
+
+                // tangent parts.
+                let tangents1 = force_dir1.orthonormal_basis();
+
+                for j in 0..DIM - 1 {
+                    #[cfg(feature = "dim2")]
+                    let impulse = SimdFloat::from(
+                        array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
+                    );
+                    #[cfg(feature = "dim3")]
+                    let impulse = SimdFloat::from(
+                        array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
+                    );
+
+                    let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
+                    let r = SimdFloat::splat(1.0)
+                        / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
+                    let rhs = (vel1 - vel2).dot(&tangents1[j]);
+
+                    constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart {
+                        gcross1,
+                        gcross2,
+                        rhs,
+                        impulse: impulse * warmstart_coeff,
+                        r,
+                    };
+                }
+            }
+
+            if push {
+                out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
+            } else {
+                out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    AnyVelocityConstraint::Grouped(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse);
+            mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            // FIXME: move this out of the for loop?
+            let tangents1 = self.dir1.orthonormal_basis();
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_parts[j];
+                mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse);
+                mj_lambda1.angular += elt.gcross1 * elt.impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda1 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        // Solve friction first.
+        for i in 0..self.num_contacts as usize {
+            // FIXME: move this out of the for loop?
+            let tangents1 = self.dir1.orthonormal_basis();
+            let normal_elt = &self.elements[i].normal_part;
+
+            for j in 0..DIM - 1 {
+                let elt = &mut self.elements[i].tangent_parts[j];
+                let dimpulse = tangents1[j].dot(&mj_lambda1.linear)
+                    + elt.gcross1.gdot(mj_lambda1.angular)
+                    - tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda);
+                mj_lambda1.angular += elt.gcross1 * dlambda;
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve non-penetration after friction.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)
+                - self.dir1.dot(&mj_lambda2.linear)
+                + elt.gcross2.gdot(mj_lambda2.angular)
+                + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda1.linear += self.dir1 * (self.im1 * dlambda);
+            mj_lambda1.angular += elt.gcross1 * dlambda;
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
+            mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii);
+        }
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        for k in 0..self.num_contacts as usize {
+            let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
+            let tangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_parts[0].impulse.into();
+            #[cfg(feature = "dim3")]
+            let bitangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_parts[1].impulse.into();
+
+            for ii in 0..SIMD_WIDTH {
+                let manifold = &mut manifolds_all[self.manifold_id[ii]];
+                let k_base = self.manifold_contact_id;
+                let active_contacts = manifold.active_contacts_mut();
+                active_contacts[k_base + k].impulse = impulses[ii];
+
+                #[cfg(feature = "dim2")]
+                {
+                    active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    active_contacts[k_base + k].tangent_impulse =
+                        [tangent_impulses[ii], bitangent_impulses[ii]];
+                }
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_ground_constraint.rs

@@ -0,0 +1,300 @@
+use super::{AnyVelocityConstraint, DeltaVel};
+use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use simba::simd::SimdPartialOrd;
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityGroundConstraintElementPart {
+    pub gcross2: AngVector<f32>,
+    pub rhs: f32,
+    pub impulse: f32,
+    pub r: f32,
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityGroundConstraintElementPart {
+    fn zero() -> Self {
+        Self {
+            gcross2: na::zero(),
+            rhs: 0.0,
+            impulse: 0.0,
+            r: 0.0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityGroundConstraintElement {
+    pub normal_part: VelocityGroundConstraintElementPart,
+    pub tangent_part: [VelocityGroundConstraintElementPart; DIM - 1],
+}
+
+#[cfg(not(target_arch = "wasm32"))]
+impl VelocityGroundConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: VelocityGroundConstraintElementPart::zero(),
+            tangent_part: [VelocityGroundConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct VelocityGroundConstraint {
+    pub dir1: Vector<f32>, // Non-penetration force direction for the first body.
+    pub im2: f32,
+    pub limit: f32,
+    pub mj_lambda2: usize,
+    pub manifold_id: ContactManifoldIndex,
+    pub manifold_contact_id: usize,
+    pub num_contacts: u8,
+    pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
+}
+
+impl VelocityGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let mut rb1 = &bodies[manifold.body_pair.body1];
+        let mut rb2 = &bodies[manifold.body_pair.body2];
+        let flipped = !rb2.is_dynamic();
+
+        if flipped {
+            std::mem::swap(&mut rb1, &mut rb2);
+        }
+
+        let mj_lambda2 = rb2.active_set_offset;
+        let force_dir1 = if flipped {
+            // NOTE: we already swapped rb1 and rb2
+            // so we multiply by rb1.position.
+            rb1.position * (-manifold.local_n2)
+        } else {
+            rb1.position * (-manifold.local_n1)
+        };
+
+        let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
+
+        for (l, manifold_points) in manifold
+            .active_contacts()
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            #[cfg(not(target_arch = "wasm32"))]
+            let mut constraint = VelocityGroundConstraint {
+                dir1: force_dir1,
+                elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im2: rb2.mass_properties.inv_mass,
+                limit: manifold.friction,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l * MAX_MANIFOLD_POINTS,
+                num_contacts: manifold_points.len() as u8,
+            };
+
+            // TODO: this is a WIP optimization for WASM platforms.
+            // For some reasons, the compiler does not inline the `Vec::push` method
+            // in this method. This generates two memset and one memcpy which are both very
+            // expansive.
+            // This would likely be solved by some kind of "placement-push" (like emplace in C++).
+            // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to
+            // avoid spurious copying.
+            // Is this optimization beneficial when targeting non-WASM platforms?
+            //
+            // NOTE: joints have the same problem, but it is not easy to refactor the code that way
+            // for the moment.
+            #[cfg(target_arch = "wasm32")]
+            let constraint = if push {
+                let new_len = out_constraints.len() + 1;
+                unsafe {
+                    out_constraints.resize_with(new_len, || {
+                        AnyVelocityConstraint::NongroupedGround(
+                            std::mem::MaybeUninit::uninit().assume_init(),
+                        )
+                    });
+                }
+                out_constraints
+                    .last_mut()
+                    .unwrap()
+                    .as_nongrouped_ground_mut()
+                    .unwrap()
+            } else {
+                unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
+            };
+
+            #[cfg(target_arch = "wasm32")]
+            {
+                constraint.dir1 = force_dir1;
+                constraint.im2 = rb2.mass_properties.inv_mass;
+                constraint.limit = manifold.friction;
+                constraint.mj_lambda2 = mj_lambda2;
+                constraint.manifold_id = manifold_id;
+                constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
+                constraint.num_contacts = manifold_points.len() as u8;
+            }
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let (p1, p2) = if flipped {
+                    // NOTE: we already swapped rb1 and rb2
+                    // so we multiply by rb2.position.
+                    (
+                        rb1.position * manifold_point.local_p2,
+                        rb2.position * manifold_point.local_p1,
+                    )
+                } else {
+                    (
+                        rb1.position * manifold_point.local_p1,
+                        rb2.position * manifold_point.local_p2,
+                    )
+                };
+                let dp2 = p2.coords - rb2.position.translation.vector;
+                let dp1 = p1.coords - rb1.position.translation.vector;
+                let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
+                let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross2 = rb2
+                        .world_inv_inertia_sqrt
+                        .transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
+                    let rhs = -vel2.dot(&force_dir1)
+                        + vel1.dot(&force_dir1)
+                        + manifold_point.dist.max(0.0) * params.inv_dt();
+                    let impulse = manifold_points[k].impulse * warmstart_coeff;
+
+                    constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
+                        gcross2,
+                        rhs,
+                        impulse,
+                        r,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    let tangents1 = force_dir1.orthonormal_basis();
+
+                    for j in 0..DIM - 1 {
+                        let gcross2 = rb2
+                            .world_inv_inertia_sqrt
+                            .transform_vector(dp2.gcross(-tangents1[j]));
+                        let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
+                        let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
+                        #[cfg(feature = "dim2")]
+                        let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
+                        #[cfg(feature = "dim3")]
+                        let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
+
+                        constraint.elements[k].tangent_part[j] =
+                            VelocityGroundConstraintElementPart {
+                                gcross2,
+                                rhs,
+                                impulse,
+                                r,
+                            };
+                    }
+                }
+            }
+
+            #[cfg(not(target_arch = "wasm32"))]
+            if push {
+                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
+            } else {
+                out_constraints[manifold.constraint_index + l] =
+                    AnyVelocityConstraint::NongroupedGround(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel::zero();
+        let tangents1 = self.dir1.orthonormal_basis();
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_part[j];
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear;
+        mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular;
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        // Solve friction.
+        let tangents1 = self.dir1.orthonormal_basis();
+
+        for i in 0..self.num_contacts as usize {
+            for j in 0..DIM - 1 {
+                let normal_elt = &self.elements[i].normal_part;
+                let elt = &mut self.elements[i].tangent_part[j];
+                let dimpulse = -tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve penetration.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse =
+                -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0);
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        let manifold = &mut manifolds_all[self.manifold_id];
+        let k_base = self.manifold_contact_id;
+
+        for k in 0..self.num_contacts as usize {
+            let active_contacts = manifold.active_contacts_mut();
+            active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
+            #[cfg(feature = "dim2")]
+            {
+                active_contacts[k_base + k].tangent_impulse =
+                    self.elements[k].tangent_part[0].impulse;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                active_contacts[k_base + k].tangent_impulse = [
+                    self.elements[k].tangent_part[0].impulse,
+                    self.elements[k].tangent_part[1].impulse,
+                ];
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -0,0 +1,300 @@
+use super::{AnyVelocityConstraint, DeltaVel};
+use crate::dynamics::{IntegrationParameters, RigidBodySet};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS,
+    SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
+use num::Zero;
+use simba::simd::{SimdPartialOrd, SimdValue};
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityGroundConstraintElementPart {
+    pub gcross2: AngVector<SimdFloat>,
+    pub rhs: SimdFloat,
+    pub impulse: SimdFloat,
+    pub r: SimdFloat,
+}
+
+impl WVelocityGroundConstraintElementPart {
+    pub fn zero() -> Self {
+        Self {
+            gcross2: AngVector::zero(),
+            rhs: SimdFloat::zero(),
+            impulse: SimdFloat::zero(),
+            r: SimdFloat::zero(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityGroundConstraintElement {
+    pub normal_part: WVelocityGroundConstraintElementPart,
+    pub tangent_parts: [WVelocityGroundConstraintElementPart; DIM - 1],
+}
+
+impl WVelocityGroundConstraintElement {
+    pub fn zero() -> Self {
+        Self {
+            normal_part: WVelocityGroundConstraintElementPart::zero(),
+            tangent_parts: [WVelocityGroundConstraintElementPart::zero(); DIM - 1],
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct WVelocityGroundConstraint {
+    pub dir1: Vector<SimdFloat>, // Non-penetration force direction for the first body.
+    pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
+    pub num_contacts: u8,
+    pub im2: SimdFloat,
+    pub limit: SimdFloat,
+    pub mj_lambda2: [usize; SIMD_WIDTH],
+    pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+    pub manifold_contact_id: usize,
+}
+
+impl WVelocityGroundConstraint {
+    pub fn generate(
+        params: &IntegrationParameters,
+        manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
+        manifolds: [&ContactManifold; SIMD_WIDTH],
+        bodies: &RigidBodySet,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
+        push: bool,
+    ) {
+        let inv_dt = SimdFloat::splat(params.inv_dt());
+        let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
+        let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
+        let mut flipped = [false; SIMD_WIDTH];
+
+        for ii in 0..SIMD_WIDTH {
+            if !rbs2[ii].is_dynamic() {
+                std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
+                flipped[ii] = true;
+            }
+        }
+
+        let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
+        let ii2: AngularInertia<SimdFloat> =
+            AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+
+        let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
+        let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+
+        let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
+        let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+
+        let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
+        let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
+
+        let force_dir1 = position1
+            * -Vector::from(
+                array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
+            );
+
+        let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
+
+        let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
+        let warmstart_multiplier =
+            SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
+        let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
+
+        for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
+            let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
+            let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
+
+            let mut constraint = WVelocityGroundConstraint {
+                dir1: force_dir1,
+                elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im2,
+                limit: friction,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: l,
+                num_contacts: num_points as u8,
+            };
+
+            for k in 0..num_points {
+                let p1 = position1
+                    * Point::from(
+                        array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH],
+                    );
+                let p2 = position2
+                    * Point::from(
+                        array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
+                    );
+
+                let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
+
+                let impulse =
+                    SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
+                let dp1 = p1.coords - position1.translation.vector;
+                let dp2 = p2.coords - position2.translation.vector;
+
+                let vel1 = linvel1 + angvel1.gcross(dp1);
+                let vel2 = linvel2 + angvel2.gcross(dp2);
+
+                // Normal part.
+                {
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
+
+                    let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
+                    let rhs = -vel2.dot(&force_dir1)
+                        + vel1.dot(&force_dir1)
+                        + dist.simd_max(SimdFloat::zero()) * inv_dt;
+
+                    constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart {
+                        gcross2,
+                        rhs,
+                        impulse: impulse * warmstart_coeff,
+                        r,
+                    };
+                }
+
+                // tangent parts.
+                let tangents1 = force_dir1.orthonormal_basis();
+
+                for j in 0..DIM - 1 {
+                    #[cfg(feature = "dim2")]
+                    let impulse = SimdFloat::from(
+                        array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
+                    );
+                    #[cfg(feature = "dim3")]
+                    let impulse = SimdFloat::from(
+                        array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
+                    );
+
+                    let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
+                    let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2));
+                    let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
+
+                    constraint.elements[k].tangent_parts[j] =
+                        WVelocityGroundConstraintElementPart {
+                            gcross2,
+                            rhs,
+                            impulse: impulse * warmstart_coeff,
+                            r,
+                        };
+                }
+            }
+
+            if push {
+                out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
+            } else {
+                out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
+                    AnyVelocityConstraint::GroupedGround(constraint);
+            }
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        let tangents1 = self.dir1.orthonormal_basis();
+
+        for i in 0..self.num_contacts as usize {
+            let elt = &self.elements[i].normal_part;
+            mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse);
+            mj_lambda2.angular += elt.gcross2 * elt.impulse;
+
+            for j in 0..DIM - 1 {
+                let elt = &self.elements[i].tangent_parts[j];
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse);
+                mj_lambda2.angular += elt.gcross2 * elt.impulse;
+            }
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<f32>]) {
+        let mut mj_lambda2 = DeltaVel {
+            linear: Vector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
+            ),
+            angular: AngVector::from(
+                array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
+            ),
+        };
+
+        // Solve friction first.
+        let tangents1 = self.dir1.orthonormal_basis();
+
+        for i in 0..self.num_contacts as usize {
+            let normal_elt = &self.elements[i].normal_part;
+
+            for j in 0..DIM - 1 {
+                let elt = &mut self.elements[i].tangent_parts[j];
+                let dimpulse = -tangents1[j].dot(&mj_lambda2.linear)
+                    + elt.gcross2.gdot(mj_lambda2.angular)
+                    + elt.rhs;
+                let limit = self.limit * normal_elt.impulse;
+                let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit);
+                let dlambda = new_impulse - elt.impulse;
+                elt.impulse = new_impulse;
+
+                mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda);
+                mj_lambda2.angular += elt.gcross2 * dlambda;
+            }
+        }
+
+        // Solve non-penetration after friction.
+        for i in 0..self.num_contacts as usize {
+            let elt = &mut self.elements[i].normal_part;
+            let dimpulse =
+                -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs;
+            let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero());
+            let dlambda = new_impulse - elt.impulse;
+            elt.impulse = new_impulse;
+
+            mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda);
+            mj_lambda2.angular += elt.gcross2 * dlambda;
+        }
+
+        for ii in 0..SIMD_WIDTH {
+            mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
+            mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii);
+        }
+    }
+
+    // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        for k in 0..self.num_contacts as usize {
+            let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
+            let tangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_parts[0].impulse.into();
+            #[cfg(feature = "dim3")]
+            let bitangent_impulses: [_; SIMD_WIDTH] =
+                self.elements[k].tangent_parts[1].impulse.into();
+
+            for ii in 0..SIMD_WIDTH {
+                let manifold = &mut manifolds_all[self.manifold_id[ii]];
+                let k_base = self.manifold_contact_id;
+                let active_contacts = manifold.active_contacts_mut();
+                active_contacts[k_base + k].impulse = impulses[ii];
+
+                #[cfg(feature = "dim2")]
+                {
+                    active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    active_contacts[k_base + k].tangent_impulse =
+                        [tangent_impulses[ii], bitangent_impulses[ii]];
+                }
+            }
+        }
+    }
+}

src/dynamics/solver/velocity_solver.rs

@@ -0,0 +1,405 @@
+use super::{
+    AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+use super::{WVelocityConstraint, WVelocityGroundConstraint};
+use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts};
+use crate::dynamics::{
+    solver::{AnyVelocityConstraint, DeltaVel},
+    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+use crate::utils::WAngularInertia;
+
+pub(crate) struct VelocitySolver {
+    pub mj_lambdas: Vec<DeltaVel<f32>>,
+    pub contact_part: VelocitySolverPart<AnyVelocityConstraint>,
+    pub joint_part: VelocitySolverPart<AnyJointVelocityConstraint>,
+}
+
+impl VelocitySolver {
+    pub fn new() -> Self {
+        Self {
+            mj_lambdas: Vec::new(),
+            contact_part: VelocitySolverPart::new(),
+            joint_part: VelocitySolverPart::new(),
+        }
+    }
+
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        self.contact_part
+            .init_constraints(island_id, params, bodies, manifolds, manifold_indices);
+        self.joint_part.init_constraints(
+            island_id,
+            params,
+            bodies,
+            joints,
+            joint_constraint_indices,
+        )
+    }
+
+    pub fn solve_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &mut RigidBodySet,
+        manifolds_all: &mut [&mut ContactManifold],
+        joints_all: &mut [JointGraphEdge],
+    ) {
+        self.mj_lambdas.clear();
+        self.mj_lambdas
+            .resize(bodies.active_island(island_id).len(), DeltaVel::zero());
+
+        /*
+         * Warmstart constraints.
+         */
+        for constraint in &self.joint_part.constraints {
+            constraint.warmstart(&mut self.mj_lambdas[..]);
+        }
+
+        for constraint in &self.contact_part.constraints {
+            constraint.warmstart(&mut self.mj_lambdas[..]);
+        }
+
+        /*
+         * Solve constraints.
+         */
+        for _ in 0..params.max_velocity_iterations {
+            for constraint in &mut self.joint_part.constraints {
+                constraint.solve(&mut self.mj_lambdas[..]);
+            }
+
+            for constraint in &mut self.contact_part.constraints {
+                constraint.solve(&mut self.mj_lambdas[..]);
+            }
+        }
+
+        // Update velocities.
+        bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
+            let dvel = self.mj_lambdas[rb.active_set_offset];
+            rb.linvel += dvel.linear;
+            rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular);
+        });
+
+        // Write impulses back into the manifold structures.
+        for constraint in &self.joint_part.constraints {
+            constraint.writeback_impulses(joints_all);
+        }
+
+        for constraint in &self.contact_part.constraints {
+            constraint.writeback_impulses(manifolds_all);
+        }
+    }
+}
+
+pub(crate) struct VelocitySolverPart<Constraint> {
+    pub not_ground_interactions: Vec<usize>,
+    pub ground_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub constraints: Vec<Constraint>,
+}
+
+impl<Constraint> VelocitySolverPart<Constraint> {
+    pub fn new() -> Self {
+        Self {
+            not_ground_interactions: Vec::new(),
+            ground_interactions: Vec::new(),
+            interaction_groups: InteractionGroups::new(),
+            ground_interaction_groups: InteractionGroups::new(),
+            constraints: Vec::new(),
+        }
+    }
+}
+
+impl VelocitySolverPart<AnyVelocityConstraint> {
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.not_ground_interactions.clear();
+        self.ground_interactions.clear();
+        categorize_velocity_contacts(
+            bodies,
+            manifolds,
+            manifold_indices,
+            &mut self.ground_interactions,
+            &mut self.not_ground_interactions,
+        );
+
+        self.interaction_groups.clear_groups();
+        self.interaction_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds,
+            &self.not_ground_interactions,
+        );
+
+        self.ground_interaction_groups.clear_groups();
+        self.ground_interaction_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds,
+            &self.ground_interactions,
+        );
+
+        // NOTE: uncomment this do disable SIMD contact resolution.
+        //        self.interaction_groups
+        //            .nongrouped_interactions
+        //            .append(&mut self.interaction_groups.grouped_interactions);
+        //        self.ground_interaction_groups
+        //            .nongrouped_interactions
+        //            .append(&mut self.ground_interaction_groups.grouped_interactions);
+    }
+
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
+        self.constraints.clear();
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_constraints(params, bodies, manifolds);
+        }
+        self.compute_nongrouped_constraints(params, bodies, manifolds);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_ground_constraints(params, bodies, manifolds);
+        }
+        self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifolds_i in self
+            .interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
+            let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+            WVelocityConstraint::generate(
+                params,
+                manifold_id,
+                manifolds,
+                bodies,
+                &mut self.constraints,
+                true,
+            );
+        }
+    }
+
+    fn compute_nongrouped_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifold_i in &self.interaction_groups.nongrouped_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            VelocityConstraint::generate(
+                params,
+                *manifold_i,
+                manifold,
+                bodies,
+                &mut self.constraints,
+                true,
+            );
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifolds_i in self
+            .ground_interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
+            let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+            WVelocityGroundConstraint::generate(
+                params,
+                manifold_id,
+                manifolds,
+                bodies,
+                &mut self.constraints,
+                true,
+            );
+        }
+    }
+
+    fn compute_nongrouped_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            VelocityGroundConstraint::generate(
+                params,
+                *manifold_i,
+                manifold,
+                bodies,
+                &mut self.constraints,
+                true,
+            )
+        }
+    }
+}
+
+impl VelocitySolverPart<AnyJointVelocityConstraint> {
+    pub fn init_constraints(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        // Generate constraints for joints.
+        self.not_ground_interactions.clear();
+        self.ground_interactions.clear();
+        categorize_joints(
+            bodies,
+            joints,
+            joint_constraint_indices,
+            &mut self.ground_interactions,
+            &mut self.not_ground_interactions,
+        );
+
+        self.constraints.clear();
+
+        self.interaction_groups.clear_groups();
+        self.interaction_groups.group_joints(
+            island_id,
+            bodies,
+            joints,
+            &self.not_ground_interactions,
+        );
+
+        self.ground_interaction_groups.clear_groups();
+        self.ground_interaction_groups.group_joints(
+            island_id,
+            bodies,
+            joints,
+            &self.ground_interactions,
+        );
+        // NOTE: uncomment this do disable SIMD joint resolution.
+        // self.interaction_groups
+        //     .nongrouped_interactions
+        //     .append(&mut self.interaction_groups.grouped_interactions);
+        // self.ground_interaction_groups
+        //     .nongrouped_interactions
+        //     .append(&mut self.ground_interaction_groups.grouped_interactions);
+
+        self.compute_nongrouped_joint_ground_constraints(params, bodies, joints);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_joint_ground_constraints(params, bodies, joints);
+        }
+        self.compute_nongrouped_joint_constraints(params, bodies, joints);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_joint_constraints(params, bodies, joints);
+        }
+    }
+
+    fn compute_nongrouped_joint_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
+            let joint = &joints_all[*joint_i].weight;
+            let vel_constraint =
+                AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies);
+            self.constraints.push(vel_constraint);
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_joint_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joints_i in self
+            .ground_interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
+            let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
+            let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
+                params, joints_id, joints, bodies,
+            );
+            self.constraints.push(vel_constraint);
+        }
+    }
+
+    fn compute_nongrouped_joint_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joint_i in &self.interaction_groups.nongrouped_interactions {
+            let joint = &joints_all[*joint_i].weight;
+            let vel_constraint =
+                AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies);
+            self.constraints.push(vel_constraint);
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_joint_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joints_i in self
+            .interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
+            let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
+            let vel_constraint =
+                AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
+            self.constraints.push(vel_constraint);
+        }
+    }
+}

src/geometry/ball.rs

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

src/geometry/broad_phase.rs

@@ -0,0 +1,255 @@
+use crate::geometry::ColliderHandle;
+use ncollide::bounding_volume::AABB;
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::geometry::WAABB,
+    crate::math::{Point, SIMD_WIDTH},
+    crate::utils::WVec,
+    simba::simd::SimdBool as _,
+};
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct ColliderPair {
+    pub collider1: ColliderHandle,
+    pub collider2: ColliderHandle,
+}
+
+impl ColliderPair {
+    pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
+        ColliderPair {
+            collider1,
+            collider2,
+        }
+    }
+
+    pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self {
+        if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 {
+            Self::new(collider1, collider2)
+        } else {
+            Self::new(collider2, collider1)
+        }
+    }
+
+    pub fn swap(self) -> Self {
+        Self::new(self.collider2, self.collider1)
+    }
+
+    pub fn zero() -> Self {
+        Self {
+            collider1: ColliderHandle::from_raw_parts(0, 0),
+            collider2: ColliderHandle::from_raw_parts(0, 0),
+        }
+    }
+}
+
+pub struct WAABBHierarchyIntersections {
+    curr_level_interferences: Vec<usize>,
+    next_level_interferences: Vec<usize>,
+}
+
+impl WAABBHierarchyIntersections {
+    pub fn new() -> Self {
+        Self {
+            curr_level_interferences: Vec::new(),
+            next_level_interferences: Vec::new(),
+        }
+    }
+
+    pub fn computed_interferences(&self) -> &[usize] {
+        &self.curr_level_interferences[..]
+    }
+
+    pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec<usize> {
+        &mut self.curr_level_interferences
+    }
+}
+
+#[cfg(feature = "simd-is-enabled")]
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct WAABBHierarchy {
+    levels: Vec<Vec<WAABB>>,
+}
+
+#[cfg(feature = "simd-is-enabled")]
+impl WAABBHierarchy {
+    pub fn new(aabbs: &[AABB<f32>]) -> Self {
+        let mut waabbs: Vec<_> = aabbs
+            .chunks_exact(SIMD_WIDTH)
+            .map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH]))
+            .collect();
+
+        if aabbs.len() % SIMD_WIDTH != 0 {
+            let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH;
+            let last_i = aabbs.len() - 1;
+            let last_waabb =
+                WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]);
+            waabbs.push(last_waabb);
+        }
+
+        let mut levels = vec![waabbs];
+
+        loop {
+            let last_level = levels.last().unwrap();
+            let mut next_level = Vec::new();
+
+            for chunk in last_level.chunks_exact(SIMD_WIDTH) {
+                let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]);
+                let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]);
+                next_level.push(WAABB::new(mins, maxs));
+            }
+
+            // Deal with the last non-exact chunk.
+            if last_level.len() % SIMD_WIDTH != 0 {
+                let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH;
+                let last_id = last_level.len() - 1;
+                let mins = array![|ii| last_level[(first_id + ii).min(last_id)]
+                    .mins
+                    .horizontal_inf(); SIMD_WIDTH];
+                let maxs = array![|ii| last_level[(first_id + ii).min(last_id)]
+                    .maxs
+                    .horizontal_sup(); SIMD_WIDTH];
+
+                let mins = Point::from(mins);
+                let maxs = Point::from(maxs);
+                next_level.push(WAABB::new(mins, maxs));
+            }
+
+            if next_level.len() == 1 {
+                levels.push(next_level);
+                break;
+            }
+
+            levels.push(next_level);
+        }
+
+        Self { levels }
+    }
+
+    pub fn compute_interferences_with(
+        &self,
+        aabb: AABB<f32>,
+        workspace: &mut WAABBHierarchyIntersections,
+    ) {
+        let waabb1 = WAABB::splat(aabb);
+        workspace.next_level_interferences.clear();
+        workspace.curr_level_interferences.clear();
+        workspace.curr_level_interferences.push(0);
+
+        for level in self.levels.iter().rev() {
+            for i in &workspace.curr_level_interferences {
+                // This `if let` handle the case when `*i` is out of bounds because
+                // the initial number of aabbs was not a power of SIMD_WIDTH.
+                if let Some(waabb2) = level.get(*i) {
+                    // NOTE: using `intersect.bitmask()` and performing bit comparisons
+                    // is much more efficient than testing if each intersect.extract(i) is true.
+                    let intersect = waabb1.intersects_lanewise(waabb2);
+                    let bitmask = intersect.bitmask();
+
+                    for j in 0..SIMD_WIDTH {
+                        if (bitmask & (1 << j)) != 0 {
+                            workspace.next_level_interferences.push(i * SIMD_WIDTH + j)
+                        }
+                    }
+                }
+            }
+
+            std::mem::swap(
+                &mut workspace.curr_level_interferences,
+                &mut workspace.next_level_interferences,
+            );
+            workspace.next_level_interferences.clear();
+        }
+    }
+}
+
+#[cfg(not(feature = "simd-is-enabled"))]
+#[derive(Clone)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+pub struct WAABBHierarchy {
+    levels: Vec<Vec<AABB<f32>>>,
+}
+
+#[cfg(not(feature = "simd-is-enabled"))]
+impl WAABBHierarchy {
+    const GROUP_SIZE: usize = 4;
+
+    pub fn new(aabbs: &[AABB<f32>]) -> Self {
+        use ncollide::bounding_volume::BoundingVolume;
+
+        let mut levels = vec![aabbs.to_vec()];
+
+        loop {
+            let last_level = levels.last().unwrap();
+            let mut next_level = Vec::new();
+
+            for chunk in last_level.chunks(Self::GROUP_SIZE) {
+                let mut merged = chunk[0];
+                for aabb in &chunk[1..] {
+                    merged.merge(aabb)
+                }
+
+                next_level.push(merged);
+            }
+
+            if next_level.len() == 1 {
+                levels.push(next_level);
+                break;
+            }
+
+            levels.push(next_level);
+        }
+
+        Self { levels }
+    }
+
+    pub fn compute_interferences_with(
+        &self,
+        aabb1: AABB<f32>,
+        workspace: &mut WAABBHierarchyIntersections,
+    ) {
+        use ncollide::bounding_volume::BoundingVolume;
+
+        workspace.next_level_interferences.clear();
+        workspace.curr_level_interferences.clear();
+        workspace.curr_level_interferences.push(0);
+
+        for level in self.levels[1..].iter().rev() {
+            for i in &workspace.curr_level_interferences {
+                for j in 0..Self::GROUP_SIZE {
+                    if let Some(aabb2) = level.get(*i + j) {
+                        if aabb1.intersects(aabb2) {
+                            workspace
+                                .next_level_interferences
+                                .push((i + j) * Self::GROUP_SIZE)
+                        }
+                    }
+                }
+            }
+
+            std::mem::swap(
+                &mut workspace.curr_level_interferences,
+                &mut workspace.next_level_interferences,
+            );
+            workspace.next_level_interferences.clear();
+        }
+
+        // Last level.
+        for i in &workspace.curr_level_interferences {
+            for j in 0..Self::GROUP_SIZE {
+                if let Some(aabb2) = self.levels[0].get(*i + j) {
+                    if aabb1.intersects(aabb2) {
+                        workspace.next_level_interferences.push(i + j)
+                    }
+                }
+            }
+        }
+
+        std::mem::swap(
+            &mut workspace.curr_level_interferences,
+            &mut workspace.next_level_interferences,
+        );
+        workspace.next_level_interferences.clear();
+    }
+}

src/geometry/broad_phase_multi_sap.rs

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

src/geometry/capsule.rs

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

src/geometry/collider.rs

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

src/geometry/collider_set.rs

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

src/geometry/contact.rs

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

src/geometry/contact_generator/ball_ball_contact_generator.rs

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

src/geometry/contact_generator/ball_convex_contact_generator.rs

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

src/geometry/contact_generator/capsule_capsule_contact_generator.rs

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

src/geometry/contact_generator/contact_dispatcher.rs

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

src/geometry/contact_generator/contact_generator.rs

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

src/geometry/contact_generator/cuboid_capsule_contact_generator.rs

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

src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs

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

src/geometry/contact_generator/cuboid_triangle_contact_generator.rs

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

src/geometry/contact_generator/heightfield_shape_contact_generator.rs

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

src/geometry/contact_generator/mod.rs

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

src/geometry/contact_generator/polygon_polygon_contact_generator.rs

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

src/geometry/contact_generator/trimesh_shape_contact_generator.rs

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

src/geometry/cuboid.rs

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

src/geometry/cuboid_feature2d.rs

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

src/geometry/cuboid_feature3d.rs

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

src/geometry/interaction_graph.rs

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

src/geometry/mod.rs

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

src/geometry/narrow_phase.rs

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

src/geometry/polygon.rs

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

src/geometry/polygon_intersection.rs

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

src/geometry/polyhedron_feature3d.rs

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

src/geometry/proximity.rs

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

src/geometry/proximity_detector/ball_ball_proximity_detector.rs

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

src/geometry/proximity_detector/ball_convex_proximity_detector.rs

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

src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs

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

src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs

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

src/geometry/proximity_detector/mod.rs

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

src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs

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

src/geometry/proximity_detector/proximity_detector.rs

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