Start experimenting with a generic joint implementation for joint drives.

11 files changed, 1334 insertions, 7 deletions

diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs
new file mode 100644
index 0000000..cfea537
--- /dev/null
+++ b/src/dynamics/joint/generic_joint.rs
@@ -0,0 +1,46 @@
+use crate::math::{Isometry, Real, SpacialVector, SPATIAL_DIM};
+
+#[derive(Copy, Clone, Debug)]
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+/// A joint that prevents all relative movement between two bodies.
+///
+/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space.
+pub struct GenericJoint {
+    /// The frame of reference for the first body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor1: Isometry<Real>,
+    /// The frame of reference for the second body affected by this joint, expressed in the local frame
+    /// of the first body.
+    pub local_anchor2: Isometry<Real>,
+    /// The impulse applied to the first body affected by this joint.
+    ///
+    /// The impulse applied to the second body affected by this joint is given by `-impulse`.
+    /// This combines both linear and angular impulses:
+    /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse.
+    /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse.
+    pub impulse: SpacialVector<Real>,
+
+    pub min_position: SpacialVector<Real>,
+    pub max_position: SpacialVector<Real>,
+    pub target_velocity: SpacialVector<Real>,
+    /// The maximum negative impulse the joint can apply on each DoF. Must be <= 0.0
+    pub max_negative_impulse: SpacialVector<Real>,
+    /// The maximum positive impulse the joint can apply on each DoF. Must be >= 0.0
+    pub max_positive_impulse: SpacialVector<Real>,
+}
+
+impl GenericJoint {
+    /// Creates a new fixed joint from the frames of reference of both bodies.
+    pub fn new(local_anchor1: Isometry<Real>, local_anchor2: Isometry<Real>) -> Self {
+        Self {
+            local_anchor1,
+            local_anchor2,
+            impulse: SpacialVector::zeros(),
+            min_position: SpacialVector::zeros(),
+            max_position: SpacialVector::zeros(),
+            target_velocity: SpacialVector::zeros(),
+            max_negative_impulse: SpacialVector::repeat(-Real::MAX),
+            max_positive_impulse: SpacialVector::repeat(Real::MAX),
+        }
+    }
+}
diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs
index 9fe6488..31c5e0a 100644
--- a/src/dynamics/joint/joint.rs
+++ b/src/dynamics/joint/joint.rs
@@ -1,6 +1,8 @@
 #[cfg(feature = "dim3")]
 use crate::dynamics::RevoluteJoint;
-use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle};
+use crate::dynamics::{
+    BallJoint, FixedJoint, GenericJoint, JointHandle, PrismaticJoint, RigidBodyHandle,
+};
 
 #[derive(Copy, Clone)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -17,6 +19,7 @@ pub enum JointParams {
     /// A revolute joint that removes all degrees of degrees of freedom between the affected
     /// bodies except for the translation along one axis.
     RevoluteJoint(RevoluteJoint),
+    GenericJoint(GenericJoint),
 }
 
 impl JointParams {
@@ -26,8 +29,9 @@ impl JointParams {
             JointParams::BallJoint(_) => 0,
             JointParams::FixedJoint(_) => 1,
             JointParams::PrismaticJoint(_) => 2,
+            JointParams::GenericJoint(_) => 3,
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(_) => 3,
+            JointParams::RevoluteJoint(_) => 4,
         }
     }
 
@@ -49,6 +53,15 @@ impl JointParams {
         }
     }
 
+    /// Gets a reference to the underlying generic joint, if `self` is one.
+    pub fn as_generic_joint(&self) -> Option<&GenericJoint> {
+        if let JointParams::GenericJoint(j) = self {
+            Some(j)
+        } else {
+            None
+        }
+    }
+
     /// Gets a reference to the underlying prismatic joint, if `self` is one.
     pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> {
         if let JointParams::PrismaticJoint(j) = self {
@@ -81,6 +94,12 @@ impl From<FixedJoint> for JointParams {
     }
 }
 
+impl From<GenericJoint> for JointParams {
+    fn from(j: GenericJoint) -> Self {
+        JointParams::GenericJoint(j)
+    }
+}
+
 #[cfg(feature = "dim3")]
 impl From<RevoluteJoint> for JointParams {
     fn from(j: RevoluteJoint) -> Self {
diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs
index b4dd60e..92dd715 100644
--- a/src/dynamics/joint/mod.rs
+++ b/src/dynamics/joint/mod.rs
@@ -1,5 +1,6 @@
 pub use self::ball_joint::BallJoint;
 pub use self::fixed_joint::FixedJoint;
+pub use self::generic_joint::GenericJoint;
 pub use self::joint::{Joint, JointParams};
 pub(crate) use self::joint_set::{JointGraphEdge, JointIndex};
 pub use self::joint_set::{JointHandle, JointSet};
@@ -9,6 +10,7 @@ pub use self::revolute_joint::RevoluteJoint;
 
 mod ball_joint;
 mod fixed_joint;
+mod generic_joint;
 mod joint;
 mod joint_set;
 mod prismatic_joint;
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 8c38dc2..3b6a977 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -5,7 +5,7 @@ pub(crate) use self::joint::JointIndex;
 #[cfg(feature = "dim3")]
 pub use self::joint::RevoluteJoint;
 pub use self::joint::{
-    BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint,
+    BallJoint, FixedJoint, GenericJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint,
 };
 pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
 pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
new file mode 100644
index 0000000..f5138ea
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
@@ -0,0 +1,171 @@
+use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
+use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::math::{
+    AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
+    DIM,
+};
+use crate::utils::{WAngularInertia, WCross};
+use na::{Vector3, Vector6};
+
+// FIXME: review this code for the case where the center of masses are not the origin.
+#[derive(Debug)]
+pub(crate) struct GenericPositionConstraint {
+    position1: usize,
+    position2: usize,
+    local_anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
+    im1: Real,
+    im2: Real,
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
+
+    joint: GenericJoint,
+
+    lin_impulse: Cell<Vector3<Real>>,
+    ang_impulse: Cell<Vector3<Real>>,
+}
+
+impl GenericPositionConstraint {
+    pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, joint: &GenericJoint) -> Self {
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
+
+        Self {
+            local_anchor1: joint.local_anchor1,
+            local_anchor2: joint.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,
+            joint: *joint,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        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 r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
+        let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+
+        let delta_pos = anchor1.inverse() * anchor2;
+        let mass_matrix = GenericVelocityConstraint::compute_mass_matrix(
+            &self.joint,
+            self.im1,
+            self.im2,
+            self.ii1,
+            self.ii2,
+            r1,
+            r2,
+            false,
+        );
+
+        let lin_err = delta_pos.translation.vector * params.joint_erp;
+        let ang_err = delta_pos.rotation.scaled_axis() * params.joint_erp;
+        let err = Vector6::new(
+            lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+        );
+        let impulse = mass_matrix * err;
+        let lin_impulse = impulse.xyz();
+        let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]);
+
+        position1.rotation = Rotation::new(
+            self.ii1
+                .transform_vector(ang_impulse + r1.gcross(lin_impulse)),
+        ) * position1.rotation;
+        position2.rotation = Rotation::new(
+            self.ii2
+                .transform_vector(-ang_impulse - r2.gcross(lin_impulse)),
+        ) * position2.rotation;
+
+        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 GenericPositionGroundConstraint {
+    position2: usize,
+    anchor1: Isometry<Real>,
+    local_anchor2: Isometry<Real>,
+    local_com2: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    joint: GenericJoint,
+}
+
+impl GenericPositionGroundConstraint {
+    pub fn from_params(
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        joint: &GenericJoint,
+        flipped: bool,
+    ) -> Self {
+        let anchor1;
+        let local_anchor2;
+
+        if flipped {
+            anchor1 = rb1.predicted_position * joint.local_anchor2;
+            local_anchor2 = joint.local_anchor1;
+        } else {
+            anchor1 = rb1.predicted_position * joint.local_anchor1;
+            local_anchor2 = joint.local_anchor2;
+        };
+
+        Self {
+            anchor1,
+            local_anchor2,
+            position2: rb2.active_set_offset,
+            im2: rb2.effective_inv_mass,
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+            local_com2: rb2.mass_properties.local_com,
+            joint: *joint,
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        let mut position2 = positions[self.position2 as usize];
+
+        let anchor2 = position2 * self.local_anchor2;
+        let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+
+        let delta_pos = self.anchor1.inverse() * anchor2;
+        let mass_matrix = GenericVelocityGroundConstraint::compute_mass_matrix(
+            &self.joint,
+            self.im2,
+            self.ii2,
+            r2,
+            false,
+        );
+
+        let lin_err = delta_pos.translation.vector * params.joint_erp;
+        let ang_err = Vector3::zeros(); // delta_pos.rotation.scaled_axis() * params.joint_erp;
+        let err = Vector6::new(
+            lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
+        );
+        let impulse = mass_matrix * err;
+        let lin_impulse = impulse.xyz();
+        let ang_impulse = Vector3::new(impulse[3], impulse[4], impulse[5]);
+
+        position2.rotation = Rotation::new(
+            self.ii2
+                .transform_vector(-ang_impulse - r2.gcross(lin_impulse)),
+        ) * position2.rotation;
+        position2.translation.vector -= self.im2 * lin_impulse;
+
+        positions[self.position2 as usize] = position2;
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
new file mode 100644
index 0000000..9ceea67
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
@@ -0,0 +1,51 @@
+use super::{GenericPositionConstraint, GenericPositionGroundConstraint};
+use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WGenericPositionConstraint {
+    constraints: [GenericPositionConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WGenericPositionGroundConstraint {
+    constraints: [GenericPositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
new file mode 100644
index 0000000..6f501d2
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
@@ -0,0 +1,460 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{AngularInertia, Dim, Real, SpacialVector, Vector};
+use crate::parry::math::SpatialVector;
+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 GenericVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    impulse: SpacialVector<Real>,
+    max_positive_impulse: SpatialVector<Real>,
+    max_negative_impulse: SpatialVector<Real>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<Real>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<Real>,
+
+    im1: Real,
+    im2: Real,
+
+    ii1: AngularInertia<Real>,
+    ii2: AngularInertia<Real>,
+
+    ii1_sqrt: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+
+    r1: Vector<Real>,
+    r2: Vector<Real>,
+}
+
+impl GenericVelocityConstraint {
+    #[inline(always)]
+    pub fn compute_mass_matrix(
+        joint: &GenericJoint,
+        im1: Real,
+        im2: Real,
+        ii1: AngularInertia<Real>,
+        ii2: AngularInertia<Real>,
+        r1: Vector<Real>,
+        r2: Vector<Real>,
+        velocity_solver: bool,
+    ) -> Matrix6<Real> {
+        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);
+
+            // Adjust the mass matrix to take force limits into account.
+            // If a DoF has a force limit, then we need to make its
+            // constraint independent from the others because otherwise
+            // the force clamping will cause errors to propagate in the
+            // other constraints.
+            if velocity_solver {
+                for i in 0..6 {
+                    if joint.max_negative_impulse[i] > -Real::MAX
+                        || joint.max_positive_impulse[i] < Real::MAX
+                    {
+                        let diag = lhs[(i, i)];
+                        lhs.row_mut(i).fill(0.0);
+                        lhs[(i, i)] = diag;
+                    }
+                }
+            } else {
+                for i in 0..6 {
+                    let diag = lhs[(i, i)];
+                    lhs.row_mut(i).fill(0.0);
+                    lhs[(i, i)] = diag;
+                }
+            }
+        }
+
+        // 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")]
+        return lhs.try_inverse().expect("Singular system.");
+        #[cfg(feature = "dim3")]
+        return lhs.cholesky().expect("Singular system.").inverse();
+    }
+
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        joint: &GenericJoint,
+    ) -> Self {
+        let anchor1 = rb1.position * joint.local_anchor1;
+        let anchor2 = rb2.position * joint.local_anchor2;
+        let im1 = rb1.effective_inv_mass;
+        let im2 = rb2.effective_inv_mass;
+        let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+
+        let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
+        let ang_dvel = -rb1.angvel + rb2.angvel;
+
+        let inv_lhs = Self::compute_mass_matrix(joint, im1, im2, ii1, ii2, r1, r2, true);
+
+        #[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,
+        );
+
+        let impulse = (joint.impulse * params.warmstart_coeff)
+            .inf(&joint.max_positive_impulse)
+            .sup(&joint.max_negative_impulse);
+
+        GenericVelocityConstraint {
+            joint_id,
+            mj_lambda1: rb1.active_set_offset,
+            mj_lambda2: rb2.active_set_offset,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            impulse,
+            max_positive_impulse: joint.max_positive_impulse,
+            max_negative_impulse: joint.max_negative_impulse,
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        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<Real>]) {
+        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 dvel = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let new_impulse = (self.impulse + self.inv_lhs * dvel)
+            .sup(&self.max_negative_impulse)
+            .inf(&self.max_positive_impulse);
+        let effective_impulse = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = effective_impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = effective_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::GenericJoint(fixed) = &mut joint.params {
+            fixed.impulse = self.impulse;
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct GenericVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    impulse: SpacialVector<Real>,
+    max_positive_impulse: SpatialVector<Real>,
+    max_negative_impulse: SpatialVector<Real>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<Real>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<Real>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<Real>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<Real>,
+
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+    r2: Vector<Real>,
+}
+
+impl GenericVelocityGroundConstraint {
+    #[inline(always)]
+    pub fn compute_mass_matrix(
+        joint: &GenericJoint,
+        im2: Real,
+        ii2: AngularInertia<Real>,
+        r2: Vector<Real>,
+        velocity_solver: bool,
+    ) -> Matrix6<Real> {
+        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);
+
+            // Adjust the mass matrix to take force limits into account.
+            // If a DoF has a force limit, then we need to make its
+            // constraint independent from the others because otherwise
+            // the force clamping will cause errors to propagate in the
+            // other constraints.
+            if velocity_solver {
+                for i in 0..6 {
+                    if joint.max_negative_impulse[i] > -Real::MAX
+                        || joint.max_positive_impulse[i] < Real::MAX
+                    {
+                        let diag = lhs[(i, i)];
+                        lhs.row_mut(i).fill(0.0);
+                        lhs[(i, i)] = diag;
+                    }
+                }
+            }
+        }
+
+        // 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")]
+        return lhs.try_inverse().expect("Singular system.");
+        #[cfg(feature = "dim3")]
+        return lhs.cholesky().expect("Singular system.").inverse();
+    }
+
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: JointIndex,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+        joint: &GenericJoint,
+        flipped: bool,
+    ) -> Self {
+        let (anchor1, anchor2) = if flipped {
+            (
+                rb1.position * joint.local_anchor2,
+                rb2.position * joint.local_anchor1,
+            )
+        } else {
+            (
+                rb1.position * joint.local_anchor1,
+                rb2.position * joint.local_anchor2,
+            )
+        };
+
+        let r1 = anchor1.translation.vector - rb1.world_com.coords;
+        let im2 = rb2.effective_inv_mass;
+        let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+        let r2 = anchor2.translation.vector - rb2.world_com.coords;
+
+        let inv_lhs = Self::compute_mass_matrix(joint, im2, ii2, r2, true);
+
+        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,
+        );
+
+        let impulse = (joint.impulse * params.warmstart_coeff)
+            .inf(&joint.max_positive_impulse)
+            .sup(&joint.max_negative_impulse);
+
+        GenericVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            impulse,
+            max_positive_impulse: joint.max_positive_impulse,
+            max_negative_impulse: joint.max_negative_impulse,
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        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<Real>]) {
+        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 dvel = Vector6::new(
+            dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z,
+        ) + self.rhs;
+
+        let new_impulse = (self.impulse + self.inv_lhs * dvel)
+            .sup(&self.max_negative_impulse)
+            .inf(&self.max_positive_impulse);
+        let effective_impulse = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        let lin_impulse = effective_impulse.fixed_rows::<Dim>(0).into_owned();
+        #[cfg(feature = "dim2")]
+        let ang_impulse = effective_impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = effective_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::GenericJoint(fixed) = &mut joint.params {
+            fixed.impulse = self.impulse;
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
new file mode 100644
index 0000000..8a6be8c
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
@@ -0,0 +1,472 @@
+use simba::simd::SimdValue;
+
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{
+    AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, Real, SimdReal, SpacialVector,
+    Vector, SIMD_WIDTH,
+};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Cholesky, Matrix6, Vector6, U3};
+#[cfg(feature = "dim2")]
+use {
+    na::{Matrix3, Vector3},
+    parry::utils::SdpMatrix3,
+};
+
+#[derive(Debug)]
+pub(crate) struct WGenericVelocityConstraint {
+    mj_lambda1: [usize; SIMD_WIDTH],
+    mj_lambda2: [usize; SIMD_WID