First public release of Rapier.

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