First public release of Rapier.

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