Fix some solver issues

- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier

16 files changed, 650 insertions, 94 deletions

diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs
index c5b2601..0083388 100644
--- a/src/dynamics/solver/categorization.rs
+++ b/src/dynamics/solver/categorization.rs
@@ -9,8 +9,8 @@ pub(crate) fn categorize_contacts(
     manifold_indices: &[ContactManifoldIndex],
     out_ground: &mut Vec<ContactManifoldIndex>,
     out_not_ground: &mut Vec<ContactManifoldIndex>,
-    out_generic: &mut Vec<ContactManifoldIndex>,
-    _unused: &mut Vec<ContactManifoldIndex>, // Unused but useful to simplify the parallel code.
+    out_generic_ground: &mut Vec<ContactManifoldIndex>,
+    out_generic_not_ground: &mut Vec<ContactManifoldIndex>,
 ) {
     for manifold_i in manifold_indices {
         let manifold = &manifolds[*manifold_i];
@@ -18,15 +18,19 @@ pub(crate) fn categorize_contacts(
         if manifold
             .data
             .rigid_body1
-            .map(|rb| multibody_joints.rigid_body_link(rb))
+            .and_then(|h| multibody_joints.rigid_body_link(h))
             .is_some()
             || manifold
                 .data
-                .rigid_body1
-                .map(|rb| multibody_joints.rigid_body_link(rb))
+                .rigid_body2
+                .and_then(|h| multibody_joints.rigid_body_link(h))
                 .is_some()
         {
-            out_generic.push(*manifold_i);
+            if manifold.data.relative_dominance != 0 {
+                out_generic_ground.push(*manifold_i);
+            } else {
+                out_generic_not_ground.push(*manifold_i);
+            }
         } else if manifold.data.relative_dominance != 0 {
             out_ground.push(*manifold_i)
         } else {
diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs
index 9160f2e..697fd24 100644
--- a/src/dynamics/solver/delta_vel.rs
+++ b/src/dynamics/solver/delta_vel.rs
@@ -1,7 +1,7 @@
 use crate::math::{AngVector, Vector, SPATIAL_DIM};
 use na::{DVectorSlice, DVectorSliceMut};
 use na::{Scalar, SimdRealField};
-use std::ops::AddAssign;
+use std::ops::{AddAssign, Sub};
 
 #[derive(Copy, Clone, Debug)]
 #[repr(C)]
@@ -44,3 +44,14 @@ impl<N: SimdRealField + Copy> AddAssign for DeltaVel<N> {
         self.angular += rhs.angular;
     }
 }
+
+impl<N: SimdRealField + Copy> Sub for DeltaVel<N> {
+    type Output = Self;
+
+    fn sub(self, rhs: Self) -> Self {
+        DeltaVel {
+            linear: self.linear - rhs.linear,
+            angular: self.angular - rhs.angular,
+        }
+    }
+}
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index 8c93511..f1ab0ea 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -9,11 +9,61 @@ use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::{WAngularInertia, WCross, WDot};
 
 use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
+use crate::dynamics::solver::GenericVelocityGroundConstraint;
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use na::DVector;
 
 #[derive(Copy, Clone, Debug)]
+pub(crate) enum AnyGenericVelocityConstraint {
+    NongroupedGround(GenericVelocityGroundConstraint),
+    Nongrouped(GenericVelocityConstraint),
+}
+
+impl AnyGenericVelocityConstraint {
+    pub fn solve(
+        &mut self,
+        jacobians: &DVector<Real>,
+        mj_lambdas: &mut [DeltaVel<Real>],
+        generic_mj_lambdas: &mut DVector<Real>,
+        solve_restitution: bool,
+        solve_friction: bool,
+    ) {
+        match self {
+            AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
+                jacobians,
+                mj_lambdas,
+                generic_mj_lambdas,
+                solve_restitution,
+                solve_friction,
+            ),
+            AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
+                jacobians,
+                generic_mj_lambdas,
+                solve_restitution,
+                solve_friction,
+            ),
+        }
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        match self {
+            AnyGenericVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifolds_all),
+            AnyGenericVelocityConstraint::NongroupedGround(c) => {
+                c.writeback_impulses(manifolds_all)
+            }
+        }
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        match self {
+            AnyGenericVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
+            AnyGenericVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
 pub(crate) struct GenericVelocityConstraint {
     // We just build the generic constraint on top of the velocity constraint,
     // adding some information we can use in the generic case.
@@ -31,7 +81,7 @@ impl GenericVelocityConstraint {
         manifold: &ContactManifold,
         bodies: &Bodies,
         multibodies: &MultibodyJointSet,
-        out_constraints: &mut Vec<GenericVelocityConstraint>,
+        out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
         push: bool,
@@ -293,6 +343,9 @@ impl GenericVelocityConstraint {
                             (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
 
                         constraint.elements[k].tangent_part.rhs[j] = rhs;
+                        // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
+                        // in lhs. See the corresponding code on the `velocity_constraint.rs`
+                        // file.
                         constraint.elements[k].tangent_part.r[j] = r;
                     }
                 }
@@ -316,9 +369,10 @@ impl GenericVelocityConstraint {
             };
 
             if push {
-                out_constraints.push(constraint);
+                out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint));
             } else {
-                out_constraints[manifold.data.constraint_index + _l] = constraint;
+                out_constraints[manifold.data.constraint_index + _l] =
+                    AnyGenericVelocityConstraint::Nongrouped(constraint);
             }
         }
     }
diff --git a/src/dynamics/solver/generic_velocity_constraint_element.rs b/src/dynamics/solver/generic_velocity_constraint_element.rs
index c31bbfb..e75dd01 100644
--- a/src/dynamics/solver/generic_velocity_constraint_element.rs
+++ b/src/dynamics/solver/generic_velocity_constraint_element.rs
@@ -41,7 +41,7 @@ fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize {
 
 impl GenericRhs {
     #[inline(always)]
-    fn dimpulse(
+    fn dvel(
         &self,
         j_id: usize,
         ndofs: usize,
@@ -110,14 +110,14 @@ impl VelocityConstraintTangentPart<Real> {
 
         #[cfg(feature = "dim2")]
         {
-            let dimpulse_0 = mj_lambda1.dimpulse(
+            let dvel_0 = mj_lambda1.dvel(
                 j_id1,
                 ndofs1,
                 jacobians,
                 &tangents1[0],
                 &self.gcross1[0],
                 mj_lambdas,
-            ) + mj_lambda2.dimpulse(
+            ) + mj_lambda2.dvel(
                 j_id2,
                 ndofs2,
                 jacobians,
@@ -126,7 +126,7 @@ impl VelocityConstraintTangentPart<Real> {
                 mj_lambdas,
             ) + self.rhs[0];
 
-            let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit);
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
             let dlambda = new_impulse - self.impulse[0];
             self.impulse[0] = new_impulse;
 
@@ -154,14 +154,14 @@ impl VelocityConstraintTangentPart<Real> {
 
         #[cfg(feature = "dim3")]
         {
-            let dimpulse_0 = mj_lambda1.dimpulse(
+            let dvel_0 = mj_lambda1.dvel(
                 j_id1,
                 ndofs1,
                 jacobians,
                 &tangents1[0],
                 &self.gcross1[0],
                 mj_lambdas,
-            ) + mj_lambda2.dimpulse(
+            ) + mj_lambda2.dvel(
                 j_id2,
                 ndofs2,
                 jacobians,
@@ -169,14 +169,14 @@ impl VelocityConstraintTangentPart<Real> {
                 &self.gcross2[0],
                 mj_lambdas,
             ) + self.rhs[0];
-            let dimpulse_1 = mj_lambda1.dimpulse(
+            let dvel_1 = mj_lambda1.dvel(
                 j_id1 + j_step,
                 ndofs1,
                 jacobians,
                 &tangents1[1],
                 &self.gcross1[1],
                 mj_lambdas,
-            ) + mj_lambda2.dimpulse(
+            ) + mj_lambda2.dvel(
                 j_id2 + j_step,
                 ndofs2,
                 jacobians,
@@ -186,8 +186,8 @@ impl VelocityConstraintTangentPart<Real> {
             ) + self.rhs[1];
 
             let new_impulse = na::Vector2::new(
-                self.impulse[0] - self.r[0] * dimpulse_0,
-                self.impulse[1] - self.r[1] * dimpulse_1,
+                self.impulse[0] - self.r[0] * dvel_0,
+                self.impulse[1] - self.r[1] * dvel_1,
             );
             let new_impulse = new_impulse.cap_magnitude(limit);
 
@@ -257,12 +257,11 @@ impl VelocityConstraintNormalPart<Real> {
         let j_id1 = j_id1(j_id, ndofs1, ndofs2);
         let j_id2 = j_id2(j_id, ndofs1, ndofs2);
 
-        let dimpulse =
-            mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
-                + mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
-                + self.rhs;
+        let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+            + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+            + self.rhs;
 
-        let new_impulse = (self.impulse - self.r * dimpulse).max(0.0);
+        let new_impulse = (self.impulse - self.r * dvel).max(0.0);
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
new file mode 100644
index 0000000..c9b2c3f
--- /dev/null
+++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs
@@ -0,0 +1,242 @@
+use crate::data::{BundleSet, ComponentSet};
+use crate::dynamics::solver::VelocityGroundConstraint;
+use crate::dynamics::{
+    IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType,
+    RigidBodyVelocity,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
+use crate::utils::WCross;
+
+use super::{VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart};
+use crate::dynamics::solver::AnyGenericVelocityConstraint;
+#[cfg(feature = "dim2")]
+use crate::utils::WBasis;
+use na::DVector;
+
+#[derive(Copy, Clone, Debug)]
+pub(crate) struct GenericVelocityGroundConstraint {
+    // We just build the generic constraint on top of the velocity constraint,
+    // adding some information we can use in the generic case.
+    pub velocity_constraint: VelocityGroundConstraint,
+    pub j_id: usize,
+    pub ndofs2: usize,
+}
+
+impl GenericVelocityGroundConstraint {
+    pub fn generate<Bodies>(
+        params: &IntegrationParameters,
+        manifold_id: ContactManifoldIndex,
+        manifold: &ContactManifold,
+        bodies: &Bodies,
+        multibodies: &MultibodyJointSet,
+        out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
+        jacobians: &mut DVector<Real>,
+        jacobian_id: &mut usize,
+        push: bool,
+    ) where
+        Bodies: ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyType>,
+    {
+        let inv_dt = params.inv_dt();
+        let erp_inv_dt = params.erp_inv_dt();
+
+        let mut handle1 = manifold.data.rigid_body1;
+        let mut handle2 = manifold.data.rigid_body2;
+        let flipped = manifold.data.relative_dominance < 0;
+
+        let (force_dir1, flipped_multiplier) = if flipped {
+            std::mem::swap(&mut handle1, &mut handle2);
+            (manifold.data.normal, -1.0)
+        } else {
+            (-manifold.data.normal, 1.0)
+        };
+
+        let (rb_vels1, world_com1) = if let Some(handle1) = handle1 {
+            let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
+                bodies.index_bundle(handle1.0);
+            (*vels1, mprops1.world_com)
+        } else {
+            (RigidBodyVelocity::zero(), Point::origin())
+        };
+
+        let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) =
+            bodies.index_bundle(handle2.unwrap().0);
+
+        let (mb2, link_id2) = handle2
+            .and_then(|h| multibodies.rigid_body_link(h))
+            .map(|m| (&multibodies[m.multibody], m.id))
+            .unwrap();
+        let mj_lambda2 = mb2.solver_id;
+
+        #[cfg(feature = "dim2")]
+        let tangents1 = force_dir1.orthonormal_basis();
+        #[cfg(feature = "dim3")]
+        let tangents1 = super::compute_tangent_contact_directions(
+            &force_dir1,
+            &rb_vels1.linvel,
+            &rb_vels2.linvel,
+        );
+
+        let multibodies_ndof = mb2.ndofs();
+        // For each solver contact we generate DIM constraints, and each constraints appends
+        // the multibodies jacobian and weighted jacobians
+        let required_jacobian_len =
+            *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
+
+        if jacobians.nrows() < required_jacobian_len {
+            jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+        }
+
+        for (_l, manifold_points) in manifold
+            .data
+            .solver_contacts
+            .chunks(MAX_MANIFOLD_POINTS)
+            .enumerate()
+        {
+            let chunk_j_id = *jacobian_id;
+            let mut constraint = VelocityGroundConstraint {
+                dir1: force_dir1,
+                #[cfg(feature = "dim3")]
+                tangent1: tangents1[0],
+                elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
+                im2: rb_mprops2.effective_inv_mass,
+                limit: 0.0,
+                mj_lambda2,
+                manifold_id,
+                manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
+                num_contacts: manifold_points.len() as u8,
+            };
+
+            for k in 0..manifold_points.len() {
+                let manifold_point = &manifold_points[k];
+                let dp1 = manifold_point.point - world_com1;
+                let dp2 = manifold_point.point - rb_mprops2.world_com;
+
+                let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
+                let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
+
+                constraint.limit = manifold_point.friction;
+                constraint.manifold_contact_id[k] = manifold_point.contact_id;
+
+                // Normal part.
+                {
+                    let torque_dir2 = dp2.gcross(-force_dir1);
+                    let inv_r2 = mb2
+                        .fill_jacobians(
+                            link_id2,
+                            -force_dir1,
+                            #[cfg(feature = "dim2")]
+                            na::vector!(torque_dir2),
+                            #[cfg(feature = "dim3")]
+                            torque_dir2,
+                            jacobian_id,
+                            jacobians,
+                        )
+                        .0;
+
+                    let r = crate::utils::inv(inv_r2);
+
+                    let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
+                    let is_resting = 1.0 - is_bouncy;
+
+                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1);
+                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
+                    rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
+                    let rhs_bias =
+                        /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
+
+                    constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
+                        gcross2: na::zero(), // Unused for generic constraints.
+                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs_wo_bias,
+                        impulse: na::zero(),
+                        r,
+                    };
+                }
+
+                // Tangent parts.
+                {
+                    constraint.elements[k].tangent_part.impulse = na::zero();
+
+                    for j in 0..DIM - 1 {
+                        let torque_dir2 = dp2.gcross(-tangents1[j]);
+                        let inv_r2 = mb2
+                            .fill_jacobians(
+                                link_id2,
+                                -tangents1[j],
+                                #[cfg(feature = "dim2")]
+                                na::vector![torque_dir2],
+                                #[cfg(feature = "dim3")]
+                                torque_dir2,
+                                jacobian_id,
+                                jacobians,
+                            )
+                            .0;
+
+                        let r = crate::utils::inv(inv_r2);
+
+                        let rhs = (vel1 - vel2
+                            + flipped_multiplier * manifold_point.tangent_velocity)
+                            .dot(&tangents1[j]);
+
+                        constraint.elements[k].tangent_part.rhs[j] = rhs;
+
+                        // FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
+                        // in lhs. See the corresponding code on the `velocity_constraint.rs`
+                        // file.
+                        constraint.elements[k].tangent_part.r[j] = r;
+                    }
+                }
+            }
+
+            let constraint = GenericVelocityGroundConstraint {
+                velocity_constraint: constraint,
+                j_id: chunk_j_id,
+                ndofs2: mb2.ndofs(),
+            };
+
+            if push {
+                out_constraints.push(AnyGenericVelocityConstraint::NongroupedGround(constraint));
+            } else {
+                out_constraints[manifold.data.constraint_index + _l] =
+                    AnyGenericVelocityConstraint::NongroupedGround(constraint);
+            }
+        }
+    }
+
+    pub fn solve(
+        &mut self,
+        jacobians: &DVector<Real>,
+        generic_mj_lambdas: &mut DVector<Real>,
+        solve_restitution: bool,
+        solve_friction: bool,
+    ) {
+        let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize;
+
+        let elements = &mut self.velocity_constraint.elements
+            [..self.velocity_constraint.num_contacts as usize];
+        VelocityGroundConstraintElement::generic_solve_group(
+            elements,
+            jacobians,
+            self.velocity_constraint.limit,
+            self.ndofs2,
+            self.j_id,
+            mj_lambda2,
+            generic_mj_lambdas,
+            solve_restitution,
+            solve_friction,
+        );
+    }
+
+    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
+        self.velocity_constraint.writeback_impulses(manifolds_all);
+    }
+
+    pub fn remove_bias_from_rhs(&mut self) {
+        self.velocity_constraint.remove_bias_from_rhs();
+    }
+}
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint_element.rs b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs
new file mode 100644
index 0000000..80c97ab
--- /dev/null
+++ b/src/dynamics/solver/generic_velocity_ground_constraint_element.rs
@@ -0,0 +1,141 @@
+use crate::dynamics::solver::{
+    VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
+    VelocityGroundConstraintTangentPart,
+};
+use crate::math::{Real, DIM};
+use na::DVector;
+#[cfg(feature = "dim2")]
+use na::SimdPartialOrd;
+
+impl VelocityGroundConstraintTangentPart<Real> {
+    #[inline]
+    pub fn generic_solve(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        limit: Real,
+        mj_lambda2: usize,
+        mj_lambdas: &mut DVector<Real>,
+    ) {
+        #[cfg(feature = "dim2")]
+        {
+            let dvel_0 = jacobians
+                .rows(j_id2, ndofs2)
+                .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+                + self.rhs[0];
+
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
+            let dlambda = new_impulse - self.impulse[0];
+            self.impulse[0] = new_impulse;
+
+            mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+                dlambda,
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),
+                1.0,
+            );
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let j_step = ndofs2 * 2;
+            let dvel_0 = jacobians
+                .rows(j_id2, ndofs2)
+                .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+                + self.rhs[0];
+            let dvel_1 = jacobians
+                .rows(j_id2 + j_step, ndofs2)
+                .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+                + self.rhs[1];
+
+            let new_impulse = na::Vector2::new(
+                self.impulse[0] - self.r[0] * dvel_0,
+                self.impulse[1] - self.r[1] * dvel_1,
+            );
+            let new_impulse = new_impulse.cap_magnitude(limit);
+
+            let dlambda = new_impulse - self.impulse;
+            self.impulse = new_impulse;
+
+            mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+                dlambda[0],
+                &jacobians.rows(j_id2 + ndofs2, ndofs2),
+                1.0,
+            );
+            mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+                dlambda[1],
+                &jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
+                1.0,
+            );
+        }
+    }
+}
+
+impl VelocityGroundConstraintNormalPart<Real> {
+    #[inline]
+    pub fn generic_solve(
+        &mut self,
+        j_id2: usize,
+        jacobians: &DVector<Real>,
+        ndofs2: usize,
+        mj_lambda2: usize,
+        mj_lambdas: &mut DVector<Real>,
+    ) {
+        let dvel = jacobians
+            .rows(j_id2, ndofs2)
+            .dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+            + self.rhs;
+
+        let new_impulse = (self.impulse - self.r * dvel).max(0.0);
+        let dlambda = new_impulse - self.impulse;
+        self.impulse = new_impulse;
+
+        mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
+            dlambda,
+            &jacobians.rows(j_id2 + ndofs2, ndofs2),
+            1.0,
+        );
+    }
+}
+
+impl VelocityGroundConstraintElement<Real> {
+    #[inline]
+    pub fn generic_solve_group(
+        elements: &mut [Self],
+        jacobians: &DVector<Real>,
+        limit: Real,
+        ndofs2: usize,
+        // Jacobian index of the first constraint.
+        j_id: usize,
+        mj_lambda2: usize,
+        mj_lambdas: &mut DVector<Real>,
+        solve_restitution: bool,
+        solve_friction: bool,
+    ) {
+        let j_step = ndofs2 * 2 * DIM;
+
+        // Solve penetration.
+        if solve_restitution {
+            let mut nrm_j_id = j_id;
+
+            for element in elements.iter_mut() {
+                element
+                    .normal_part
+                    .generic_solve(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
+                nrm_j_id += j_step;
+            }
+        }
+
+        // Solve friction.
+        if solve_friction {
+            let mut tng_j_id = j_id + ndofs2 * 2;
+
+            for element in elements.iter_mut() {
+                let limit = limit * element.normal_part.impulse;
+                let part = &mut element.tangent_part;
+                part.generic_solve(tng_j_id, jacobians, ndofs2, limit, mj_lambda2, mj_lambdas);
+                tng_j_id += j_step;
+            }
+        }
+    }
+}
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index aeb2698..9f82182 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -2,7 +2,8 @@ use super::VelocitySolver;
 use crate::counters::Counters;
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
-    AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints,
+    AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
+    SolverConstraints,
 };
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
@@ -13,7 +14,7 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::prelude::{MultibodyJointSet, RigidBodyActivation};
 
 pub struct IslandSolver {
-    contact_constraints: SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
+    contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
     joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
     velocity_solver: VelocitySolver,
 }
diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs
index 7607c28..3ffa94c 100644
--- a/src/dynamics/solver/mod.rs
+++ b/src/dynamics/solver/mod.rs
@@ -13,6 +13,7 @@ pub(self) use self::velocity_solver::VelocitySolver;
 pub(self) use delta_vel::DeltaVel;
 pub(self) use generic_velocity_constraint::*;
 pub(self) use generic_velocity_constraint_element::*;
+pub(self) use generic_velocity_ground_constraint::*;
 pub(self) use interaction_groups::*;
 pub(crate) use joint_constraint::MotorParameters;
 pub use joint_constraint::*;
@@ -29,6 +30,8 @@ mod categorization;
 mod delta_vel;
 mod generic_velocity_constraint;
 mod generic_velocity_constraint_element;
+mod generic_velocity_ground_constraint;
+mod generic_velocity_ground_constraint_element;
 mod interaction_groups;
 #[cfg(not(feature = "parallel"))]
 mod island_solver;
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
index cfd7575..2e92a5e 100644
--- a/src/dynamics/solver/solver_constraints.rs
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -5,6 +5,8 @@ use super::{
 use super::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
+use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint;
+use crate::dynamics::solver::AnyGenericVelocityConstraint;
 use crate::dynamics::solver::GenericVelocityConstraint;
 use crate::dynamics::{
     solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex,
@@ -58,7 +60,7 @@ impl<VelocityConstraint, GenVelocityConstraint>
     }
 }
 
-impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
+impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
     pub fn init_constraint_groups<Bodies>(
         &mut self,
         island_id: usize,
@@ -82,8 +84,8 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
             manifold_indices,
             &mut self.ground_interactions,
             &mut self.not_ground_interactions,
-            &mut self.generic_not_ground_interactions,
             &mut self.generic_ground_interactions,
+            &mut self.generic_not_ground_interactions,
         );
 
         self.interaction_groups.clear_groups();
@@ -141,18 +143,32 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
             manifold_indices,
         );
 
+        let mut jacobian_id = 0;
         #[cfg(feature = "simd-is-enabled")]
         {
             self.compute_grouped_constraints(params, bodies, manifolds);
         }
         self.compute_nongrouped_constraints(params, bodies, manifolds);
-        self.compute_generic_constraints(params, bodies, multibody_joints, manifolds);
+        self.compute_generic_constraints(
+            params,
+            bodies,
+            multibody_joints,
+            manifolds,
+            &mut jacobian_id,
+        );
 
         #[cfg(feature = "simd-is-enabled")]
         {
             self.compute_grouped_ground_constraints(params, bodies, manifolds);
         }
         self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
+        self.compute_generic_ground_constraints(
+            params,
+            bodies,
+            multibody_joints,
+            manifolds,
+            &mut jacobian_id,
+        );
     }
 
     #[cfg(feature = "simd-is-enabled")]
@@ -215,6 +231,7 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
         bodies: &Bodies,
         multibody_joints: &MultibodyJointSet,
         manifolds_all: &[&mut ContactManifold],
+        jacobian_id: &mut usize,
     ) where
         Bodies: ComponentSet<RigidBodyVelocity>
             + ComponentSet<RigidBodyPosition>
@@ -222,7 +239,6 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
             + ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyType>,
     {
-        let mut jacobian_id = 0;
         for manifold_i in &self.generic_not_ground_interactions {
             let manifold = &manifolds_all[*manifold_i];
             GenericVelocityConstraint::generate(
@@ -233,7 +249,37 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
                 multibody_joints,
                 &mut self.generic_velocity_constraints,
                 &mut self.generic_jacobians,
-                &mut jacobian_id,
+                jacobian_id,
+                true,
+            );
+        }
+    }
+
+    fn compute_generic_ground_constraints<Bodies>(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &Bodies,
+        multibody_joints: &M