Merge pull request #119 from dimforge/joint_drive

Add joint motors

14 files changed, 2738 insertions, 335 deletions

diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 21cc642..0f01798 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/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;
diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
index e75f978..a110fbb 100644
--- a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
+++ b/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;
         }
     }
 }
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint.rs
new file mode 100644
index 0000000..be12258
--- /dev/null
+++ b/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;
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
new file mode 100644
index 0000000..9ceea67
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
@@ -0,0 +1,51 @@
+use super::{GenericPositionConstraint, GenericPositionGroundConstraint};
+use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
+use crate::math::{Isometry, Real, SIMD_WIDTH};
+
+// TODO: this does not uses SIMD optimizations yet.
+#[derive(Debug)]
+pub(crate) struct WGenericPositionConstraint {
+    constraints: [GenericPositionConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
+
+#[derive(Debug)]
+pub(crate) struct WGenericPositionGroundConstraint {
+    constraints: [GenericPositionGroundConstraint; SIMD_WIDTH],
+}
+
+impl WGenericPositionGroundConstraint {
+    pub fn from_params(
+        rbs1: [&RigidBody; SIMD_WIDTH],
+        rbs2: [&RigidBody; SIMD_WIDTH],
+        cparams: [&GenericJoint; SIMD_WIDTH],
+        flipped: [bool; SIMD_WIDTH],
+    ) -> Self {
+        Self {
+            constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
+        }
+    }
+
+    pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}
diff --git a/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
new file mode 100644
index 0000000..f391873
--- /dev/null
+++ b/src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
@@ -0,