Start experimenting with a generic joint implementation for joint drives.

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

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 {

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;

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

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

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

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

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_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdReal>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdReal>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdReal>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdReal>,
+
+    im1: SimdReal,
+    im2: SimdReal,
+
+    ii1: AngularInertia<SimdReal>,
+    ii2: AngularInertia<SimdReal>,
+
+    ii1_sqrt: AngularInertia<SimdReal>,
+    ii2_sqrt: AngularInertia<SimdReal>,
+
+    r1: Vector<SimdReal>,
+    r2: Vector<SimdReal>,
+}
+
+impl WGenericVelocityConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; 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::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii1_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs1[ii].effective_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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_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,
+        );
+
+        WGenericVelocityConstraint {
+            joint_id,
+            mj_lambda1,
+            mj_lambda2,
+            im1,
+            im2,
+            ii1,
+            ii2,
+            ii1_sqrt,
+            ii2_sqrt,
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            inv_lhs,
+            r1,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        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<Real>]) {
+        let mut mj_lambda1: DeltaVel<SimdReal> = 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<SimdReal> = 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::GenericJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WGenericVelocityGroundConstraint {
+    mj_lambda2: [usize; SIMD_WIDTH],
+
+    joint_id: [JointIndex; SIMD_WIDTH],
+
+    impulse: SpacialVector<SimdReal>,
+
+    #[cfg(feature = "dim3")]
+    inv_lhs: Matrix6<SimdReal>, // FIXME: replace by Cholesky.
+    #[cfg(feature = "dim3")]
+    rhs: Vector6<SimdReal>,
+
+    #[cfg(feature = "dim2")]
+    inv_lhs: Matrix3<SimdReal>,
+    #[cfg(feature = "dim2")]
+    rhs: Vector3<SimdReal>,
+
+    im2: SimdReal,
+    ii2: AngularInertia<SimdReal>,
+    ii2_sqrt: AngularInertia<SimdReal>,
+    r2: Vector<SimdReal>,
+}
+
+impl WGenericVelocityGroundConstraint {
+    pub fn from_params(
+        params: &IntegrationParameters,
+        joint_id: [JointIndex; SIMD_WIDTH],
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; 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::<SimdReal>::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::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+        let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
+        let ii2_sqrt = AngularInertia::<SimdReal>::from(
+            array![|ii| rbs2[ii].effective_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,
+        );
+
+        WGenericVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2,
+            im2,
+            ii2,
+            ii2_sqrt,
+            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            inv_lhs,
+            r2,
+            rhs,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        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<Real>]) {
+        let mut mj_lambda2: DeltaVel<SimdReal> = 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::GenericJoint(fixed) = &mut joint.params {
+                fixed.impulse = self.impulse.extract(ii)
+            }
+        }
+    }
+}

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -7,12 +7,16 @@ use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 use super::{
     WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint,
-    WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint,
+    WFixedVelocityGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
+    WGenericVelocityConstraint, WGenericVelocityGroundConstraint, WPrismaticVelocityConstraint,
     WPrismaticVelocityGroundConstraint,
 };
 #[cfg(feature = "dim3")]
 #[cfg(feature = "simd-is-enabled")]
 use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
+use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
+    GenericVelocityConstraint, GenericVelocityGroundConstraint,
+};
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
@@ -34,6 +38,12 @@ pub(crate) enum AnyJointVelocityConstraint {
     WFixedConstraint(WFixedVelocityConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedVelocityGroundConstraint),
+    GenericConstraint(GenericVelocityConstraint),
+    GenericGroundConstraint(GenericVelocityGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WGenericConstraint(WGenericVelocityConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WGenericGroundConstraint(WGenericVelocityGroundConstraint),
     PrismaticConstraint(PrismaticVelocityConstraint),
     PrismaticGroundConstraint(PrismaticVelocityGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -79,6 +89,9 @@ impl AnyJointVelocityConstraint {
             JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint(
                 PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p),
             ),
+            JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericConstraint(
+                GenericVelocityConstraint::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),
@@ -109,6 +122,12 @@ impl AnyJointVelocityConstraint {
                     params, joint_id, rbs1, rbs2, joints,
                 ))
             }
+            JointParams::GenericJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WGenericConstraint(
+                    WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
+                )
+            }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -148,6 +167,11 @@ impl AnyJointVelocityConstraint {
             JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint(
                 FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
             ),
+            JointParams::GenericJoint(p) => AnyJointVelocityConstraint::GenericGroundConstraint(
+                GenericVelocityGroundConstraint::from_params(
+                    params, joint_id, rb1, rb2, p, flipped,
+                ),
+            ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointVelocityConstraint::PrismaticGroundConstraint(
                     PrismaticVelocityGroundConstraint::from_params(
@@ -199,6 +223,14 @@ impl AnyJointVelocityConstraint {
                     ),
                 )
             }
+            JointParams::GenericJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointVelocityConstraint::WGenericGroundConstraint(
+                    WGenericVelocityGroundConstraint::from_params(
+                        params, joint_id, rbs1, rbs2, joints, flipped,
+                    ),
+                )
+            }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -235,6 +267,12 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::GenericConstraint(c) => c.warmstart(mj_lambdas),
+            AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WGenericConstraint(c) => c.warmstart(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
@@ -269,6 +307,12 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::GenericConstraint(c) => c.solve(mj_lambdas),
+            AnyJointVelocityConstraint::GenericGroundConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WGenericConstraint(c) => c.solve(mj_lambdas),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WGenericGroundConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas),
             #[cfg(feature = "simd-is-enabled")]
@@ -311,6 +355,16 @@ impl AnyJointVelocityConstraint {
             AnyJointVelocityConstraint::WFixedGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)
             }
+            AnyJointVelocityConstraint::GenericConstraint(c) => c.writeback_impulses(joints_all),
+            AnyJointVelocityConstraint::GenericGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WGenericConstraint(c) => c.writeback_impulses(joints_all),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointVelocityConstraint::WGenericGroundConstraint(c) => {
+                c.writeback_impulses(joints_all)
+            }
             AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all),
             AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => {
                 c.writeback_impulses(joints_all)

src/dynamics/solver/joint_constraint/joint_position_constraint.rs

@@ -1,6 +1,7 @@
 use super::{
     BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint,
-    FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
+    FixedPositionGroundConstraint, GenericPositionConstraint, GenericPositionGroundConstraint,
+    PrismaticPositionConstraint, PrismaticPositionGroundConstraint,
 };
 #[cfg(feature = "dim3")]
 use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
@@ -10,8 +11,8 @@ use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint};
 #[cfg(feature = "simd-is-enabled")]
 use super::{
     WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint,
-    WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
-    WPrismaticPositionGroundConstraint,
+    WFixedPositionGroundConstraint, WGenericPositionConstraint, WGenericPositionGroundConstraint,
+    WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint,
 };
 use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
 #[cfg(feature = "simd-is-enabled")]
@@ -31,6 +32,12 @@ pub(crate) enum AnyJointPositionConstraint {
     WFixedJoint(WFixedPositionConstraint),
     #[cfg(feature = "simd-is-enabled")]
     WFixedGroundConstraint(WFixedPositionGroundConstraint),
+    GenericJoint(GenericPositionConstraint),
+    GenericGroundConstraint(GenericPositionGroundConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WGenericJoint(WGenericPositionConstraint),
+    #[cfg(feature = "simd-is-enabled")]
+    WGenericGroundConstraint(WGenericPositionGroundConstraint),
     PrismaticJoint(PrismaticPositionConstraint),
     PrismaticGroundConstraint(PrismaticPositionGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
@@ -61,6 +68,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint(
                 FixedPositionConstraint::from_params(rb1, rb2, p),
             ),
+            JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericJoint(
+                GenericPositionConstraint::from_params(rb1, rb2, p),
+            ),
             JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint(
                 PrismaticPositionConstraint::from_params(rb1, rb2, p),
             ),
@@ -89,6 +99,12 @@ impl AnyJointPositionConstraint {
                     rbs1, rbs2, joints,
                 ))
             }
+            JointParams::GenericJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
+                    rbs1, rbs2, joints,
+                ))
+            }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -123,6 +139,9 @@ impl AnyJointPositionConstraint {
             JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint(
                 FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
             ),
+            JointParams::GenericJoint(p) => AnyJointPositionConstraint::GenericGroundConstraint(
+                GenericPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
+            ),
             JointParams::PrismaticJoint(p) => {
                 AnyJointPositionConstraint::PrismaticGroundConstraint(
                     PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -161,6 +180,12 @@ impl AnyJointPositionConstraint {
                     WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
                 )
             }
+            JointParams::GenericJoint(_) => {
+                let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
+                AnyJointPositionConstraint::WGenericGroundConstraint(
+                    WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
+                )
+            }
             JointParams::PrismaticJoint(_) => {
                 let joints =
                     array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
@@ -193,6 +218,12 @@ impl AnyJointPositionConstraint {
             AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]
             AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::GenericJoint(c) => c.solve(params, positions),
+            AnyJointPositionConstraint::GenericGroundConstraint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WGenericJoint(c) => c.solve(params, positions),
+            #[cfg(feature = "simd-is-enabled")]
+            AnyJointPositionConstraint::WGenericGroundConstraint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions),
             AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions),
             #[cfg(feature = "simd-is-enabled")]

src/dynamics/solver/joint_constraint/mod.rs

@@ -18,6 +18,21 @@ pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocity
 pub(self) use fixed_velocity_constraint_wide::{
     WFixedVelocityConstraint, WFixedVelocityGroundConstraint,
 };
+pub(self) use generic_position_constraint::{
+    GenericPositionConstraint, GenericPositionGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use generic_position_constraint_wide::{
+    WGenericPositionConstraint, WGenericPositionGroundConstraint,
+};
+pub(self) use generic_velocity_constraint::{
+    GenericVelocityConstraint, GenericVelocityGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+pub(self) use generic_velocity_constraint_wide::{
+    WGenericVelocityConstraint, WGenericVelocityGroundConstraint,
+};
+
 pub(crate) use joint_constraint::AnyJointVelocityConstraint;
 pub(crate) use joint_position_constraint::AnyJointPositionConstraint;
 pub(self) use prismatic_position_constraint::{
@@ -63,6 +78,12 @@ mod fixed_position_constraint_wide;
 mod fixed_velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod fixed_velocity_constraint_wide;
+mod generic_position_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod generic_position_constraint_wide;
+mod generic_velocity_constraint;
+#[cfg(feature = "simd-is-enabled")]
+mod generic_velocity_constraint_wide;
 mod joint_constraint;
 mod joint_position_constraint;
 mod prismatic_position_constraint;

src/lib.rs

@@ -148,4 +148,10 @@ pub mod math {
     /// single contact constraint.
     #[cfg(feature = "dim3")]
     pub const MAX_MANIFOLD_POINTS: usize = 4;
+
+    #[cfg(feature = "dim2")]
+    pub const SPATIAL_DIM: usize = 3;
+
+    #[cfg(feature = "dim3")]
+    pub const SPATIAL_DIM: usize = 6;
 }