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

26 files changed, 753 insertions, 128 deletions

diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs
index d998eec..844db41 100644
--- a/src/dynamics/integration_parameters.rs
+++ b/src/dynamics/integration_parameters.rs
@@ -30,6 +30,13 @@ pub struct IntegrationParameters {
     /// (default `0.0`).
     pub erp: Real,
 
+    /// 0-1: multiplier applied to each accumulated impulse during  constraints resolution.
+    /// This is similar to the concept of CFN (Constraint Force Mixing) except that it is
+    /// a multiplicative factor instead of an additive factor.
+    /// Larger values lead to stiffer constraints (1.0 being completely stiff).
+    /// Smaller values lead to more compliant constraints.
+    pub delassus_inv_factor: Real,
+
     /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
     pub allowed_linear_error: Real,
     /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
@@ -96,6 +103,7 @@ impl Default for IntegrationParameters {
             min_ccd_dt: 1.0 / 60.0 / 100.0,
             velocity_solve_fraction: 1.0,
             erp: 0.8,
+            delassus_inv_factor: 0.75,
             allowed_linear_error: 0.001, // 0.005
             prediction_distance: 0.002,
             max_velocity_iterations: 4,
diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs
index 55b47c9..53f803d 100644
--- a/src/dynamics/island_manager.rs
+++ b/src/dynamics/island_manager.rs
@@ -5,6 +5,7 @@ use crate::dynamics::{
 };
 use crate::geometry::{ColliderParent, NarrowPhase};
 use crate::math::Real;
+use crate::utils::WDot;
 
 /// Structure responsible for maintaining the set of active rigid-bodies, and
 /// putting non-moving rigid-bodies to sleep to save computation times.
@@ -172,6 +173,7 @@ impl IslandManager {
 
     pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>(
         &mut self,
+        dt: Real,
         bodies: &mut Bodies,
         colliders: &Colliders,
         narrow_phase: &NarrowPhase,
@@ -207,12 +209,15 @@ impl IslandManager {
             let stack = &mut self.stack;
 
             let vels: &RigidBodyVelocity = bodies.index(h.0);
-            let pseudo_kinetic_energy = vels.pseudo_kinetic_energy();
+            let sq_linvel = vels.linvel.norm_squared();
+            let sq_angvel = vels.angvel.gdot(vels.angvel);
 
             bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
-                update_energy(activation, pseudo_kinetic_energy);
+                update_energy(activation, sq_linvel, sq_angvel, dt);
 
-                if activation.energy <= activation.threshold {
+                if activation.time_since_can_sleep
+                    >= RigidBodyActivation::default_time_until_sleep()
+                {
                     // Mark them as sleeping for now. This will
                     // be set to false during the graph traversal
                     // if it should not be put to sleep.
@@ -346,8 +351,12 @@ impl IslandManager {
     }
 }
 
-fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) {
-    let mix_factor = 0.01;
-    let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy;
-    activation.energy = new_energy.min(activation.threshold.abs() * 4.0);
+fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) {
+    if sq_linvel < activation.linear_threshold * activation.linear_threshold
+        && sq_angvel < activation.angular_threshold * activation.angular_threshold
+    {
+        activation.time_since_can_sleep += dt;
+    } else {
+        activation.time_since_can_sleep = 0.0;
+    }
 }
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 183b668..890f021 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -100,7 +100,7 @@ impl ImpulseJointSet {
 
     /// Gets the joint with the given handle without a known generation.
     ///
-    /// This is useful when you know you want the joint at position `i` but
+    /// This is useful when you know you want the joint at index `i` but
     /// don't know what is its current generation number. Generation numbers are
     /// used to protect from the ABA problem because the joint position `i`
     /// are recycled between two insertion and a removal.
diff --git a/src/dynamics/joint/joint_data.rs b/src/dynamics/joint/joint_data.rs
index 35d832d..b1ad6c6 100644
--- a/src/dynamics/joint/joint_data.rs
+++ b/src/dynamics/joint/joint_data.rs
@@ -218,12 +218,14 @@ impl JointData {
     }
 
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
+    #[must_use]
     pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
         self.motors[axis as usize].model = model;
         self
     }
 
     /// Sets the target velocity this motor needs to reach.
+    #[must_use]
     pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
         self.motor_axis(
             axis,
@@ -235,6 +237,7 @@ impl JointData {
     }
 
     /// Sets the target angle this motor needs to reach.
+    #[must_use]
     pub fn motor_position(
         self,
         axis: JointAxis,
@@ -246,6 +249,7 @@ impl JointData {
     }
 
     /// Configure both the target angle and target velocity of the motor.
+    #[must_use]
     pub fn motor_axis(
         mut self,
         axis: JointAxis,
@@ -263,6 +267,7 @@ impl JointData {
         self
     }
 
+    #[must_use]
     pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
         self.motors[axis as usize].max_impulse = max_impulse;
         self
diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs
index 1b88245..e31a333 100644
--- a/src/dynamics/joint/multibody_joint/multibody.rs
+++ b/src/dynamics/joint/multibody_joint/multibody.rs
@@ -62,7 +62,10 @@ fn concat_rb_mass_matrix(
 }
 
 /// An articulated body simulated using the reduced-coordinates approach.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
 pub struct Multibody {
+    // TODO: serialization: skip the workspace fields.
     links: MultibodyLinkVec,
     pub(crate) velocities: DVector<Real>,
     pub(crate) damping: DVector<Real>,
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs
index 92749c1..1d14680 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs
@@ -11,6 +11,7 @@ use na::{DVector, DVectorSliceMut};
 #[cfg(feature = "dim3")]
 use na::{UnitQuaternion, Vector3};
 
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug)]
 pub struct MultibodyJoint {
     pub data: JointData,
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
index 8337158..24b329f 100644
--- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs
@@ -1,4 +1,4 @@
-use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut};
+use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
 use crate::dynamics::joint::MultibodyLink;
 use crate::dynamics::{
     IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
@@ -6,19 +6,18 @@ use crate::dynamics::{
 };
 use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
 use crate::parry::partitioning::IndexedData;
-use std::ops::Index;
 
 /// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[repr(transparent)]
-pub struct MultibodyJointHandle(pub crate::data::arena::Index);
+pub struct MultibodyJointHandle(pub Index);
 
 /// The temporary index of a multibody added to a `MultibodyJointSet`.
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[repr(transparent)]
-pub struct MultibodyIndex(pub crate::data::arena::Index);
+pub struct MultibodyIndex(pub Index);
 
 impl MultibodyJointHandle {
     /// Converts this handle into its (index, generation) components.
@@ -28,12 +27,12 @@ impl MultibodyJointHandle {
 
     /// Reconstructs an handle from its (index, generation) components.
     pub fn from_raw_parts(id: u32, generation: u32) -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(id, generation))
+        Self(Index::from_raw_parts(id, generation))
     }
 
     /// An always-invalid rigid-body handle.
     pub fn invalid() -> Self {
-        Self(crate::data::arena::Index::from_raw_parts(
+        Self(Index::from_raw_parts(
             crate::INVALID_U32,
             crate::INVALID_U32,
         ))
@@ -55,6 +54,7 @@ impl IndexedData for MultibodyJointHandle {
     }
 }
 
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Copy, Clone, Debug, PartialEq, Eq)]
 pub struct MultibodyJointLink {
     pub graph_id: RigidBodyGraphIndex,
@@ -66,7 +66,7 @@ impl Default for MultibodyJointLink {
     fn default() -> Self {
         Self {
             graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
-            multibody: MultibodyIndex(crate::data::arena::Index::from_raw_parts(
+            multibody: MultibodyIndex(Index::from_raw_parts(
                 crate::INVALID_U32,
                 crate::INVALID_U32,
             )),
@@ -76,6 +76,8 @@ impl Default for MultibodyJointLink {
 }
 
 /// A set of rigid bodies that can be handled by a physics pipeline.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
 pub struct MultibodyJointSet {
     pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
     pub(crate) rb2mb: Coarena<MultibodyJointLink>,
@@ -316,6 +318,26 @@ impl MultibodyJointSet {
         Some((multibody, link.id))
     }
 
+    /// Gets the joint with the given handle without a known generation.
+    ///
+    /// This is useful when you know you want the joint at index `i` but
+    /// don't know what is its current generation number. Generation numbers are
+    /// used to protect from the ABA problem because the joint position `i`
+    /// are recycled between two insertion and a removal.
+    ///
+    /// Using this is discouraged in favor of `self.get(handle)` which does not
+    /// suffer form the ABA problem.
+    pub fn get_unknown_gen(&self, i: u32) -> Option<(&Multibody, usize, MultibodyJointHandle)> {
+        let link = self.rb2mb.get_unknown_gen(i)?;
+        let gen = self.rb2mb.get_gen(i)?;
+        let multibody = self.multibodies.get(link.multibody.0)?;
+        Some((
+            multibody,
+            link.id,
+            MultibodyJointHandle(Index::from_raw_parts(i, gen)),
+        ))
+    }
+
     /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
     /// by an multibody_joint.
     pub fn attached_bodies<'a>(
@@ -335,7 +357,7 @@ impl MultibodyJointSet {
     }
 }
 
-impl Index<MultibodyIndex> for MultibodyJointSet {
+impl std::ops::Index<MultibodyIndex> for MultibodyJointSet {
     type Output = Multibody;
 
     fn index(&self, index: MultibodyIndex) -> &Multibody {
diff --git a/src/dynamics/joint/multibody_joint/multibody_link.rs b/src/dynamics/joint/multibody_joint/multibody_link.rs
index 4738865..3c35446 100644
--- a/src/dynamics/joint/multibody_joint/multibody_link.rs
+++ b/src/dynamics/joint/multibody_joint/multibody_link.rs
@@ -5,6 +5,8 @@ use crate::math::{Isometry, Real, Vector};
 use crate::prelude::RigidBodyVelocity;
 
 /// One link of a multibody.
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
 pub struct MultibodyLink {
     // FIXME: make all those private.
     pub(crate) internal_id: usize,
@@ -96,6 +98,8 @@ impl MultibodyLink {
 }
 
 // FIXME: keep this even if we already have the Index2 traits?
+#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+#[derive(Clone)]
 pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
 
 impl MultibodyLinkVec {
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index e5dcd04..db972d4 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1089,7 +1089,8 @@ impl RigidBodyBuilder {
         }
 
         if !self.can_sleep {
-            rb.rb_activation.threshold = -1.0;
+            rb.rb_activation.linear_threshold = -1.0;
+            rb.rb_activation.angular_threshold = -1.0;
         }
 
         rb
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index ff3a614..fdbd33f 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -927,11 +927,13 @@ impl RigidBodyDominance {
 #[derive(Copy, Clone, Debug, PartialEq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 pub struct RigidBodyActivation {
-    /// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
-    pub threshold: Real,
-    /// The current pseudo-kinetic energy of the body.
-    pub energy: Real,
-    /// Is this body already sleeping?
+    /// The threshold linear velocity bellow which the body can fall asleep.
+    pub linear_threshold: Real,
+    /// The angular linear velocity bellow which the body can fall asleep.
+    pub angular_threshold: Real,
+    /// Since how much time can this body sleep?
+    pub time_since_can_sleep: Real,
+    /// Is this body sleeping?
     pub sleeping: bool,
 }
 
@@ -942,16 +944,28 @@ impl Default for RigidBodyActivation {
 }
 
 impl RigidBodyActivation {
-    /// The default amount of energy bellow which a body can be put to sleep by rapier.
-    pub fn default_threshold() -> Real {
-        0.01
+    /// The default linear velocity bellow which a body can be put to sleep.
+    pub fn default_linear_threshold() -> Real {
+        0.4
+    }
+
+    /// The default angular velocity bellow which a body can be put to sleep.
+    pub fn default_angular_threshold() -> Real {
+        0.5
+    }
+
+    /// The amount of time the rigid-body must remain bellow it’s linear and angular velocity
+    /// threshold before falling to sleep.
+    pub fn default_time_until_sleep() -> Real {
+        2.0
     }
 
     /// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
     pub fn active() -> Self {
         RigidBodyActivation {
-            threshold: Self::default_threshold(),
-            energy: Self::default_threshold() * 4.0,
+            linear_threshold: Self::default_linear_threshold(),
+            angular_threshold: Self::default_angular_threshold(),
+            time_since_can_sleep: 0.0,
             sleeping: false,
         }
     }
@@ -959,16 +973,18 @@ impl RigidBodyActivation {
     /// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
     pub fn inactive() -> Self {
         RigidBodyActivation {
-            threshold: Self::default_threshold(),
-            energy: 0.0,
+            linear_threshold: Self::default_linear_threshold(),
+            angular_threshold: Self::default_angular_threshold(),
             sleeping: true,
+            time_since_can_sleep: Self::default_time_until_sleep(),
         }
     }
 
     /// Create a new activation status that prevents the rigid-body from sleeping.
     pub fn cannot_sleep() -> Self {
         RigidBodyActivation {
-            threshold: -Real::MAX,
+            linear_threshold: -1.0,
+            angular_threshold: -1.0,
             ..Self::active()
         }
     }
@@ -976,22 +992,22 @@ impl RigidBodyActivation {
     /// Returns `true` if the body is not asleep.
     #[inline]
     pub fn is_active(&self) -> bool {
-        self.energy != 0.0
+        !self.sleeping
     }
 
     /// Wakes up this rigid-body.
     #[inline]
     pub fn wake_up(&mut self, strong: bool) {
         self.sleeping = false;
-        if strong || self.energy == 0.0 {
-            self.energy = self.threshold.abs() * 2.0;
+        if strong {
+            self.time_since_can_sleep = 0.0;
         }
     }
 
     /// Put this rigid-body to sleep.
     #[inline]
     pub fn sleep(&mut self) {
-        self.energy = 0.0;
         self.sleeping = true;
+        self.time_since_can_sleep = Self::default_time_until_sleep();
     }
 }
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,