Merge pull request #119 from dimforge/joint_drive

Add joint motors

src/dynamics/solver/interaction_groups.rs

@@ -214,6 +214,12 @@ impl InteractionGroups {
                 continue;
             }
 
+            if !interaction.supports_simd_constraints() {
+                // This joint does not support simd constraints yet.
+                self.nongrouped_interactions.push(*interaction_i);
+                continue;
+            }
+
             let ijoint = interaction.params.type_id();
             let i1 = body1.active_set_offset;
             let i2 = body2.active_set_offset;

src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs

@@ -2,7 +2,7 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
 };
-use crate::math::{AngularInertia, Real, SdpMatrix, Vector};
+use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 
 #[derive(Debug)]
@@ -13,13 +13,18 @@ pub(crate) struct BallVelocityConstraint {
     joint_id: JointIndex,
 
     rhs: Vector<Real>,
-    pub(crate) impulse: Vector<Real>,
+    impulse: Vector<Real>,
 
     r1: Vector<Real>,
     r2: Vector<Real>,
 
     inv_lhs: SdpMatrix<Real>,
 
+    motor_rhs: AngVector<Real>,
+    motor_impulse: AngVector<Real>,
+    motor_inv_lhs: Option<AngularInertia<Real>>,
+    motor_max_impulse: Real,
+
     im1: Real,
     im2: Real,
 
@@ -33,10 +38,10 @@ impl BallVelocityConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &BallJoint,
+        joint: &BallJoint,
     ) -> Self {
-        let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com;
-        let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com;
+        let anchor1 = rb1.position * joint.local_anchor1 - rb1.world_com;
+        let anchor2 = rb2.position * joint.local_anchor2 - rb2.world_com;
 
         let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
         let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
@@ -77,17 +82,85 @@ impl BallVelocityConstraint {
 
         let inv_lhs = lhs.inverse_unchecked();
 
+        /*
+         * Motor part.
+         */
+        let mut motor_rhs = na::zero();
+        let mut motor_inv_lhs = None;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        if motor_max_impulse > 0.0 {
+            let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+                params.dt,
+                joint.motor_stiffness,
+                joint.motor_damping,
+            );
+
+            if stiffness != 0.0 {
+                let dpos = rb2.position.rotation
+                    * (rb1.position.rotation * joint.motor_target_pos).inverse();
+                #[cfg(feature = "dim2")]
+                {
+                    motor_rhs += dpos.angle() * stiffness;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    motor_rhs += dpos.scaled_axis() * stiffness;
+                }
+            }
+
+            if damping != 0.0 {
+                let curr_vel = rb2.angvel - rb1.angvel;
+                motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+            }
+
+            #[cfg(feature = "dim2")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some(gamma / (ii1 + ii2))
+                } else {
+                    Some(gamma)
+                };
+                motor_rhs /= gamma;
+            }
+
+            #[cfg(feature = "dim3")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some((ii1 + ii2).inverse_unchecked() * gamma)
+                } else {
+                    Some(SdpMatrix::diagonal(gamma))
+                };
+                motor_rhs /= gamma;
+            }
+        }
+
+        #[cfg(feature = "dim2")]
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+        #[cfg(feature = "dim3")]
+        let motor_impulse =
+            joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
+
         BallVelocityConstraint {
             joint_id,
             mj_lambda1: rb1.active_set_offset,
             mj_lambda2: rb2.active_set_offset,
             im1,
             im2,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             r1: anchor1,
             r2: anchor2,
             rhs,
             inv_lhs,
+            motor_rhs,
+            motor_impulse,
+            motor_inv_lhs,
+            motor_max_impulse: joint.motor_max_impulse,
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
@@ -98,18 +171,19 @@ impl BallVelocityConstraint {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
         mj_lambda1.linear += self.im1 * self.impulse;
-        mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(self.impulse));
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(self.r1.gcross(self.impulse) + self.motor_impulse);
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(self.r2.gcross(self.impulse) + self.motor_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];
-
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -124,6 +198,37 @@ impl BallVelocityConstraint {
 
         mj_lambda2.linear -= self.im2 * impulse;
         mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
+    }
+
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dangvel = (ang_vel2 - ang_vel1) + self.motor_rhs;
+
+            let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
+
+            #[cfg(feature = "dim2")]
+            let clamped_impulse =
+                na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
+            #[cfg(feature = "dim3")]
+            let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
+
+            let effective_impulse = clamped_impulse - self.motor_impulse;
+            self.motor_impulse = clamped_impulse;
+
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(effective_impulse);
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
+        }
+    }
+
+    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];
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -132,7 +237,8 @@ impl BallVelocityConstraint {
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse
+            ball.impulse = self.impulse;
+            ball.motor_impulse = self.motor_impulse;
         }
     }
 }
@@ -141,10 +247,17 @@ impl BallVelocityConstraint {
 pub(crate) struct BallVelocityGroundConstraint {
     mj_lambda2: usize,
     joint_id: JointIndex,
+    r2: Vector<Real>,
+
     rhs: Vector<Real>,
     impulse: Vector<Real>,
-    r2: Vector<Real>,
     inv_lhs: SdpMatrix<Real>,
+
+    motor_rhs: AngVector<Real>,
+    motor_impulse: AngVector<Real>,
+    motor_inv_lhs: Option<AngularInertia<Real>>,
+    motor_max_impulse: Real,
+
     im2: Real,
     ii2_sqrt: AngularInertia<Real>,
 }
@@ -155,18 +268,18 @@ impl BallVelocityGroundConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &BallJoint,
+        joint: &BallJoint,
         flipped: bool,
     ) -> Self {
         let (anchor1, anchor2) = if flipped {
             (
-                rb1.position * cparams.local_anchor2 - rb1.world_com,
-                rb2.position * cparams.local_anchor1 - rb2.world_com,
+                rb1.position * joint.local_anchor2 - rb1.world_com,
+                rb2.position * joint.local_anchor1 - rb2.world_com,
             )
         } else {
             (
-                rb1.position * cparams.local_anchor1 - rb1.world_com,
-                rb2.position * cparams.local_anchor2 - rb2.world_com,
+                rb1.position * joint.local_anchor1 - rb1.world_com,
+                rb2.position * joint.local_anchor2 - rb2.world_com,
             )
         };
 
@@ -199,14 +312,80 @@ impl BallVelocityGroundConstraint {
 
         let inv_lhs = lhs.inverse_unchecked();
 
+        /*
+         * Motor part.
+         */
+        let mut motor_rhs = na::zero();
+        let mut motor_inv_lhs = None;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        if motor_max_impulse > 0.0 {
+            let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+                params.dt,
+                joint.motor_stiffness,
+                joint.motor_damping,
+            );
+
+            if stiffness != 0.0 {
+                let dpos = rb2.position.rotation
+                    * (rb1.position.rotation * joint.motor_target_pos).inverse();
+                #[cfg(feature = "dim2")]
+                {
+                    motor_rhs += dpos.angle() * stiffness;
+                }
+                #[cfg(feature = "dim3")]
+                {
+                    motor_rhs += dpos.scaled_axis() * stiffness;
+                }
+            }
+
+            if damping != 0.0 {
+                let curr_vel = rb2.angvel - rb1.angvel;
+                motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+            }
+
+            #[cfg(feature = "dim2")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some(gamma / ii2)
+                } else {
+                    Some(gamma)
+                };
+                motor_rhs /= gamma;
+            }
+
+            #[cfg(feature = "dim3")]
+            if stiffness != 0.0 || damping != 0.0 {
+                motor_inv_lhs = if keep_lhs {
+                    let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
+                    Some(ii2.inverse_unchecked() * gamma)
+                } else {
+                    Some(SdpMatrix::diagonal(gamma))
+                };
+                motor_rhs /= gamma;
+            }
+        }
+
+        #[cfg(feature = "dim2")]
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+        #[cfg(feature = "dim3")]
+        let motor_impulse =
+            joint.motor_impulse.cap_magnitude(motor_max_impulse) * params.warmstart_coeff;
+
         BallVelocityGroundConstraint {
             joint_id,
             mj_lambda2: rb2.active_set_offset,
             im2,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             r2: anchor2,
             rhs,
             inv_lhs,
+            motor_rhs,
+            motor_impulse,
+            motor_inv_lhs,
+            motor_max_impulse: joint.motor_max_impulse,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
         }
     }
@@ -214,13 +393,13 @@ impl BallVelocityGroundConstraint {
     pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
         mj_lambda2.linear -= self.im2 * self.impulse;
-        mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(self.impulse));
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(self.r2.gcross(self.impulse) + self.motor_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];
-
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
         let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let vel2 = mj_lambda2.linear + angvel.gcross(self.r2);
         let dvel = vel2 + self.rhs;
@@ -230,6 +409,33 @@ impl BallVelocityGroundConstraint {
 
         mj_lambda2.linear -= self.im2 * impulse;
         mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(impulse));
+    }
+
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
+        if let Some(motor_inv_lhs) = &self.motor_inv_lhs {
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dangvel = ang_vel2 + self.motor_rhs;
+            let new_impulse = self.motor_impulse + motor_inv_lhs.transform_vector(dangvel);
+
+            #[cfg(feature = "dim2")]
+            let clamped_impulse =
+                na::clamp(new_impulse, -self.motor_max_impulse, self.motor_max_impulse);
+            #[cfg(feature = "dim3")]
+            let clamped_impulse = new_impulse.cap_magnitude(self.motor_max_impulse);
+
+            let effective_impulse = clamped_impulse - self.motor_impulse;
+            self.motor_impulse = clamped_impulse;
+
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(effective_impulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
@@ -238,7 +444,8 @@ impl BallVelocityGroundConstraint {
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::BallJoint(ball) = &mut joint.params {
-            ball.impulse = self.impulse
+            ball.impulse = self.impulse;
+            ball.motor_impulse = self.motor_impulse;
         }
     }
 }

src/dynamics/solver/joint_constraint/generic_position_constraint.rs

@@ -0,0 +1,346 @@
+use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
+use crate::dynamics::solver::DeltaVel;
+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, WCrossMatrix};
+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,
+}
+
+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];
+        let mut position2 = positions[self.position2];
+        let mut params = *params;
+        params.joint_erp = 0.8;
+
+        /*
+         *
+         * Translation part.
+         *
+         */
+        {
+            let anchor1 = position1 * self.joint.local_anchor1;
+            let anchor2 = position2 * self.joint.local_anchor2;
+            let basis = anchor1.rotation;
+            let r1 = Point::from(anchor1.translation.vector) - position1 * self.local_com1;
+            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
+            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
+
+            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.xyz(),
+                &self.joint.max_position.xyz(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let rmat1 = r1.gcross_matrix() * rotmat;
+            let rmat2 = r2.gcross_matrix() * rotmat;
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let delassus = (self.ii1.quadform(&rmat1).add_diagonal(self.im1)
+                + self.ii2.quadform(&rmat2).add_diagonal(self.im2))
+            .into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot1 = self.ii1.transform_vector(r1.gcross(impulse));
+            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
+
+            position1.translation.vector += self.im1 * impulse;
+            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
+            position2.translation.vector -= self.im2 * impulse;
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        /*
+         *
+         * Rotation part
+         *
+         */
+        {
+            let anchor1 = position1 * self.joint.local_anchor1;
+            let anchor2 = position2 * self.joint.local_anchor2;
+            let basis = anchor1.rotation;
+            let mut min_pos_impulse = self
+                .joint
+                .min_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+            let mut max_pos_impulse = self
+                .joint
+                .max_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+
+            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
+                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            // TODO: we should keep the SdpMatrix3 type.
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let delassus = (self.ii1.quadform(&rotmat) + self.ii2.quadform(&rotmat)).into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot1 = self.ii1.transform_vector(impulse);
+            let rot2 = self.ii2.transform_vector(impulse);
+
+            position1.rotation = position1.rotation.append_axisangle_linearized(&rot1);
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        positions[self.position1] = position1;
+        positions[self.position2] = 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];
+        let mut params = *params;
+        params.joint_erp = 0.8;
+
+        /*
+         *
+         * Translation part.
+         *
+         */
+        {
+            let anchor1 = self.anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+            let basis = anchor1.rotation;
+            let r2 = Point::from(anchor2.translation.vector) - position2 * self.local_com2;
+            let mut min_pos_impulse = self.joint.min_pos_impulse.xyz();
+            let mut max_pos_impulse = self.joint.max_pos_impulse.xyz();
+
+            let pos_rhs = GenericVelocityConstraint::compute_lin_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.xyz(),
+                &self.joint.max_position.xyz(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let rmat2 = r2.gcross_matrix() * rotmat;
+
+            // TODO: we should keep the SdpMatrix3 type.
+            let delassus = self
+                .ii2
+                .quadform(&rmat2)
+                .add_diagonal(self.im2)
+                .into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+
+            let rot2 = self.ii2.transform_vector(r2.gcross(impulse));
+
+            position2.translation.vector -= self.im2 * impulse;
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        /*
+         *
+         * Rotation part
+         *
+         */
+        {
+            let anchor1 = self.anchor1;
+            let anchor2 = position2 * self.local_anchor2;
+            let basis = anchor1.rotation;
+            let mut min_pos_impulse = self
+                .joint
+                .min_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+            let mut max_pos_impulse = self
+                .joint
+                .max_pos_impulse
+                .fixed_rows::<Dim>(DIM)
+                .into_owned();
+
+            let pos_rhs = GenericVelocityConstraint::compute_ang_position_error(
+                &anchor1,
+                &anchor2,
+                &basis,
+                &self.joint.min_position.fixed_rows::<Dim>(DIM).into_owned(),
+                &self.joint.max_position.fixed_rows::<Dim>(DIM).into_owned(),
+            ) * params.joint_erp;
+
+            for i in 0..3 {
+                if pos_rhs[i] < 0.0 {
+                    min_pos_impulse[i] = -Real::MAX;
+                }
+                if pos_rhs[i] > 0.0 {
+                    max_pos_impulse[i] = Real::MAX;
+                }
+            }
+
+            // Will be actually inverted right after.
+            // TODO: we should keep the SdpMatrix3 type.
+            let rotmat = basis.to_rotation_matrix().into_inner();
+            let delassus = self.ii2.quadform(&rotmat).into_matrix();
+
+            let inv_delassus = GenericVelocityConstraint::invert_partial_delassus_matrix(
+                &min_pos_impulse,
+                &max_pos_impulse,
+                &mut Vector3::zeros(),
+                delassus,
+            );
+
+            let local_impulse = (inv_delassus * pos_rhs)
+                .inf(&max_pos_impulse)
+                .sup(&min_pos_impulse);
+            let impulse = basis * local_impulse;
+            let rot2 = self.ii2.transform_vector(impulse);
+
+            position2.rotation = position2.rotation.append_axisangle_linearized(&-rot2);
+        }
+
+        positions[self.position2] = 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,706 @@
+use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::{
+    GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
+};
+use crate::math::{AngularInertia, Dim, Isometry, Real, Rotation, SpacialVector, Vector, DIM};
+use crate::na::UnitQuaternion;
+use crate::parry::math::{AngDim, SpatialVector};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+#[cfg(feature = "dim3")]
+use na::{Matrix3, Matrix6, Vector3, Vector6, U3};
+#[cfg(feature = "dim2")]
+use na::{Matrix3, Vector3};
+
+#[derive(Debug)]
+pub(crate) struct GenericVelocityConstraint {
+    mj_lambda1: usize,
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    inv_lhs_lin: Matrix3<Real>,
+    inv_lhs_ang: Matrix3<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>,
+    basis1: Rotation<Real>,
+    basis2: Rotation<Real>,
+    dependant_set_mask: SpacialVector<Real>,
+
+    vel: GenericConstraintPart,
+}
+
+impl GenericVelocityConstraint {
+    pub fn compute_velocity_error(
+        min_velocity: &SpatialVector<Real>,
+        max_velocity: &SpatialVector<Real>,
+        r1: &Vector<Real>,
+        r2: &Vector<Real>,
+        basis1: &Rotation<Real>,
+        basis2: &Rotation<Real>,
+        rb1: &RigidBody,
+        rb2: &RigidBody,
+    ) -> SpatialVector<Real> {
+        let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+            + basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
+        let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+            + basis2.inverse_transform_vector(&rb2.angvel);
+
+        let min_linvel = min_velocity.xyz();
+        let min_angvel = min_velocity.fixed_rows::<AngDim>(DIM).into_owned();
+        let max_linvel = max_velocity.xyz();
+        let max_angvel = max_velocity.fixed_rows::<AngDim>(DIM).into_owned();
+        let lin_rhs = lin_dvel - lin_dvel.sup(&min_linvel).inf(&max_linvel);
+        let ang_rhs = ang_dvel - ang_dvel.sup(&min_angvel).inf(&max_angvel);
+
+        #[cfg(feature = "dim2")]
+        return Vector3::new(lin_rhs.x, lin_rhs.y, ang_rhs);
+
+        #[cfg(feature = "dim3")]
+        return Vector6::new(
+            lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y, ang_rhs.z,
+        );
+    }
+
+    pub fn compute_lin_position_error(
+        anchor1: &Isometry<Real>,
+        anchor2: &Isometry<Real>,
+        basis: &Rotation<Real>,
+        min_position: &Vector<Real>,
+        max_position: &Vector<Real>,
+    ) -> Vector<Real> {
+        let dpos = anchor2.translation.vector - anchor1.translation.vector;
+        let local_dpos = basis.inverse_transform_vector(&dpos);
+        local_dpos - local_dpos.sup(min_position).inf(max_position)
+    }
+
+    pub fn compute_ang_position_error(
+        anchor1: &Isometry<Real>,
+        anchor2: &Isometry<Real>,
+        basis: &Rotation<Real>,
+        min_position: &Vector<Real>,
+        max_position: &Vector<Real>,
+    ) -> Vector<Real> {
+        let drot = anchor2.rotation * anchor1.rotation.inverse();
+        let local_drot_diff = basis.inverse_transform_vector(&drot.scaled_axis());
+
+        let error = local_drot_diff - local_drot_diff.sup(min_position).inf(max_position);
+        let error_code =
+            (error[0] == 0.0) as usize + (error[1] == 0.0) as usize + (error[2] == 0.0) as usize;
+
+        if error_code == 1 {
+            // Align two axes.
+            let constrained_axis = error.iamin();
+            let axis1 = anchor1
+                .rotation
+                .to_rotation_matrix()
+                .into_inner()
+                .column(constrained_axis)
+                .into_owned();
+            let axis2 = anchor2
+                .rotation
+                .to_rotation_matrix()
+                .into_inner()
+                .column(constrained_axis)
+                .into_owned();
+            let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
+                .unwrap_or(UnitQuaternion::identity());
+            let local_drot_diff = basis.inverse_transform_vector(&rot_cross.scaled_axis());
+            local_drot_diff - local_drot_diff.sup(min_position).inf(max_position)
+        } else {
+            error
+        }
+    }
+
+    pub fn invert_partial_delassus_matrix(
+        min_impulse: &Vector<Real>,
+        max_impulse: &Vector<Real>,
+        dependant_set_mask: &mut Vector<Real>,
+        mut delassus: na::Matrix3<Real>,
+    ) -> na::Matrix3<Real> {
+        // Adjust the Delassus 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.
+        for i in 0..3 {
+            if min_impulse[i] > -Real::MAX || max_impulse[i] < Real::MAX {
+                let diag = delassus[(i, i)];
+                delassus.column_mut(i).fill(0.0);
+                delassus.row_mut(i).fill(0.0);
+                delassus[(i, i)] = diag;
+                dependant_set_mask[i] = 0.0;
+            } else {
+                dependant_set_mask[i] = 1.0;
+            }
+        }
+
+        delassus.try_inverse().unwrap()
+    }
+
+    pub fn compute_position_error(
+        joint: &GenericJoint,
+        anchor1: &Isometry<Real>,
+        anchor2: &Isometry<Real>,
+        basis: &Rotation<Real>,
+    ) -> SpatialVector<Real> {
+        let delta_pos = Isometry::from_parts(
+            anchor2.translation * anchor1.translation.inverse(),
+            anchor2.rotation * anchor1.rotation.inverse(),
+        );
+        let lin_dpos = basis.inverse_transform_vector(&delta_pos.translation.vector);
+        let ang_dpos = basis.inverse_transform_vector(&delta_pos.rotation.scaled_axis());
+
+        let dpos = Vector6::new(
+            lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
+        );
+
+        let error = dpos - dpos.sup(&joint.min_position).inf(&joint.max_position);
+        let error_code =
+            (error[3] == 0.0) as usize + (error[4] == 0.0) as usize + (error[5] == 0.0) as usize;
+
+        match error_code {
+            1 => {
+                let constrained_axis = error.rows(3, 3).iamin();
+                let axis1 = anchor1
+                    .rotation
+                    .to_rotation_matrix()
+                    .into_inner()
+                    .column(constrained_axis)
+                    .into_owned();
+                let axis2 = anchor2
+                    .rotation
+                    .to_rotation_matrix()
+                    .into_inner()
+                    .column(constrained_axis)
+                    .into_owned();
+                let rot_cross = UnitQuaternion::rotation_between(&axis1, &axis2)
+                    .unwrap_or(UnitQuaternion::identity());
+                let ang_dpos = basis.inverse_transform_vector(&rot_cross.scaled_axis());
+                let dpos = Vector6::new(
+                    lin_dpos.x, lin_dpos.y, lin_dpos.z, ang_dpos.x, ang_dpos.y, ang_dpos.z,
+                );
+
+                dpos - dpos.sup(&joint.min_position).inf(&joint.max_position)
+            }
+            _ => error,
+        }
+    }
+
+    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 basis1 = anchor1.rotation;
+        let basis2 = anchor2.rotation;
+        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 mut min_impulse = joint.min_impulse;
+        let mut max_impulse = joint.max_impulse;
+        let mut min_pos_impulse = joint.min_pos_impulse;
+        let mut max_pos_impulse = joint.max_pos_impulse;
+        let mut min_velocity = joint.min_velocity;
+        let mut max_velocity = joint.max_velocity;
+        let mut dependant_set_mask = SpacialVector::repeat(1.0);
+
+        let pos_rhs = Self::compute_position_error(joint, &anchor1, &anchor2, &basis1)
+            * params.inv_dt()
+            * params.joint_erp;
+
+        for i in 0..6 {
+            if pos_rhs[i] < 0.0 {
+                min_impulse[i] = -Real::MAX;
+                min_pos_impulse[i] = -Real::MAX;
+                min_velocity[i] = 0.0;
+            }
+            if pos_rhs[i] > 0.0 {
+                max_impulse[i] = Real::MAX;
+                max_pos_impulse[i] = Real::MAX;
+                max_velocity[i] = 0.0;
+            }
+        }
+
+        let rhs = Self::compute_velocity_error(
+            &min_velocity,
+            &max_velocity,
+            &r1,
+            &r2,
+            &basis1,
+            &basis2,
+            rb1,
+            rb2,
+        );
+        let rhs_lin = rhs.xyz();
+        let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into();
+
+        // TODO: we should keep the SdpMatrix3 type.
+        let rotmat1 = basis1.to_rotation_matrix().into_inner();
+        let rotmat2 = basis2.to_rotation_matrix().into_inner();
+        let rmat1 = r1.gcross_matrix() * rotmat1;
+        let rmat2 = r2.gcross_matrix() * rotmat2;
+        let delassus00 = (ii1.quadform(&rmat1).add_diagonal(im1)
+            + ii2.quadform(&rmat2).add_diagonal(im2))
+        .into_matrix();
+        let delassus11 = (ii1.quadform(&rotmat1) + ii2.quadform(&rotmat2)).into_matrix();
+
+        let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.xyz(),
+            &max_pos_impulse.xyz(),
+            &mut Vector3::zeros(),
+            delassus00,
+        );
+        let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &mut Vector3::zeros(),
+            delassus11,
+        );
+
+        let impulse = (joint.impulse * params.warmstart_coeff)
+            .inf(&max_impulse)
+            .sup(&min_impulse);
+
+        let lin_impulse = impulse.xyz();
+        let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let min_lin_impulse = min_impulse.xyz();
+        let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let max_lin_impulse = max_impulse.xyz();
+        let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
+
+        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,
+            inv_lhs_lin,
+            inv_lhs_ang,
+            r1,
+            r2,
+            basis1,
+            basis2,
+            dependant_set_mask,
+            vel: GenericConstraintPart {
+                lin_impulse,
+                ang_impulse,
+                min_lin_impulse,
+                min_ang_impulse,
+                max_lin_impulse,
+                max_ang_impulse,
+                rhs_lin,
+                rhs_ang,
+            },
+        }
+    }
+
+    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_impulse1 = self.basis1 * self.vel.lin_impulse;
+        let ang_impulse1 = self.basis1 * self.vel.ang_impulse;
+        let lin_impulse2 = self.basis2 * self.vel.lin_impulse;
+        let ang_impulse2 = self.basis2 * self.vel.ang_impulse;
+
+        mj_lambda1.linear += self.im1 * lin_impulse1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+
+        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 (lin_imp, ang_imp) = self.vel.solve(self, &mut mj_lambda1, &mut mj_lambda2);
+        self.vel.lin_impulse = lin_imp;
+        self.vel.ang_impulse = ang_imp;
+
+        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;
+        match &mut joint.params {
+            JointParams::GenericJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.impulse[3] = self.vel.ang_impulse.x;
+                out.impulse[4] = self.vel.ang_impulse.y;
+                out.impulse[5] = self.vel.ang_impulse.z;
+            }
+            JointParams::RevoluteJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.motor_impulse = self.vel.ang_impulse.x;
+                out.impulse[3] = self.vel.ang_impulse.y;
+                out.impulse[4] = self.vel.ang_impulse.z;
+            }
+            _ => unimplemented!(),
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct GenericVelocityGroundConstraint {
+    mj_lambda2: usize,
+
+    joint_id: JointIndex,
+
+    inv_lhs_lin: Matrix3<Real>,
+    inv_lhs_ang: Matrix3<Real>,
+
+    im2: Real,
+    ii2: AngularInertia<Real>,
+    ii2_sqrt: AngularInertia<Real>,
+    r2: Vector<Real>,
+    basis: Rotation<Real>,
+
+    dependant_set_mask: SpacialVector<Real>,
+
+    vel: GenericConstraintPart,
+}
+
+impl GenericVelocityGroundConstraint {
+    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 basis = anchor2.rotation;
+        let im2 = rb2.effective_inv_mass;
+        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 mut min_impulse = joint.min_impulse;
+        let mut max_impulse = joint.max_impulse;
+        let mut min_pos_impulse = joint.min_pos_impulse;
+        let mut max_pos_impulse = joint.max_pos_impulse;
+        let mut min_velocity = joint.min_velocity;
+        let mut max_velocity = joint.max_velocity;
+        let mut dependant_set_mask = SpacialVector::repeat(1.0);
+
+        let pos_rhs =
+            GenericVelocityConstraint::compute_position_error(joint, &anchor1, &anchor2, &basis)
+                * params.inv_dt()
+                * params.joint_erp;
+
+        for i in 0..6 {
+            if pos_rhs[i] < 0.0 {
+                min_impulse[i] = -Real::MAX;
+                min_pos_impulse[i] = -Real::MAX;
+                min_velocity[i] = 0.0;
+            }
+            if pos_rhs[i] > 0.0 {
+                max_impulse[i] = Real::MAX;
+                max_pos_impulse[i] = Real::MAX;
+                max_velocity[i] = 0.0;
+            }
+        }
+
+        let rhs = GenericVelocityConstraint::compute_velocity_error(
+            &min_velocity,
+            &max_velocity,
+            &r1,
+            &r2,
+            &basis,
+            &basis,
+            rb1,
+            rb2,
+        );
+        let rhs_lin = rhs.xyz();
+        let rhs_ang = rhs.fixed_rows::<Dim>(DIM).into_owned();
+
+        // TODO: we should keep the SdpMatrix3 type.
+        let rotmat = basis.to_rotation_matrix().into_inner();
+        let rmat2 = r2.gcross_matrix() * rotmat;
+        let delassus00 = ii2.quadform(&rmat2).add_diagonal(im2).into_matrix();
+        let delassus11 = ii2.quadform(&rotmat).into_matrix();
+
+        let inv_lhs_lin = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.xyz(),
+            &max_pos_impulse.xyz(),
+            &mut Vector3::zeros(),
+            delassus00,
+        );
+        let inv_lhs_ang = GenericVelocityConstraint::invert_partial_delassus_matrix(
+            &min_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &max_pos_impulse.fixed_rows::<Dim>(DIM).into_owned(),
+            &mut Vector3::zeros(),
+            delassus11,
+        );
+
+        let impulse = (joint.impulse * params.warmstart_coeff)
+            .inf(&max_impulse)
+            .sup(&min_impulse);
+
+        let lin_impulse = impulse.xyz();
+        let ang_impulse = impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let min_lin_impulse = min_impulse.xyz();
+        let min_ang_impulse = min_impulse.fixed_rows::<Dim>(DIM).into_owned();
+        let max_lin_impulse = max_impulse.xyz();
+        let max_ang_impulse = max_impulse.fixed_rows::<Dim>(DIM).into_owned();
+
+        GenericVelocityGroundConstraint {
+            joint_id,
+            mj_lambda2: rb2.active_set_offset,
+            im2,
+            ii2,
+            ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
+            inv_lhs_lin,
+            inv_lhs_ang,
+            r2,
+            basis,
+            vel: GenericConstraintPart {
+                lin_impulse,
+                ang_impulse,
+                min_lin_impulse,
+                min_ang_impulse,
+                max_lin_impulse,
+                max_ang_impulse,
+                rhs_lin,
+                rhs_ang,
+            },
+            dependant_set_mask,
+        }
+    }
+
+    pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        let lin_impulse = self.basis * self.vel.lin_impulse;
+        #[cfg(feature = "dim2")]
+        let ang_impulse = self.basis * self.vel.impulse[2];
+        #[cfg(feature = "dim3")]
+        let ang_impulse = self.basis * self.vel.ang_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_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 (lin_imp, ang_imp) = self.vel.solve_ground(self, &mut mj_lambda2);
+        self.vel.lin_impulse = lin_imp;
+        self.vel.ang_impulse = ang_imp;
+
+        mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
+    }
+
+    // TODO: 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;
+        match &mut joint.params {
+            JointParams::GenericJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.impulse[3] = self.vel.ang_impulse.x;
+                out.impulse[4] = self.vel.ang_impulse.y;
+                out.impulse[5] = self.vel.ang_impulse.z;
+            }
+            JointParams::RevoluteJoint(out) => {
+                out.impulse[0] = self.vel.lin_impulse.x;
+                out.impulse[1] = self.vel.lin_impulse.y;
+                out.impulse[2] = self.vel.lin_impulse.z;
+                out.motor_impulse = self.vel.ang_impulse.x;
+                out.impulse[3] = self.vel.ang_impulse.y;
+                out.impulse[4] = self.vel.ang_impulse.z;
+            }
+            _ => unimplemented!(),
+        }
+    }
+}
+
+#[derive(Debug)]
+struct GenericConstraintPart {
+    lin_impulse: Vector3<Real>,
+    max_lin_impulse: Vector3<Real>,
+    min_lin_impulse: Vector3<Real>,
+    rhs_lin: Vector3<Real>,
+
+    ang_impulse: Vector3<Real>,
+    max_ang_impulse: Vector3<Real>,
+    min_ang_impulse: Vector3<Real>,
+    rhs_ang: Vector3<Real>,
+}
+
+impl GenericConstraintPart {
+    fn solve(
+        &self,
+        parent: &GenericVelocityConstraint,
+        mj_lambda1: &mut DeltaVel<Real>,
+        mj_lambda2: &mut DeltaVel<Real>,
+    ) -> (Vector3<Real>, Vector3<Real>) {
+        let new_lin_impulse;
+        let new_ang_impulse;
+
+        /*
+         *
+         * Solve translations.
+         *
+         */
+        {
+            let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent
+                .basis1
+                .inverse_transform_vector(&(-mj_lambda1.linear - ang_vel1.gcross(parent.r1)))
+                + parent
+                    .basis2
+                    .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
+
+            let err = dvel + self.rhs_lin;
+
+            new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
+                .sup(&self.min_lin_impulse)
+                .inf(&self.max_lin_impulse);
+            let effective_impulse1 = parent.basis1 * (new_lin_impulse - self.lin_impulse);
+            let effective_impulse2 = parent.basis2 * (new_lin_impulse - self.lin_impulse);
+
+            mj_lambda1.linear += parent.im1 * effective_impulse1;
+            mj_lambda1.angular += parent
+                .ii1_sqrt
+                .transform_vector(parent.r1.gcross(effective_impulse1));
+
+            mj_lambda2.linear -= parent.im2 * effective_impulse2;
+            mj_lambda2.angular -= parent
+                .ii2_sqrt
+                .transform_vector(parent.r2.gcross(effective_impulse2));
+        }
+
+        /*
+         *
+         * Solve rotations.
+         *
+         */
+        {
+            let ang_vel1 = parent.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent.basis2.inverse_transform_vector(&ang_vel2)
+                - parent.basis1.inverse_transform_vector(&ang_vel1);
+            let err = dvel + self.rhs_ang;
+
+            new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
+                .sup(&self.min_ang_impulse)
+                .inf(&self.max_ang_impulse);
+            let effective_impulse1 = parent.basis1 * (new_ang_impulse - self.ang_impulse);
+            let effective_impulse2 = parent.basis2 * (new_ang_impulse - self.ang_impulse);
+
+            mj_lambda1.angular += parent.ii1_sqrt.transform_vector(effective_impulse1);
+            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse2);
+        }
+
+        (new_lin_impulse, new_ang_impulse)
+    }
+
+    fn solve_ground(
+        &self,
+        parent: &GenericVelocityGroundConstraint,
+        mj_lambda2: &mut DeltaVel<Real>,
+    ) -> (Vector3<Real>, Vector3<Real>) {
+        let new_lin_impulse;
+        let new_ang_impulse;
+
+        /*
+         *
+         * Solve translations.
+         *
+         */
+        {
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent
+                .basis
+                .inverse_transform_vector(&(mj_lambda2.linear + ang_vel2.gcross(parent.r2)));
+
+            let err = dvel + self.rhs_lin;
+
+            new_lin_impulse = (self.lin_impulse + parent.inv_lhs_lin * err)
+                .sup(&self.min_lin_impulse)
+                .inf(&self.max_lin_impulse);
+            let effective_impulse = parent.basis * (new_lin_impulse - self.lin_impulse);
+
+            mj_lambda2.linear -= parent.im2 * effective_impulse;
+            mj_lambda2.angular -= parent
+                .ii2_sqrt
+                .transform_vector(parent.r2.gcross(effective_impulse));
+        }
+
+        /*
+         *
+         * Solve rotations.
+         *
+         */
+        {
+            let ang_vel2 = parent.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+            let dvel = parent.basis.inverse_transform_vector(&ang_vel2);
+            let err = dvel + self.rhs_ang;
+
+            new_ang_impulse = (self.ang_impulse + parent.inv_lhs_ang * err)
+                .sup(&self.min_ang_impulse)
+                .inf(&self.max_ang_impulse);
+            let effective_impulse = parent.basis * (new_ang_impulse - self.ang_impulse);
+
+            mj_lambda2.angular -= parent.ii2_sqrt.transform_vector(effective_impulse);
+        }
+
+        (new_lin_impulse, new_ang_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

@@ -13,6 +13,9 @@ use super::{
 #[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 +37,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 +88,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 +121,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 +166,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(
@@ -156,10 +179,8 @@ impl AnyJointVelocityConstraint {
                 )
             }
             #[cfg(feature = "dim3")]
-            JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint(
-                RevoluteVelocityGroundConstraint::from_params(
-                    params, joint_id, rb1, rb2, p, flipped,
-                ),
+            JointParams::RevoluteJoint(p) => RevoluteVelocityGroundConstraint::from_params(
+                params, joint_id, rb1, rb2, p, flipped,
             ),
         }
     }
@@ -199,6 +220,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 +264,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 +304,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 +352,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

@@ -31,6 +31,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 +67,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 +98,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 +138,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 +179,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 +217,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/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

@@ -3,7 +3,7 @@ use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
 };
 use crate::math::{AngularInertia, Real, Vector};
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
@@ -41,9 +41,17 @@ pub(crate) struct PrismaticVelocityConstraint {
     #[cfg(feature = "dim2")]
     impulse: Vector2<Real>,
 
+    motor_axis1: Vector<Real>,
+    motor_axis2: Vector<Real>,
+    motor_impulse: Real,
+    motor_rhs: Real,
+    motor_inv_lhs: Real,
+    motor_max_impulse: Real,
+
     limits_impulse: Real,
     limits_forcedirs: Option<(Vector<Real>, Vector<Real>)>,
     limits_rhs: Real,
+    limits_inv_lhs: Real,
 
     #[cfg(feature = "dim2")]
     basis1: Vector2<Real>,
@@ -63,35 +71,21 @@ impl PrismaticVelocityConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &PrismaticJoint,
+        joint: &PrismaticJoint,
     ) -> Self {
         // Linear part.
-        let anchor1 = rb1.position * cparams.local_anchor1;
-        let anchor2 = rb2.position * cparams.local_anchor2;
-        let axis1 = rb1.position * cparams.local_axis1;
-        let axis2 = rb2.position * cparams.local_axis2;
+        let anchor1 = rb1.position * joint.local_anchor1;
+        let anchor2 = rb2.position * joint.local_anchor2;
+        let axis1 = rb1.position * joint.local_axis1;
+        let axis2 = rb2.position * joint.local_axis2;
         #[cfg(feature = "dim2")]
-        let basis1 = rb1.position * cparams.basis1[0];
+        let basis1 = rb1.position * joint.basis1[0];
         #[cfg(feature = "dim3")]
         let basis1 = Matrix3x2::from_columns(&[
-            rb1.position * cparams.basis1[0],
-            rb1.position * cparams.basis1[1],
+            rb1.position * joint.basis1[0],
+            rb1.position * joint.basis1[1],
         ]);
 
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
-
         let im1 = rb1.effective_inv_mass;
         let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
@@ -149,25 +143,71 @@ impl PrismaticVelocityConstraint {
         #[cfg(feature = "dim3")]
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
 
-        // Setup limit constraint.
+        /*
+         * Setup motor.
+         */
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
+            motor_rhs += (dist - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs { gamma / (im1 + im2) } else { gamma };
+            motor_rhs /= gamma;
+        }
+
+        let motor_impulse = na::clamp(
+            joint.motor_impulse,
+            -joint.motor_max_impulse,
+            joint.motor_max_impulse,
+        );
+
+        /*
+         * Setup limit constraint.
+         */
         let mut limits_forcedirs = None;
         let mut limits_rhs = 0.0;
         let mut limits_impulse = 0.0;
+        let mut limits_inv_lhs = 0.0;
 
-        if cparams.limits_enabled {
+        if joint.limits_enabled {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // FIXME: we should allow both limits to be active at
+            // TODO: we should allow both limits to be active at
             // the same time, and allow predictive constraint activation.
-            if dist < cparams.limits[0] {
+            if dist < joint.limits[0] {
                 limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner()));
                 limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
-            } else if dist > cparams.limits[1] {
+                limits_impulse = joint.limits_impulse;
+            } else if dist > joint.limits[1] {
                 limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));
                 limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
+                limits_impulse = joint.limits_impulse;
+            }
+
+            if dist < joint.limits[0] || dist > joint.limits[1] {
+                let gcross1 = r1.gcross(*axis1);
+                let gcross2 = r2.gcross(*axis2);
+                limits_inv_lhs = crate::utils::inv(
+                    im1 + im2
+                        + gcross1.gdot(ii1.transform_vector(gcross1))
+                        + gcross2.gdot(ii2.transform_vector(gcross2)),
+                );
             }
         }
 
@@ -179,10 +219,17 @@ impl PrismaticVelocityConstraint {
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             limits_impulse: limits_impulse * params.warmstart_coeff,
             limits_forcedirs,
             limits_rhs,
+            limits_inv_lhs,
+            motor_rhs,
+            motor_inv_lhs,
+            motor_impulse,
+            motor_axis1: *axis1,
+            motor_axis2: *axis2,
+            motor_max_impulse: joint.motor_max_impulse,
             basis1,
             inv_lhs,
             rhs,
@@ -211,22 +258,30 @@ impl PrismaticVelocityConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        // Warmstart motors.
+        mj_lambda1.linear += self.motor_axis1 * (self.im1 * self.motor_impulse);
+        mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse);
+
+        // Warmstart limits.
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
+            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
+
+            mj_lambda1.linear += self.im1 * limit_impulse1;
+            mj_lambda1.angular += self
+                .ii1_sqrt
+                .transform_vector(self.r1.gcross(limit_impulse1));
+            mj_lambda2.linear += self.im2 * limit_impulse2;
+            mj_lambda2.angular += self
+                .ii2_sqrt
+                .transform_vector(self.r2.gcross(limit_impulse2));
         }
 
         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];
-
-        /*
-         * Joint consraint.
-         */
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -255,26 +310,55 @@ impl PrismaticVelocityConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
-         * Joint limits.
-         */
+    fn solve_limits(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
             let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
                 + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.limits_rhs;
-            let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0);
+            let new_impulse = (self.limits_impulse - lin_dvel * self.limits_inv_lhs).max(0.0);
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+            let lin_impulse1 = limits_forcedir1 * dimpulse;
+            let lin_impulse2 = limits_forcedir2 * dimpulse;
+
+            mj_lambda1.linear += self.im1 * lin_impulse1;
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
+            mj_lambda2.linear += self.im2 * lin_impulse2;
+            mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
         }
+    }
+
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear)
+                - self.motor_axis1.dot(&mj_lambda1.linear)
+                + self.motor_rhs;
+            let new_impulse = na::clamp(
+                self.motor_impulse + lin_dvel * self.motor_inv_lhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let dimpulse = new_impulse - self.motor_impulse;
+            self.motor_impulse = new_impulse;
+
+            mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse);
+            mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse);
+        }
+    }
+
+    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];
+
+        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -284,6 +368,7 @@ impl PrismaticVelocityConstraint {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            revolute.motor_impulse = self.motor_impulse;
             revolute.limits_impulse = self.limits_impulse;
         }
     }
@@ -315,6 +400,11 @@ pub(crate) struct PrismaticVelocityGroundConstraint {
     limits_rhs: Real,
 
     axis2: Vector<Real>,
+    motor_impulse: Real,
+    motor_rhs: Real,
+    motor_inv_lhs: Real,
+    motor_max_impulse: Real,
+
     #[cfg(feature = "dim2")]
     basis1: Vector2<Real>,
     #[cfg(feature = "dim3")]
@@ -331,7 +421,7 @@ impl PrismaticVelocityGroundConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &PrismaticJoint,
+        joint: &PrismaticJoint,
         flipped: bool,
     ) -> Self {
         let anchor2;
@@ -341,35 +431,35 @@ impl PrismaticVelocityGroundConstraint {
         let basis1;
 
         if flipped {
-            anchor2 = rb2.position * cparams.local_anchor1;
-            anchor1 = rb1.position * cparams.local_anchor2;
-            axis2 = rb2.position * cparams.local_axis1;
-            axis1 = rb1.position * cparams.local_axis2;
+            anchor2 = rb2.position * joint.local_anchor1;
+            anchor1 = rb1.position * joint.local_anchor2;
+            axis2 = rb2.position * joint.local_axis1;
+            axis1 = rb1.position * joint.local_axis2;
             #[cfg(feature = "dim2")]
             {
-                basis1 = rb1.position * cparams.basis2[0];
+                basis1 = rb1.position * joint.basis2[0];
             }
             #[cfg(feature = "dim3")]
             {
                 basis1 = Matrix3x2::from_columns(&[
-                    rb1.position * cparams.basis2[0],
-                    rb1.position * cparams.basis2[1],
+                    rb1.position * joint.basis2[0],
+                    rb1.position * joint.basis2[1],
                 ]);
             }
         } else {
-            anchor2 = rb2.position * cparams.local_anchor2;
-            anchor1 = rb1.position * cparams.local_anchor1;
-            axis2 = rb2.position * cparams.local_axis2;
-            axis1 = rb1.position * cparams.local_axis1;
+            anchor2 = rb2.position * joint.local_anchor2;
+            anchor1 = rb1.position * joint.local_anchor1;
+            axis2 = rb2.position * joint.local_axis2;
+            axis1 = rb1.position * joint.local_axis1;
             #[cfg(feature = "dim2")]
             {
-                basis1 = rb1.position * cparams.basis1[0];
+                basis1 = rb1.position * joint.basis1[0];
             }
             #[cfg(feature = "dim3")]
             {
                 basis1 = Matrix3x2::from_columns(&[
-                    rb1.position * cparams.basis1[0],
-                    rb1.position * cparams.basis1[1],
+                    rb1.position * joint.basis1[0],
+                    rb1.position * joint.basis1[1],
                 ]);
             }
         };
@@ -438,26 +528,61 @@ impl PrismaticVelocityGroundConstraint {
         #[cfg(feature = "dim3")]
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
 
-        // Setup limit constraint.
+        /*
+         * Setup motor.
+         */
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            let dist = anchor2.coords.dot(&axis2) - anchor1.coords.dot(&axis1);
+            motor_rhs += (dist - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs { gamma / im2 } else { gamma };
+            motor_rhs /= gamma;
+        }
+
+        let motor_impulse = na::clamp(
+            joint.motor_impulse,
+            -joint.motor_max_impulse,
+            joint.motor_max_impulse,
+        );
+
+        /*
+         * Setup limit constraint.
+         */
         let mut limits_forcedir2 = None;
         let mut limits_rhs = 0.0;
         let mut limits_impulse = 0.0;
 
-        if cparams.limits_enabled {
+        if joint.limits_enabled {
             let danchor = anchor2 - anchor1;
             let dist = danchor.dot(&axis1);
 
-            // FIXME: we should allow both limits to be active at
+            // TODO: we should allow both limits to be active at
             // the same time.
-            // FIXME: allow predictive constraint activation.
-            if dist < cparams.limits[0] {
+            // TODO: allow predictive constraint activation.
+            if dist < joint.limits[0] {
                 limits_forcedir2 = Some(axis2.into_inner());
                 limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
-            } else if dist > cparams.limits[1] {
+                limits_impulse = joint.limits_impulse;
+            } else if dist > joint.limits[1] {
                 limits_forcedir2 = Some(-axis2.into_inner());
                 limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1);
-                limits_impulse = cparams.limits_impulse;
+                limits_impulse = joint.limits_impulse;
             }
         }
 
@@ -466,8 +591,12 @@ impl PrismaticVelocityGroundConstraint {
             mj_lambda2: rb2.active_set_offset,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse: joint.impulse * params.warmstart_coeff,
             limits_impulse: limits_impulse * params.warmstart_coeff,
+            motor_rhs,
+            motor_inv_lhs,
+            motor_impulse,
+            motor_max_impulse: joint.motor_max_impulse,
             basis1,
             inv_lhs,
             rhs,
@@ -492,6 +621,10 @@ impl PrismaticVelocityGroundConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        // Warmstart motors.
+        mj_lambda2.linear -= self.axis2 * (self.im2 * self.motor_impulse);
+
+        // Warmstart limits.
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
         }
@@ -499,12 +632,7 @@ impl PrismaticVelocityGroundConstraint {
         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];
-
-        /*
-         * Joint consraint.
-         */
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let lin_dvel = self.basis1.tr_mul(&lin_vel2);
@@ -526,13 +654,10 @@ impl PrismaticVelocityGroundConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
-         * Joint limits.
-         */
+    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
             let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2)))
@@ -543,15 +668,39 @@ impl PrismaticVelocityGroundConstraint {
 
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
         }
+    }
+
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let lin_dvel = self.axis2.dot(&mj_lambda2.linear) + self.motor_rhs;
+            let new_impulse = na::clamp(
+                self.motor_impulse + lin_dvel * self.motor_inv_lhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let dimpulse = new_impulse - self.motor_impulse;
+            self.motor_impulse = new_impulse;
+
+            mj_lambda2.linear -= self.axis2 * (self.im2 * dimpulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_limits(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
+        self.solve_dofs(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
-    // FIXME: duplicated code with the non-ground constraint.
+    // TODO: duplicated code with the non-ground constraint.
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::PrismaticJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            revolute.motor_impulse = self.motor_impulse;
             revolute.limits_impulse = self.limits_impulse;
         }
     }

src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs

@@ -7,7 +7,7 @@ use crate::dynamics::{
 use crate::math::{
     AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
 };
-use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
 #[cfg(feature = "dim3")]
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 #[cfg(feature = "dim2")]
@@ -48,6 +48,7 @@ pub(crate) struct WPrismaticVelocityConstraint {
     limits_impulse: SimdReal,
     limits_forcedirs: Option<(Vector<SimdReal>, Vector<SimdReal>)>,
     limits_rhs: SimdReal,
+    limits_inv_lhs: SimdReal,
 
     #[cfg(feature = "dim2")]
     basis1: Vector2<SimdReal>,
@@ -187,10 +188,13 @@ impl WPrismaticVelocityConstraint {
         #[cfg(feature = "dim3")]
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z);
 
-        // Setup limit constraint.
+        /*
+         * Setup limit constraint.
+         */
         let mut limits_forcedirs = None;
         let mut limits_rhs = na::zero();
         let mut limits_impulse = na::zero();
+        let mut limits_inv_lhs = na::zero();
         let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
 
         if limits_enabled.any() {
@@ -210,9 +214,17 @@ impl WPrismaticVelocityConstraint {
             let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0));
 
             if sign != _0 {
+                let gcross1 = r1.gcross(axis1);
+                let gcross2 = r2.gcross(axis2);
+
                 limits_forcedirs = Some((axis1 * -sign, axis2 * sign));
                 limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign;
                 limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0);
+                limits_inv_lhs = SimdReal::splat(1.0)
+                    / (im1
+                        + im2
+                        + gcross1.gdot(ii1.transform_vector(gcross1))
+                        + gcross2.gdot(ii2.transform_vector(gcross2)));
             }
         }
 
@@ -228,6 +240,7 @@ impl WPrismaticVelocityConstraint {
             limits_impulse: limits_impulse * SimdReal::splat(params.warmstart_coeff),
             limits_forcedirs,
             limits_rhs,
+            limits_inv_lhs,
             basis1,
             inv_lhs,
             rhs,
@@ -270,9 +283,19 @@ impl WPrismaticVelocityConstraint {
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        // Warmstart limits.
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse);
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse);
+            let limit_impulse1 = limits_forcedir1 * self.limits_impulse;
+            let limit_impulse2 = limits_forcedir2 * self.limits_impulse;
+
+            mj_lambda1.linear += limit_impulse1 * self.im1;
+            mj_lambda1.angular += self
+                .ii1_sqrt
+                .transform_vector(self.r1.gcross(limit_impulse1));
+            mj_lambda2.linear += limit_impulse2 * self.im2;
+            mj_lambda2.angular += self
+                .ii2_sqrt
+                .transform_vector(self.r2.gcross(limit_impulse2));
         }
 
         for ii in 0..SIMD_WIDTH {
@@ -285,27 +308,11 @@ impl WPrismaticVelocityConstraint {
         }
     }
 
-    pub fn solve(&mut 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],
-            ),
-        };
-
-        /*
-         * Joint consraint.
-         */
+    fn solve_dofs(
+        &mut self,
+        mj_lambda1: &mut DeltaVel<SimdReal>,
+        mj_lambda2: &mut DeltaVel<SimdReal>,
+    ) {
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1);
@@ -334,13 +341,14 @@ impl WPrismaticVelocityConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
-         * Joint limits.
-         */
+    fn solve_limits(
+        &mut self,
+        mj_lambda1: &mut DeltaVel<SimdReal>,
+        mj_lambda2: &mut DeltaVel<SimdReal>,
+    ) {
         if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs {
-            // FIXME: the transformation by ii2_sqrt could be avoided by
-            // reusing some computations above.
             let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
             let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
 
@@ -348,13 +356,40 @@ impl WPrismaticVelocityConstraint {
                 + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1)))
                 + self.limits_rhs;
             let new_impulse =
-                (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero());
+                (self.limits_impulse - lin_dvel * self.limits_inv_lhs).simd_max(na::zero());
             let dimpulse = new_impulse - self.limits_impulse;
             self.limits_impulse = new_impulse;
 
-            mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse);
-            mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
+            let lin_impulse1 = limits_forcedir1 * dimpulse;
+            let lin_impulse2 = limits_forcedir2 * dimpulse;
+
+            mj_lambda1.linear += lin_impulse1 * self.im1;
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1));
+            mj_lambda2.linear += lin_impulse2 * self.im2;
+            mj_lambda2.angular += self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2));
         }
+    }
+
+    pub fn solve(&mut 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],
+            ),
+        };
+
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_limits(&mut mj_lambda1, &mut mj_lambda2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -477,19 +512,6 @@ impl WPrismaticVelocityGroundConstraint {
         let axis1 = position1 * local_axis1;
         let axis2 = position2 * local_axis2;
 
-        // #[cfg(feature = "dim2")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // #[cfg(feature = "dim3")]
-        // let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //     .unwrap_or_else(Rotation::identity)
-        //     .to_rotation_matrix()
-        //     .into_inner();
-        // let basis2 = r21 * basis1;
-        // NOTE: we use basis2 := basis1 for now is that allows
-        // simplifications of the computation without introducing
-        // much instabilities.
         let ii2 = ii2_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r2 = anchor2 - world_com2;
@@ -616,19 +638,7 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    pub fn solve(&mut 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],
-            ),
-        };
-
-        /*
-         * Joint consraint.
-         */
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2);
         let lin_dvel = self.basis1.tr_mul(&lin_vel2);
@@ -650,10 +660,9 @@ impl WPrismaticVelocityGroundConstraint {
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
 
-        /*
-         * Joint limits.
-         */
+    fn solve_limits(&mut self, mj_lambda2: &mut DeltaVel<SimdReal>) {
         if let Some(limits_forcedir2) = self.limits_forcedir2 {
             // FIXME: the transformation by ii2_sqrt could be avoided by
             // reusing some computations above.
@@ -667,6 +676,20 @@ impl WPrismaticVelocityGroundConstraint {
 
             mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse);
         }
+    }
+
+    pub fn solve(&mut 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],
+            ),
+        };
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_limits(&mut mj_lambda2);
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii);
@@ -674,7 +697,6 @@ impl WPrismaticVelocityGroundConstraint {
         }
     }
 
-    // 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;

src/dynamics/solver/joint_constraint/revolute_position_constraint.rs

@@ -1,6 +1,6 @@
 use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
 use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
-use crate::utils::WAngularInertia;
+use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::Unit;
 
 #[derive(Debug)]
@@ -8,13 +8,15 @@ pub(crate) struct RevolutePositionConstraint {
     position1: usize,
     position2: usize,
 
+    local_com1: Point<Real>,
+    local_com2: Point<Real>,
+
     im1: Real,
     im2: Real,
 
     ii1: AngularInertia<Real>,
     ii2: AngularInertia<Real>,
 
-    lin_inv_lhs: Real,
     ang_inv_lhs: AngularInertia<Real>,
 
     local_anchor1: Point<Real>,
@@ -22,6 +24,8 @@ pub(crate) struct RevolutePositionConstraint {
 
     local_axis1: Unit<Vector<Real>>,
     local_axis2: Unit<Vector<Real>>,
+    local_basis1: [Vector<Real>; 2],
+    local_basis2: [Vector<Real>; 2],
 }
 
 impl RevolutePositionConstraint {
@@ -30,7 +34,6 @@ impl RevolutePositionConstraint {
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
-        let lin_inv_lhs = 1.0 / (im1 + im2);
         let ang_inv_lhs = (ii1 + ii2).inverse();
 
         Self {
@@ -38,14 +41,17 @@ impl RevolutePositionConstraint {
             im2,
             ii1,
             ii2,
-            lin_inv_lhs,
             ang_inv_lhs,
+            local_com1: rb1.mass_properties.local_com,
+            local_com2: rb2.mass_properties.local_com,
             local_anchor1: cparams.local_anchor1,
             local_anchor2: cparams.local_anchor2,
             local_axis1: cparams.local_axis1,
             local_axis2: cparams.local_axis2,
             position1: rb1.active_set_offset,
             position2: rb2.active_set_offset,
+            local_basis1: cparams.basis1,
+            local_basis2: cparams.basis2,
         }
     }
 
@@ -53,27 +59,56 @@ impl RevolutePositionConstraint {
         let mut position1 = positions[self.position1 as usize];
         let mut position2 = positions[self.position2 as usize];
 
-        let axis1 = position1 * self.local_axis1;
-        let axis2 = position2 * self.local_axis2;
-        let delta_rot =
-            Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
-        let ang_error = delta_rot.scaled_axis() * params.joint_erp;
-        let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error);
+        /*
+         * Linear part.
+         */
+        {
+            let anchor1 = position1 * self.local_anchor1;
+            let anchor2 = position2 * self.local_anchor2;
 
-        position1.rotation =
-            Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
-        position2.rotation =
-            Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
+            let r1 = anchor1 - position1 * self.local_com1;
+            let r2 = anchor2 - position2 * self.local_com2;
 
-        let anchor1 = position1 * self.local_anchor1;
-        let anchor2 = position2 * self.local_anchor2;
+            // TODO: don't do the "to_matrix".
+            let lhs = (self
+                .ii2
+                .quadform(&r2.gcross_matrix())
+                .add_diagonal(self.im2)
+                + self
+                    .ii1
+                    .quadform(&r1.gcross_matrix())
+                    .add_diagonal(self.im1))
+            .into_matrix();
+            let inv_lhs = lhs.try_inverse().unwrap();
 
-        let delta_tra = anchor2 - anchor1;
-        let lin_error = delta_tra * params.joint_erp;
-        let lin_impulse = self.lin_inv_lhs * lin_error;
+            let delta_tra = anchor2 - anchor1;
+            let lin_error = delta_tra * params.joint_erp;
+            let lin_impulse = inv_lhs * lin_error;
 
-        position1.translation.vector += self.im1 * lin_impulse;
-        position2.translation.vector -= self.im2 * lin_impulse;
+            let rot1 = self.ii1 * r1.gcross(lin_impulse);
+            let rot2 = self.ii2 * r2.gcross(lin_impulse);
+            position1.rotation = Rotation::new(rot1) * position1.rotation;
+            position2.rotation = Rotation::new(-rot2) * position2.rotation;
+            position1.translation.vector += self.im1 * lin_impulse;
+            position2.translation.vector -= self.im2 * lin_impulse;
+        }
+
+        /*
+         * Angular part.
+         */
+        {
+            let axis1 = position1 * self.local_axis1;
+            let axis2 = position2 * self.local_axis2;
+            let delta_rot =
+                Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity);
+            let ang_error = delta_rot.scaled_axis() * params.joint_erp;
+            let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error);
+
+            position1.rotation =
+                Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation;
+            position2.rotation =
+                Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation;
+        }
 
         positions[self.position1 as usize] = position1;
         positions[self.position2 as usize] = position2;
@@ -83,10 +118,16 @@ impl RevolutePositionConstraint {
 #[derive(Debug)]
 pub(crate) struct RevolutePositionGroundConstraint {
     position2: usize,
+    local_com2: Point<Real>,
+    im2: Real,
+    ii2: AngularInertia<Real>,
     anchor1: Point<Real>,
     local_anchor2: Point<Real>,
     axis1: Unit<Vector<Real>>,
     local_axis2: Unit<Vector<Real>>,
+
+    basis1: [Vector<Real>; 2],
+    local_basis2: [Vector<Real>; 2],
 }
 
 impl RevolutePositionGroundConstraint {
@@ -100,42 +141,82 @@ impl RevolutePositionGroundConstraint {
         let local_anchor2;
         let axis1;
         let local_axis2;
+        let basis1;
+        let local_basis2;
 
         if flipped {
             anchor1 = rb1.predicted_position * cparams.local_anchor2;
             local_anchor2 = cparams.local_anchor1;
             axis1 = rb1.predicted_position * cparams.local_axis2;
             local_axis2 = cparams.local_axis1;
+            basis1 = [
+                rb1.predicted_position * cparams.basis2[0],
+                rb1.predicted_position * cparams.basis2[1],
+            ];
+            local_basis2 = cparams.basis1;
         } else {
             anchor1 = rb1.predicted_position * cparams.local_anchor1;
             local_anchor2 = cparams.local_anchor2;
             axis1 = rb1.predicted_position * cparams.local_axis1;
             local_axis2 = cparams.local_axis2;
+            basis1 = [
+                rb1.predicted_position * cparams.basis1[0],
+                rb1.predicted_position * cparams.basis1[1],
+            ];
+            local_basis2 = cparams.basis2;
         };
 
         Self {
             anchor1,
             local_anchor2,
+            im2: rb2.effective_inv_mass,
+            ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
+            local_com2: rb2.mass_properties.local_com,
             axis1,
             local_axis2,
             position2: rb2.active_set_offset,
+            basis1,
+            local_basis2,
         }
     }
 
     pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
         let mut position2 = positions[self.position2 as usize];
 
-        let axis2 = position2 * self.local_axis2;
+        /*
+         * Linear part.
+         */
+        {
+            let anchor2 = position2 * self.local_anchor2;
 
-        let delta_rot =
-            Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp)
+            let r2 = anchor2 - position2 * self.local_com2;
+            // TODO: don't the the "to_matrix".
+            let lhs = self
+                .ii2
+                .quadform(&r2.gcross_matrix())
+                .add_diagonal(self.im2)
+                .into_matrix();
+            let inv_lhs = lhs.try_inverse().unwrap();
+
+            let delta_tra = anchor2 - self.anchor1;
+            let lin_error = delta_tra * params.joint_erp;
+            let lin_impulse = inv_lhs * lin_error;
+
+            let rot2 = self.ii2 * r2.gcross(lin_impulse);
+            position2.rotation = Rotation::new(-rot2) * position2.rotation;
+            position2.translation.vector -= self.im2 * lin_impulse;
+        }
+
+        /*
+         * Angular part.
+         */
+        {
+            let axis2 = position2 * self.local_axis2;
+            let delta_rot = Rotation::rotation_between_axis(&self.axis1, &axis2)
                 .unwrap_or_else(Rotation::identity);
-        position2.rotation = delta_rot * position2.rotation;
-
-        let anchor2 = position2 * self.local_anchor2;
-        let delta_tra = anchor2 - self.anchor1;
-        let lin_error = delta_tra * params.joint_erp;
-        position2.translation.vector -= lin_error;
+            let ang_error = delta_rot.scaled_axis() * params.joint_erp;
+            position2.rotation = Rotation::new(-ang_error) * position2.rotation;
+        }
 
         positions[self.position2 as usize] = position2;
     }

src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

@@ -1,8 +1,9 @@
-use crate::dynamics::solver::DeltaVel;
+use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
-use crate::math::{AngularInertia, Real, Vector};
+use crate::math::{AngularInertia, Real, Rotation, Vector};
+use crate::na::UnitQuaternion;
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 
@@ -20,7 +21,17 @@ pub(crate) struct RevoluteVelocityConstraint {
     rhs: Vector5<Real>,
     impulse: Vector5<Real>,
 
+    motor_inv_lhs: Real,
+    motor_rhs: Real,
+    motor_impulse: Real,
+    motor_max_impulse: Real,
+    motor_angle: Real, // Exists only to write it back into the joint.
+
+    motor_axis1: Vector<Real>,
+    motor_axis2: Vector<Real>,
+
     basis1: Matrix3x2<Real>,
+    basis2: Matrix3x2<Real>,
 
     im1: Real,
     im2: Real,
@@ -35,23 +46,23 @@ impl RevoluteVelocityConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &RevoluteJoint,
+        joint: &RevoluteJoint,
     ) -> Self {
         // Linear part.
-        let anchor1 = rb1.position * cparams.local_anchor1;
-        let anchor2 = rb2.position * cparams.local_anchor2;
+        let anchor1 = rb1.position * joint.local_anchor1;
+        let anchor2 = rb2.position * joint.local_anchor2;
         let basis1 = Matrix3x2::from_columns(&[
-            rb1.position * cparams.basis1[0],
-            rb1.position * cparams.basis1[1],
+            rb1.position * joint.basis1[0],
+            rb1.position * joint.basis1[1],
         ]);
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = r21 * basis1;
-        // NOTE: to simplify, we use basis2 = basis1.
-        // Though we may want to test if that does not introduce any instability.
+        let basis2 = Matrix3x2::from_columns(&[
+            rb2.position * joint.basis2[0],
+            rb2.position * joint.basis2[1],
+        ]);
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
+
         let im1 = rb1.effective_inv_mass;
         let im2 = rb2.effective_inv_mass;
 
@@ -64,12 +75,13 @@ impl RevoluteVelocityConstraint {
         let r2_mat = r2.gcross_matrix();
 
         let mut lhs = Matrix5::zeros();
+
         let lhs00 =
             ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
-        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat)) + basis1.tr_mul(&(ii1 * r1_mat));
+        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
 
-        // Note that cholesky won't read the upper-right part
+        // Note that Cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
         lhs.fixed_slice_mut::<U3, U3>(0, 0)
             .copy_from(&lhs00.into_matrix());
@@ -78,10 +90,64 @@ impl RevoluteVelocityConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
-        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
+        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
+        /*
+         * Motor.
+         */
+        let motor_axis1 = rb1.position * *joint.local_axis1;
+        let motor_axis2 = rb2.position * *joint.local_axis2;
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+        let mut motor_angle = 0.0;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
+            motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs {
+                crate::utils::inv(
+                    motor_axis2.dot(&ii2.transform_vector(motor_axis2))
+                        + motor_axis1.dot(&ii1.transform_vector(motor_axis1)),
+                ) * gamma
+            } else {
+                gamma
+            };
+            motor_rhs /= gamma;
+        }
+
+        /*
+         * Adjust the warmstart impulse.
+         * If the velocity along the free axis is somewhat high,
+         * we need to adjust the angular warmstart impulse because it
+         * may have a direction that is too different than last frame,
+         * making it counter-productive.
+         */
+        let mut impulse = joint.impulse * params.warmstart_coeff;
+        let axis_rot = Rotation::rotation_between(&joint.prev_axis1, &motor_axis1)
+            .unwrap_or_else(UnitQuaternion::identity);
+        let rotated_impulse = basis1.tr_mul(&(axis_rot * joint.world_ang_impulse));
+        impulse[3] = rotated_impulse.x * params.warmstart_coeff;
+        impulse[4] = rotated_impulse.y * params.warmstart_coeff;
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+
         RevoluteVelocityConstraint {
             joint_id,
             mj_lambda1: rb1.active_set_offset,
@@ -89,13 +155,21 @@ impl RevoluteVelocityConstraint {
             im1,
             ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
             basis1,
+            basis2,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
+            impulse,
             inv_lhs,
             rhs,
             r1,
             r2,
+            motor_rhs,
+            motor_inv_lhs,
+            motor_max_impulse,
+            motor_axis1,
+            motor_axis2,
+            motor_impulse,
+            motor_angle,
         }
     }
 
@@ -103,49 +177,90 @@ impl RevoluteVelocityConstraint {
         let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize];
         let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
 
-        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += self.im1 * lin_impulse;
+        mj_lambda1.linear += self.im1 * lin_impulse1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= self.im2 * lin_impulse;
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+
+        /*
+         * Motor
+         */
+        if self.motor_inv_lhs != 0.0 {
+            mj_lambda1.angular += self
+                .ii1_sqrt
+                .transform_vector(self.motor_axis1 * self.motor_impulse);
+            mj_lambda2.angular -= self
+                .ii2_sqrt
+                .transform_vector(self.motor_axis2 * self.motor_impulse);
+        }
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
 
+    fn solve_dofs(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+        let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
+        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
+            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
+        let rhs =
+            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
+        let impulse = self.inv_lhs * rhs;
+        self.impulse += impulse;
+        let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
+
+        mj_lambda1.linear += self.im1 * lin_impulse1;
+        mj_lambda1.angular += self
+            .ii1_sqrt
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
+
+        mj_lambda2.linear -= self.im2 * lin_impulse2;
+        mj_lambda2.angular -= self
+            .ii2_sqrt
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
+    }
+
+    fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+            let ang_dvel = ang_vel2.dot(&self.motor_axis2) - ang_vel1.dot(&self.motor_axis1);
+            let rhs = ang_dvel + self.motor_rhs;
+
+            let new_motor_impulse = na::clamp(
+                self.motor_impulse + self.motor_inv_lhs * rhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let impulse = new_motor_impulse - self.motor_impulse;
+            self.motor_impulse = new_motor_impulse;
+
+            mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.motor_axis1 * impulse);
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
+        }
+    }
+
     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 lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
-            - mj_lambda1.linear
-            - ang_vel1.gcross(self.r1);
-        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
-        let rhs =
-            Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
-        let impulse = self.inv_lhs * rhs;
-        self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
-
-        mj_lambda1.linear += self.im1 * lin_impulse;
-        mj_lambda1.angular += self
-            .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
-
-        mj_lambda2.linear -= self.im2 * lin_impulse;
-        mj_lambda2.angular -= self
-            .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+        self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda1, &mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1;
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
@@ -155,6 +270,11 @@ impl RevoluteVelocityConstraint {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
+            revolute.world_ang_impulse = self.basis1 * rot_part;
+            revolute.prev_axis1 = self.motor_axis1;
+            revolute.motor_last_angle = self.motor_angle;
+            revolute.motor_impulse = self.motor_impulse;
         }
     }
 }
@@ -171,7 +291,14 @@ pub(crate) struct RevoluteVelocityGroundConstraint {
     rhs: Vector5<Real>,
     impulse: Vector5<Real>,
 
-    basis1: Matrix3x2<Real>,
+    motor_axis2: Vector<Real>,
+    motor_inv_lhs: Real,
+    motor_rhs: Real,
+    motor_impulse: Real,
+    motor_max_impulse: Real,
+    motor_angle: Real, // Exists just for writing it into the joint.
+
+    basis2: Matrix3x2<Real>,
 
     im2: Real,
 
@@ -184,34 +311,46 @@ impl RevoluteVelocityGroundConstraint {
         joint_id: JointIndex,
         rb1: &RigidBody,
         rb2: &RigidBody,
-        cparams: &RevoluteJoint,
+        joint: &RevoluteJoint,
         flipped: bool,
-    ) -> Self {
+    ) -> AnyJointVelocityConstraint {
         let anchor2;
         let anchor1;
+        let axis1;
+        let axis2;
         let basis1;
+        let basis2;
 
         if flipped {
-            anchor1 = rb1.position * cparams.local_anchor2;
-            anchor2 = rb2.position * cparams.local_anchor1;
+            axis1 = rb1.position * *joint.local_axis2;
+            axis2 = rb2.position * *joint.local_axis1;
+            anchor1 = rb1.position * joint.local_anchor2;
+            anchor2 = rb2.position * joint.local_anchor1;
             basis1 = Matrix3x2::from_columns(&[
-                rb1.position * cparams.basis2[0],
-                rb1.position * cparams.basis2[1],
+                rb1.position * joint.basis2[0],
+                rb1.position * joint.basis2[1],
+            ]);
+            basis2 = Matrix3x2::from_columns(&[
+                rb2.position * joint.basis1[0],
+                rb2.position * joint.basis1[1],
             ]);
         } else {
-            anchor1 = rb1.position * cparams.local_anchor1;
-            anchor2 = rb2.position * cparams.local_anchor2;
+            axis1 = rb1.position * *joint.local_axis1;
+            axis2 = rb2.position * *joint.local_axis2;
+            anchor1 = rb1.position * joint.local_anchor1;
+            anchor2 = rb2.position * joint.local_anchor2;
             basis1 = Matrix3x2::from_columns(&[
-                rb1.position * cparams.basis1[0],
-                rb1.position * cparams.basis1[1],
+                rb1.position * joint.basis1[0],
+                rb1.position * joint.basis1[1],
+            ]);
+            basis2 = Matrix3x2::from_columns(&[
+                rb2.position * joint.basis2[0],
+                rb2.position * joint.basis2[1],
             ]);
         };
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = /*r21 * */ basis1;
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
         let im2 = rb2.effective_inv_mass;
         let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
         let r1 = anchor1 - rb1.world_com;
@@ -220,8 +359,8 @@ impl RevoluteVelocityGroundConstraint {
 
         let mut lhs = Matrix5::zeros();
         let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
 
         // Note that cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
@@ -232,54 +371,130 @@ impl RevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
-        let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel));
+        let lin_rhs = (rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
+        let ang_rhs = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
-        RevoluteVelocityGroundConstraint {
+        /*
+         * Motor part.
+         */
+        let mut motor_rhs = 0.0;
+        let mut motor_inv_lhs = 0.0;
+        let mut motor_angle = 0.0;
+        let motor_max_impulse = joint.motor_max_impulse;
+
+        let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients(
+            params.dt,
+            joint.motor_stiffness,
+            joint.motor_damping,
+        );
+
+        if stiffness != 0.0 {
+            motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
+            motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
+        }
+
+        if damping != 0.0 {
+            let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
+            motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
+        }
+
+        if stiffness != 0.0 || damping != 0.0 {
+            motor_inv_lhs = if keep_lhs {
+                crate::utils::inv(axis2.dot(&ii2.transform_vector(axis2))) * gamma
+            } else {
+                gamma
+            };
+            motor_rhs /= gamma;
+        }
+
+        let motor_impulse = na::clamp(joint.motor_impulse, -motor_max_impulse, motor_max_impulse)
+            * params.warmstart_coeff;
+
+        let result = RevoluteVelocityGroundConstraint {
             joint_id,
             mj_lambda2: rb2.active_set_offset,
             im2,
             ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
-            impulse: cparams.impulse * params.warmstart_coeff,
-            basis1,
+            impulse: joint.impulse * params.warmstart_coeff,
+            basis2,
             inv_lhs,
             rhs,
             r2,
-        }
+            motor_inv_lhs,
+            motor_impulse,
+            motor_axis2: axis2,
+            motor_max_impulse,
+            motor_rhs,
+            motor_angle,
+        };
+
+        AnyJointVelocityConstraint::RevoluteGroundConstraint(result)
     }
 
     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::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= self.im2 * lin_impulse;
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
 
+        /*
+         * Motor
+         */
+        if self.motor_inv_lhs != 0.0 {
+            mj_lambda2.angular -= self
+                .ii2_sqrt
+                .transform_vector(self.motor_axis2 * self.motor_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];
-
+    fn solve_dofs(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
         let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= self.im2 * lin_impulse;
         mj_lambda2.angular -= self
             .ii2_sqrt
             .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+    }
+    fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
+        if self.motor_inv_lhs != 0.0 {
+            let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
+            let ang_dvel = ang_vel2.dot(&self.motor_axis2);
+            let rhs = ang_dvel + self.motor_rhs;
+
+            let new_motor_impulse = na::clamp(
+                self.motor_impulse + self.motor_inv_lhs * rhs,
+                -self.motor_max_impulse,
+                self.motor_max_impulse,
+            );
+            let impulse = new_motor_impulse - self.motor_impulse;
+            self.motor_impulse = new_motor_impulse;
+
+            mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.motor_axis2 * impulse);
+        }
+    }
+
+    pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
+        let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize];
+
+        self.solve_dofs(&mut mj_lambda2);
+        self.solve_motors(&mut mj_lambda2);
 
         mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2;
     }
@@ -289,6 +504,8 @@ impl RevoluteVelocityGroundConstraint {
         let joint = &mut joints_all[self.joint_id].weight;
         if let JointParams::RevoluteJoint(revolute) = &mut joint.params {
             revolute.impulse = self.impulse;
+            revolute.motor_impulse = self.motor_impulse;
+            revolute.motor_last_angle = self.motor_angle;
         }
     }
 }

src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs

@@ -4,7 +4,9 @@ use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
 };
-use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, SimdReal, Vector, SIMD_WIDTH};
+use crate::math::{
+    AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
+};
 use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
 use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
 
@@ -22,7 +24,9 @@ pub(crate) struct WRevoluteVelocityConstraint {
     rhs: Vector5<SimdReal>,
     impulse: Vector5<SimdReal>,
 
+    axis1: [Vector<Real>; SIMD_WIDTH],
     basis1: Matrix3x2<SimdReal>,
+    basis2: Matrix3x2<SimdReal>,
 
     im1: SimdReal,
     im2: SimdReal,
@@ -37,7 +41,7 @@ impl WRevoluteVelocityConstraint {
         joint_id: [JointIndex; SIMD_WIDTH],
         rbs1: [&RigidBody; SIMD_WIDTH],
         rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        joints: [&RevoluteJoint; SIMD_WIDTH],
     ) -> Self {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
@@ -59,26 +63,27 @@ impl WRevoluteVelocityConstraint {
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
-        let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
+        let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
+        let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
         let local_basis1 = [
-            Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
-            Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
         ];
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let local_basis2 = [
+            Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
+            Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
+        ];
+        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
         let basis1 =
             Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]);
+        let basis2 =
+            Matrix3x2::from_columns(&[position2 * local_basis2[0], position2 * local_basis2[1]]);
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = r21 * basis1;
-        // NOTE: to simplify, we use basis2 = basis1.
-        // Though we may want to test if that does not introduce any instability.
         let ii1 = ii1_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r1_mat = r1.gcross_matrix();
@@ -90,10 +95,10 @@ impl WRevoluteVelocityConstraint {
         let mut lhs = Matrix5::zeros();
         let lhs00 =
             ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat));
-        let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix();
+        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)) + basis2.tr_mul(&(ii1 * r1_mat));
+        let lhs11 = (ii1.quadform3x2(&basis1) + ii2.quadform3x2(&basis2)).into_matrix();
 
-        // Note that cholesky won't read the upper-right part
+        // Note that Cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
         lhs.fixed_slice_mut::<U3, U3>(0, 0)
             .copy_from(&lhs00.into_matrix());
@@ -103,19 +108,42 @@ impl WRevoluteVelocityConstraint {
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
         let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
+        /*
+         * Adjust the warmstart impulse.
+         * If the velocity along the free axis is somewhat high,
+         * we need to adjust the angular warmstart impulse because it
+         * may have a direction that is too different than last frame,
+         * making it counter-productive.
+         */
+        let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
+        let mut impulse = impulse * warmstart_coeff;
+
+        let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
+        let rotated_impulse = Vector::from(array![|ii| {
+            let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
+                .unwrap_or_else(Rotation::identity);
+            axis_rot * joints[ii].world_ang_impulse
+        }; SIMD_WIDTH]);
+
+        let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
+        impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
+        impulse[4] = rotated_basis_impulse.y * warmstart_coeff;
+
         WRevoluteVelocityConstraint {
             joint_id,
             mj_lambda1,
             mj_lambda2,
             im1,
             ii1_sqrt,
+            axis1,
             basis1,
+            basis2,
             im2,
             ii2_sqrt,
-            impulse: impulse * SimdReal::splat(params.warmstart_coeff),
+            impulse,
             inv_lhs,
             rhs,
             r1,
@@ -141,18 +169,20 @@ impl WRevoluteVelocityConstraint {
             ),
         };
 
-        let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse1 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = self.impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -184,26 +214,28 @@ impl WRevoluteVelocityConstraint {
 
         let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
-        let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2)
-            - mj_lambda1.linear
-            - ang_vel1.gcross(self.r1);
-        let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1));
+
+        let lin_dvel = (mj_lambda2.linear + ang_vel2.gcross(self.r2))
+            - (mj_lambda1.linear + ang_vel1.gcross(self.r1));
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2) - self.basis1.tr_mul(&ang_vel1);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
-        let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let lin_impulse1 = impulse.fixed_rows::<U3>(0).into_owned();
+        let lin_impulse2 = impulse.fixed_rows::<U3>(0).into_owned();
+        let ang_impulse1 = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse2 = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
-        mj_lambda1.linear += lin_impulse * self.im1;
+        mj_lambda1.linear += lin_impulse1 * self.im1;
         mj_lambda1.angular += self
             .ii1_sqrt
-            .transform_vector(ang_impulse + self.r1.gcross(lin_impulse));
+            .transform_vector(ang_impulse1 + self.r1.gcross(lin_impulse1));
 
-        mj_lambda2.linear -= lin_impulse * self.im2;
+        mj_lambda2.linear -= lin_impulse2 * self.im2;
         mj_lambda2.angular -= self
             .ii2_sqrt
-            .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
+            .transform_vector(ang_impulse2 + self.r2.gcross(lin_impulse2));
 
         for ii in 0..SIMD_WIDTH {
             mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii);
@@ -216,10 +248,15 @@ impl WRevoluteVelocityConstraint {
     }
 
     pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
+        let rot_part = self.impulse.fixed_rows::<U2>(3).into_owned();
+        let world_ang_impulse = self.basis1 * rot_part;
+
         for ii in 0..SIMD_WIDTH {
             let joint = &mut joints_all[self.joint_id[ii]].weight;
             if let JointParams::RevoluteJoint(rev) = &mut joint.params {
-                rev.impulse = self.impulse.extract(ii)
+                rev.impulse = self.impulse.extract(ii);
+                rev.world_ang_impulse = world_ang_impulse.extract(ii);
+                rev.prev_axis1 = self.axis1[ii];
             }
         }
     }
@@ -237,7 +274,7 @@ pub(crate) struct WRevoluteVelocityGroundConstraint {
     rhs: Vector5<SimdReal>,
     impulse: Vector5<SimdReal>,
 
-    basis1: Matrix3x2<SimdReal>,
+    basis2: Matrix3x2<SimdReal>,
 
     im2: SimdReal,
 
@@ -250,7 +287,7 @@ impl WRevoluteVelocityGroundConstraint {
         joint_id: [JointIndex; SIMD_WIDTH],
         rbs1: [&RigidBody; SIMD_WIDTH],
         rbs2: [&RigidBody; SIMD_WIDTH],
-        cparams: [&RevoluteJoint; SIMD_WIDTH],
+        joints: [&RevoluteJoint; SIMD_WIDTH],
         flipped: [bool; SIMD_WIDTH],
     ) -> Self {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
@@ -267,33 +304,40 @@ impl WRevoluteVelocityGroundConstraint {
             array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
         );
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
-        let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
+        let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
 
         let local_anchor1 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
         );
         let local_anchor2 = Point::from(
-            array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
+            array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
         );
         let basis1 = Matrix3x2::from_columns(&[
             position1
                 * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
+                    array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
                 ),
             position1
                 * Vector::from(
-                    array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
+                    array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
                 ),
         ]);
+        let basis2 = Matrix3x2::from_columns(&[
+            position2
+                * Vector::from(
+                    array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
+                ),
+            position2
+                * Vector::from(
+                    array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
+                ),
+        ]);
+        let basis_projection2 = basis2 * basis2.transpose();
+        let basis2 = basis_projection2 * basis1;
 
         let anchor1 = position1 * local_anchor1;
         let anchor2 = position2 * local_anchor2;
 
-        //        let r21 = Rotation::rotation_between_axis(&axis1, &axis2)
-        //            .unwrap_or_else(Rotation::identity)
-        //            .to_rotation_matrix()
-        //            .into_inner();
-        //        let basis2 = /*r21 * */ basis1;
         let ii2 = ii2_sqrt.squared();
         let r1 = anchor1 - world_com1;
         let r2 = anchor2 - world_com2;
@@ -301,8 +345,8 @@ impl WRevoluteVelocityGroundConstraint {
 
         let mut lhs = Matrix5::zeros();
         let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2);
-        let lhs10 = basis1.tr_mul(&(ii2 * r2_mat));
-        let lhs11 = ii2.quadform3x2(&basis1).into_matrix();
+        let lhs10 = basis2.tr_mul(&(ii2 * r2_mat));
+        let lhs11 = ii2.quadform3x2(&basis2).into_matrix();
 
         // Note that cholesky won't read the upper-right part
         // of lhs so we don't have to fill it.
@@ -313,8 +357,8 @@ impl WRevoluteVelocityGroundConstraint {
 
         let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
 
-        let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1);
-        let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1));
+        let lin_rhs = (linvel2 + angvel2.gcross(r2)) - (linvel1 + angvel1.gcross(r1));
+        let ang_rhs = basis2.tr_mul(&angvel2) - basis1.tr_mul(&angvel1);
         let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y);
 
         WRevoluteVelocityGroundConstraint {
@@ -323,7 +367,7 @@ impl WRevoluteVelocityGroundConstraint {
             im2,
             ii2_sqrt,
             impulse: impulse * SimdReal::splat(params.warmstart_coeff),
-            basis1,
+            basis2,
             inv_lhs,
             rhs,
             r2,
@@ -341,7 +385,7 @@ impl WRevoluteVelocityGroundConstraint {
         };
 
         let lin_impulse = self.impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * self.impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * self.impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= lin_impulse * self.im2;
         mj_lambda2.angular -= self
@@ -366,13 +410,13 @@ impl WRevoluteVelocityGroundConstraint {
 
         let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);
         let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2);
-        let ang_dvel = self.basis1.tr_mul(&ang_vel2);
+        let ang_dvel = self.basis2.tr_mul(&ang_vel2);
         let rhs =
             Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs;
         let impulse = self.inv_lhs * rhs;
         self.impulse += impulse;
         let lin_impulse = impulse.fixed_rows::<U3>(0).into_owned();
-        let ang_impulse = self.basis1 * impulse.fixed_rows::<U2>(3).into_owned();
+        let ang_impulse = self.basis2 * impulse.fixed_rows::<U2>(3).into_owned();
 
         mj_lambda2.linear -= lin_impulse * self.im2;
         mj_lambda2.angular -= self