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

crates/rapier_testbed2d/Cargo.toml

@@ -21,7 +21,7 @@ required-features = [ "dim2" ]
 [features]
 default = [ "dim2" ]
 dim2 = [ ]
-parallel = [ "rapier2d/parallel", "num_cpus" ]
+parallel = [ "rapier/parallel", "num_cpus" ]
 other-backends = [ "wrapped2d" ]
 
 
@@ -33,7 +33,6 @@ instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 num_cpus   = { version = "1", optional = true }
 wrapped2d  = { version = "0.4", optional = true }
-parry2d = "0.8"
 crossbeam = "0.8"
 bincode = "1"
 Inflector  = "0.11"
@@ -51,7 +50,8 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
 bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
 #bevy_webgl2 = "0.5"
 
-[dependencies.rapier2d]
+[dependencies.rapier]
+package = "rapier2d"
 path = "../rapier2d"
 version = "0.12.0-alpha.0"
 features = [ "serde-serialize" ]

crates/rapier_testbed3d/Cargo.toml

@@ -21,7 +21,7 @@ required-features = [ "dim3" ]
 [features]
 default = [ "dim3" ]
 dim3 = [ ]
-parallel = [ "rapier3d/parallel", "num_cpus" ]
+parallel = [ "rapier/parallel", "num_cpus" ]
 other-backends = [ "physx", "physx-sys", "glam" ]
 
 [dependencies]
@@ -32,7 +32,6 @@ instant    = { version = "0.1", features = [ "web-sys", "now" ]}
 bitflags   = "1"
 glam       = { version = "0.12", optional = true }
 num_cpus   = { version = "1", optional = true }
-parry3d    = "0.8"
 physx      = { version = "0.11", optional = true }
 physx-sys = { version = "0.4", optional = true }
 crossbeam = "0.8"
@@ -53,7 +52,8 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
 bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
 #bevy_webgl2 = "0.5"
 
-[dependencies.rapier3d]
+[dependencies.rapier]
+package = "rapier3d"
 path = "../rapier3d"
 version = "0.12.0-alpha.0"
 features = [ "serde-serialize" ]

examples2d/all_examples2.rs

@@ -14,6 +14,7 @@ mod collision_groups2;
 mod convex_polygons2;
 mod damping2;
 mod debug_box_ball2;
+mod drum2;
 mod heightfield2;
 mod joints2;
 mod locked_rotations2;
@@ -63,6 +64,7 @@ pub fn main() {
         ("Collision groups", collision_groups2::init_world),
         ("Convex polygons", convex_polygons2::init_world),
         ("Damping", damping2::init_world),
+        ("Drum", drum2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Joints", joints2::init_world),
         ("Locked rotations", locked_rotations2::init_world),

examples2d/drum2.rs

@@ -0,0 +1,81 @@
+use rapier2d::prelude::*;
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Create the boxes
+     */
+    let num = 30;
+    let rad = 0.2;
+
+    let shift = rad * 2.0;
+    let centerx = shift * num as f32 / 2.0;
+    let centery = shift * num as f32 / 2.0;
+
+    for i in 0usize..num {
+        for j in 0usize..num {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift - centery;
+
+            // Build the rigid body.
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .translation(vector![x, y])
+                .build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad).build();
+            colliders.insert_with_parent(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Setup a velocity-based kinematic rigid body.
+     */
+    let platform_body = RigidBodyBuilder::new_kinematic_velocity_based().build();
+    let velocity_based_platform_handle = bodies.insert(platform_body);
+
+    let sides = [
+        (10.0, 0.25, vector![0.0, 10.0]),
+        (10.0, 0.25, vector![0.0, -10.0]),
+        (0.25, 10.0, vector![10.0, 0.0]),
+        (0.25, 10.0, vector![-10.0, 0.0]),
+    ];
+    let balls = [
+        (1.25, vector![6.0, 6.0]),
+        (1.25, vector![-6.0, 6.0]),
+        (1.25, vector![6.0, -6.0]),
+        (1.25, vector![-6.0, -6.0]),
+    ];
+
+    for (hx, hy, pos) in sides {
+        let collider = ColliderBuilder::cuboid(hx, hy).translation(pos).build();
+        colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
+    }
+    for (r, pos) in balls {
+        let collider = ColliderBuilder::ball(r).translation(pos).build();
+        colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
+    }
+
+    /*
+     * Setup a callback to control the platform.
+     */
+    testbed.add_callback(move |_, physics, _, _| {
+        // Update the velocity-based kinematic body by setting its velocity.
+        if let Some(platform) = physics.bodies.get_mut(velocity_based_platform_handle) {
+            platform.set_angvel(-0.15, true);
+        }
+    });
+
+    /*
+     * Run the simulation.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![0.0, 1.0], 40.0);
+}

examples3d/debug_articulations3.rs

@@ -66,17 +66,15 @@ pub fn init_world(testbed: &mut Testbed) {
     let mut impulse_joints = ImpulseJointSet::new();
     let mut multibody_joints = MultibodyJointSet::new();
 
-    let rigid_body = RigidBodyBuilder::new_dynamic().build();
+    let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
-    let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis())
+        .translation(vector![0.0, -3.02, 0.0])
-        .translation(vector![0.0, -3.0, 0.0])
         .rotation(vector![0.1, 0.0, 0.1])
         .build();
-    let handle = bodies.insert(rigid_body);
+    colliders.insert(collider);
-    colliders.insert_with_parent(collider, handle, &mut bodies);
 
-    let rigid_body = RigidBodyBuilder::new_static().build();
+    let rigid_body = RigidBodyBuilder::new_dynamic().build();
-    let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis())
+    let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
-        .translation(vector![0.0, -3.02, 0.0])
+        .translation(vector![0.0, -3.0, 0.0])
         .rotation(vector![0.1, 0.0, 0.1])
         .build();
     let handle = bodies.insert(rigid_body);

src/data/arena.rs

@@ -70,11 +70,8 @@ impl Index {
     ///
     /// Providing arbitrary values will lead to malformed indices and ultimately
     /// panics.
-    pub fn from_raw_parts(a: u32, b: u32) -> Index {
+    pub fn from_raw_parts(index: u32, generation: u32) -> Index {
-        Index {
+        Index { index, generation }
-            index: a,
-            generation: b,
-        }
     }
 
     /// Convert this `Index` into its raw parts.

src/data/coarena.rs

@@ -29,6 +29,10 @@ impl<T> Coarena<T> {
         self.data.get(index as usize).map(|(_, t)| t)
     }
 
+    pub(crate) fn get_gen(&self, index: u32) -> Option<u32> {
+        self.data.get(index as usize).map(|(gen, _)| *gen)
+    }
+
     /// Deletes an element for the coarena and returns its value.
     ///
     /// This method will reset the value to the given `removed_value`.

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,

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) {
+fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) {
-    let mix_factor = 0.01;
+    if sq_linvel < activation.linear_threshold * activation.linear_threshold
-    let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy;
+        && sq_angvel < activation.angular_threshold * activation.angular_threshold
-    activation.energy = new_energy.min(activation.threshold.abs() * 4.0);
+    {
+        activation.time_since_can_sleep += dt;
+    } else {
+        activation.time_since_can_sleep = 0.0;
+    }
 }

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.

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

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>,

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,

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 {

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 {

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

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.
+    /// The threshold linear velocity bellow which the body can fall asleep.
-    pub threshold: Real,
+    pub linear_threshold: Real,
-    /// The current pseudo-kinetic energy of the body.
+    /// The angular linear velocity bellow which the body can fall asleep.
-    pub energy: Real,
+    pub angular_threshold: Real,
-    /// Is this body already sleeping?
+    /// 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.
+    /// The default linear velocity bellow which a body can be put to sleep.
-    pub fn default_threshold() -> Real {
+    pub fn default_linear_threshold() -> Real {
-        0.01
+        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(),
+            linear_threshold: Self::default_linear_threshold(),
-            energy: Self::default_threshold() * 4.0,
+            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(),
+            linear_threshold: Self::default_linear_threshold(),
-            energy: 0.0,
+            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 {
+        if strong {
-            self.energy = self.threshold.abs() * 2.0;
+            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();
     }
 }

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>,
+    out_generic_ground: &mut Vec<ContactManifoldIndex>,
-    _unused: &mut Vec<ContactManifoldIndex>, // Unused but useful to simplify the parallel code.
+    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
+                .rigid_body2
-                .map(|rb| multibody_joints.rigid_body_link(rb))
+                .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 {

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

src/dynamics/solver/generic_velocity_constraint.rs

@@ -9,10 +9,60 @@ 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,
@@ -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);
             }
         }
     }

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[0] - self.r[0] * dvel_0,
-                self.impulse[1] - self.r[1] * dimpulse_1,
+                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 =
+        let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
-            mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+            + mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
-                + mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+            + self.rhs;
-                + 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;
 

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

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

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

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;

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: &MultibodyJointSet,
+        manifolds_all: &[&mut ContactManifold],
+        jacobian_id: &mut usize,
+    ) where
+        Bodies: ComponentSet<RigidBodyVelocity>
+            + ComponentSet<RigidBodyPosition>
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>,
+    {
+        for manifold_i in &self.generic_ground_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            GenericVelocityGroundConstraint::generate(
+                params,
+                *manifold_i,
+                manifold,
+                bodies,
+                multibody_joints,
+                &mut self.generic_velocity_constraints,
+                &mut self.generic_jacobians,
+                jacobian_id,
                 true,
             );
         }

src/dynamics/solver/velocity_constraint.rs

@@ -236,7 +236,7 @@ impl VelocityConstraint {
                         .transform_vector(dp2.gcross(-force_dir1));
 
                     let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
-                    let r = 1.0
+                    let r = params.delassus_inv_factor
                         / (force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
@@ -246,8 +246,7 @@ impl VelocityConstraint {
 
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
-                    rhs_wo_bias +=
+                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                        (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
                     let rhs_bias = /* is_resting
                         * */  erp_inv_dt
@@ -275,17 +274,26 @@ impl VelocityConstraint {
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
                         let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
-                        let r = 1.0
+                        let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
-                            / (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+                            + gcross1.gdot(gcross1)
-                                + gcross1.gdot(gcross1)
+                            + gcross2.gdot(gcross2);
-                                + gcross2.gdot(gcross2));
                         let rhs =
                             (vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
 
                         constraint.elements[k].tangent_part.gcross1[j] = gcross1;
                         constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                         constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        constraint.elements[k].tangent_part.r[j] = r;
+                        constraint.elements[k].tangent_part.r[j] =
+                            if cfg!(feature = "dim2") { 1.0 / r } else { r };
+                    }
+
+                    #[cfg(feature = "dim3")]
+                    {
+                        constraint.elements[k].tangent_part.r[2] = 2.0
+                            * (constraint.elements[k].tangent_part.gcross1[0]
+                                .gdot(constraint.elements[k].tangent_part.gcross1[1])
+                                + constraint.elements[k].tangent_part.gcross2[0]
+                                    .gdot(constraint.elements[k].tangent_part.gcross2[1]));
                     }
                 }
             }

src/dynamics/solver/velocity_constraint_element.rs

@@ -12,7 +12,10 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
     pub impulse: na::Vector1<N>,
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
-    pub r: [N; DIM - 1],
+    #[cfg(feature = "dim2")]
+    pub r: [N; 1],
+    #[cfg(feature = "dim3")]
+    pub r: [N; DIM],
 }
 
 impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
@@ -22,7 +25,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
             gcross2: [na::zero(); DIM - 1],
             rhs: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            r: [na::zero(); DIM - 1],
+            #[cfg(feature = "dim2")]
+            r: [na::zero(); 1],
+            #[cfg(feature = "dim3")]
+            r: [na::zero(); DIM],
         }
     }
 
@@ -41,12 +47,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
     {
         #[cfg(feature = "dim2")]
         {
-            let dimpulse = tangents1[0].dot(&mj_lambda1.linear)
+            let dvel = tangents1[0].dot(&mj_lambda1.linear)
                 + self.gcross1[0].gdot(mj_lambda1.angular)
                 - tangents1[0].dot(&mj_lambda2.linear)
                 + self.gcross2[0].gdot(mj_lambda2.angular)
                 + self.rhs[0];
-            let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
             let dlambda = new_impulse - self.impulse[0];
             self.impulse[0] = new_impulse;
 
@@ -59,25 +65,30 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
 
         #[cfg(feature = "dim3")]
         {
-            let dimpulse_0 = tangents1[0].dot(&mj_lambda1.linear)
+            let dvel_0 = tangents1[0].dot(&mj_lambda1.linear)
                 + self.gcross1[0].gdot(mj_lambda1.angular)
                 - tangents1[0].dot(&mj_lambda2.linear)
                 + self.gcross2[0].gdot(mj_lambda2.angular)
                 + self.rhs[0];
-            let dimpulse_1 = tangents1[1].dot(&mj_lambda1.linear)
+            let dvel_1 = tangents1[1].dot(&mj_lambda1.linear)
                 + self.gcross1[1].gdot(mj_lambda1.angular)
                 - tangents1[1].dot(&mj_lambda2.linear)
                 + self.gcross2[1].gdot(mj_lambda2.angular)
                 + self.rhs[1];
 
-            let new_impulse = na::Vector2::new(
+            let dvel_00 = dvel_0 * dvel_0;
-                self.impulse[0] - self.r[0] * dimpulse_0,
+            let dvel_11 = dvel_1 * dvel_1;
-                self.impulse[1] - self.r[1] * dimpulse_1,
+            let dvel_01 = dvel_0 * dvel_1;
-            );
+            let inv_lhs = (dvel_00 + dvel_11)
+                * crate::utils::simd_inv(
+                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
+                );
+            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
+            let new_impulse = self.impulse - delta_impulse;
             let new_impulse = {
                 let _disable_fe_except =
-                    crate::utils::DisableFloatingPointExceptionsFlags::
+                        crate::utils::DisableFloatingPointExceptionsFlags::
-                    disable_floating_point_exceptions();
+                        disable_floating_point_exceptions();
                 new_impulse.simd_cap_magnitude(limit)
             };
 
@@ -128,11 +139,11 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
     ) where
         AngVector<N>: WDot<AngVector<N>, Result = N>,
     {
-        let dimpulse = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
+        let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
             - dir1.dot(&mj_lambda2.linear)
             + self.gcross2.gdot(mj_lambda2.angular)
             + self.rhs;
-        let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
+        let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 

src/dynamics/solver/velocity_constraint_wide.rs

@@ -49,6 +49,7 @@ impl WVelocityConstraint {
         let inv_dt = SimdReal::splat(params.inv_dt());
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
+        let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
 
         let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
@@ -136,14 +137,14 @@ impl WVelocityConstraint {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
                     let imsum = im1 + im2;
-                    let r = SimdReal::splat(1.0)
+                    let r = delassus_inv_factor
                         / (force_dir1.dot(&imsum.component_mul(&force_dir1))
                             + gcross1.gdot(gcross1)
                             + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
-                    rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
+                    rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
                     let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
@@ -165,16 +166,28 @@ impl WVelocityConstraint {
                     let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
                     let imsum = im1 + im2;
-                    let r = SimdReal::splat(1.0)
+                    let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
-                        / (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+                        + gcross1.gdot(gcross1)
-                            + gcross1.gdot(gcross1)
+                        + gcross2.gdot(gcross2);
-                            + gcross2.gdot(gcross2));
                     let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_part.gcross1[j] = gcross1;
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                     constraint.elements[k].tangent_part.rhs[j] = rhs;
-                    constraint.elements[k].tangent_part.r[j] = r;
+                    constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
+                        SimdReal::splat(1.0) / r
+                    } else {
+                        r
+                    };
+                }
+
+                #[cfg(feature = "dim3")]
+                {
+                    constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0)
+                        * (constraint.elements[k].tangent_part.gcross1[0]
+                            .gdot(constraint.elements[k].tangent_part.gcross1[1])
+                            + constraint.elements[k].tangent_part.gcross2[0]
+                                .gdot(constraint.elements[k].tangent_part.gcross2[1]));
                 }
             }
 

src/dynamics/solver/velocity_ground_constraint.rs

@@ -153,7 +153,7 @@ impl VelocityGroundConstraint {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = 1.0
+                    let r = params.delassus_inv_factor
                         / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
                             + gcross2.gdot(gcross2));
 
@@ -162,8 +162,7 @@ impl VelocityGroundConstraint {
 
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
-                    rhs_wo_bias +=
+                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                        (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
                     let rhs_bias = /* is_resting
                         * */ erp_inv_dt
@@ -186,17 +185,24 @@ impl VelocityGroundConstraint {
                         let gcross2 = mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
-                        let r = 1.0
+                        let r = tangents1[j]
-                            / (tangents1[j]
+                            .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
-                                .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+                            + gcross2.gdot(gcross2);
-                                + gcross2.gdot(gcross2));
                         let rhs = (vel1 - vel2
                             + flipped_multiplier * manifold_point.tangent_velocity)
                             .dot(&tangents1[j]);
 
                         constraint.elements[k].tangent_part.gcross2[j] = gcross2;
                         constraint.elements[k].tangent_part.rhs[j] = rhs;
-                        constraint.elements[k].tangent_part.r[j] = r;
+                        constraint.elements[k].tangent_part.r[j] =
+                            if cfg!(feature = "dim2") { 1.0 / r } else { r };
+                    }
+
+                    #[cfg(feature = "dim3")]
+                    {
+                        constraint.elements[k].tangent_part.r[2] = 2.0
+                            * constraint.elements[k].tangent_part.gcross2[0]
+                                .gdot(constraint.elements[k].tangent_part.gcross2[1]);
                     }
                 }
             }

src/dynamics/solver/velocity_ground_constraint_element.rs

@@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
     pub impulse: na::Vector1<N>,
     #[cfg(feature = "dim3")]
     pub impulse: na::Vector2<N>,
-    pub r: [N; DIM - 1],
+    #[cfg(feature = "dim2")]
+    pub r: [N; 1],
+    #[cfg(feature = "dim3")]
+    pub r: [N; DIM],
 }
 
 impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross2: [na::zero(); DIM - 1],
             rhs: [na::zero(); DIM - 1],
             impulse: na::zero(),
-            r: [na::zero(); DIM - 1],
+            #[cfg(feature = "dim2")]
+            r: [na::zero(); 1],
+            #[cfg(feature = "dim3")]
+            r: [na::zero(); DIM],
         }
     }
 
@@ -38,10 +43,10 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
     {
         #[cfg(feature = "dim2")]
         {
-            let dimpulse = -tangents1[0].dot(&mj_lambda2.linear)
+            let dvel = -tangents1[0].dot(&mj_lambda2.linear)
                 + self.gcross2[0].gdot(mj_lambda2.angular)
                 + self.rhs[0];
-            let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
+            let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
             let dlambda = new_impulse - self.impulse[0];
             self.impulse[0] = new_impulse;
 
@@ -51,17 +56,22 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
 
         #[cfg(feature = "dim3")]
         {
-            let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear)
+            let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
                 + self.gcross2[0].gdot(mj_lambda2.angular)
                 + self.rhs[0];
-            let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear)
+            let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
                 + self.gcross2[1].gdot(mj_lambda2.angular)
                 + self.rhs[1];
 
-            let new_impulse = na::Vector2::new(
+            let dvel_00 = dvel_0 * dvel_0;
-                self.impulse[0] - self.r[0] * dimpulse_0,
+            let dvel_11 = dvel_1 * dvel_1;
-                self.impulse[1] - self.r[1] * dimpulse_1,
+            let dvel_01 = dvel_0 * dvel_1;
-            );
+            let inv_lhs = (dvel_00 + dvel_11)
+                * crate::utils::simd_inv(
+                    dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
+                );
+            let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
+            let new_impulse = self.impulse - delta_impulse;
             let new_impulse = {
                 let _disable_fe_except =
                     crate::utils::DisableFloatingPointExceptionsFlags::
@@ -69,7 +79,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
                 new_impulse.simd_cap_magnitude(limit)
             };
             let dlambda = new_impulse - self.impulse;
-
             self.impulse = new_impulse;
 
             mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
@@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
 }
 
 impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     fn zero() -> Self {
         Self {
             gcross2: na::zero(),
@@ -105,9 +113,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
     where
         AngVector<N>: WDot<AngVector<N>, Result = N>,
     {
-        let dimpulse =
+        let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
-            -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
+        let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
-        let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
@@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
 }
 
 impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
-    #[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
     pub fn zero() -> Self {
         Self {
             normal_part: VelocityGroundConstraintNormalPart::zero(),

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -44,6 +44,7 @@ impl WVelocityGroundConstraint {
         let inv_dt = SimdReal::splat(params.inv_dt());
         let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
+        let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
 
         let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
@@ -142,12 +143,12 @@ impl WVelocityGroundConstraint {
                 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = SimdReal::splat(1.0)
+                    let r = delassus_inv_factor
                         / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
                     let projected_velocity = (vel1 - vel2).dot(&force_dir1);
                     let mut rhs_wo_bias =
                         (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
-                    rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
+                    rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
                     let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
@@ -166,14 +167,24 @@ impl WVelocityGroundConstraint {
 
                 for j in 0..DIM - 1 {
                     let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
-                    let r = SimdReal::splat(1.0)
+                    let r =
-                        / (tangents1[j].dot(&im2.component_mul(&tangents1[j]))
+                        tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2);
-                            + gcross2.gdot(gcross2));
                     let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
 
                     constraint.elements[k].tangent_part.gcross2[j] = gcross2;
-                    constraint.elements[k].tangent_part.r[j] = r;
                     constraint.elements[k].tangent_part.rhs[j] = rhs;
+                    constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
+                        SimdReal::splat(1.0) / r
+                    } else {
+                        r
+                    };
+                }
+
+                #[cfg(feature = "dim3")]
+                {
+                    constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0)
+                        * constraint.elements[k].tangent_part.gcross2[0]
+                            .gdot(constraint.elements[k].tangent_part.gcross2[1]);
                 }
             }
 

src/dynamics/solver/velocity_solver.rs

@@ -1,6 +1,6 @@
 use super::AnyJointVelocityConstraint;
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
-use crate::dynamics::solver::GenericVelocityConstraint;
+use crate::dynamics::solver::AnyGenericVelocityConstraint;
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
     IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType,
@@ -36,7 +36,7 @@ impl VelocitySolver {
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
         contact_constraints: &mut [AnyVelocityConstraint],
-        generic_contact_constraints: &mut [GenericVelocityConstraint],
+        generic_contact_constraints: &mut [AnyGenericVelocityConstraint],
         generic_contact_jacobians: &DVector<Real>,
         joint_constraints: &mut [AnyJointVelocityConstraint],
         generic_joint_jacobians: &DVector<Real>,

src/pipeline/physics_pipeline.rs

@@ -179,6 +179,7 @@ impl PhysicsPipeline {
     {
         self.counters.stages.island_construction_time.resume();
         islands.update_active_set_with_contacts(
+            integration_parameters.dt,
             bodies,
             colliders,
             narrow_phase,

src_testbed/camera2d.rs

@@ -39,10 +39,8 @@ impl OrbitCameraPlugin {
         mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
     ) {
         for (camera, mut transform) in query.iter_mut() {
-            if camera.enabled {
+            transform.translation = camera.center;
-                transform.translation = camera.center;
+            transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
-                transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
-            }
         }
     }
 

src_testbed/camera3d.rs

@@ -50,12 +50,10 @@ impl OrbitCameraPlugin {
         mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
     ) {
         for (camera, mut transform) in query.iter_mut() {
-            if camera.enabled {
+            let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
-                let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
+                * Quat::from_axis_angle(-Vec3::X, camera.y);
-                    * Quat::from_axis_angle(-Vec3::X, camera.y);
+            transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
-                transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
+            transform.look_at(camera.center, Vec3::Y);
-                transform.look_at(camera.center, Vec3::Y);
-            }
         }
     }
 

src_testbed/graphics.rs

@@ -2,10 +2,10 @@ use bevy::prelude::*;
 
 use na::{point, Point3};
 
-use crate::math::Isometry;
 use crate::objects::node::EntityWithGraphics;
 use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
+use rapier::math::{Isometry, Real};
 //use crate::objects::capsule::Capsule;
 //#[cfg(feature = "dim3")]
 //use crate::objects::mesh::Mesh;
@@ -301,8 +301,8 @@ impl GraphicsManager {
         handle: Option<ColliderHandle>,
         shape: &dyn Shape,
         sensor: bool,
-        pos: &Isometry<f32>,
+        pos: &Isometry<Real>,
-        delta: &Isometry<f32>,
+        delta: &Isometry<Real>,
         color: Point3<f32>,
         out: &mut Vec<EntityWithGraphics>,
     ) {
@@ -347,18 +347,24 @@ impl GraphicsManager {
         _bodies: &RigidBodySet,
         colliders: &ColliderSet,
         components: &mut Query<(&mut Transform,)>,
+        _materials: &mut Assets<BevyMaterial>,
     ) {
         for (_, ns) in self.b2sn.iter_mut() {
             for n in ns.iter_mut() {
-                // if let Some(co) = colliders.get(n.collider()) {
+                // if let Some(bo) = n
-                //     let bo = &_bodies[co.parent()];
+                //     .collider
-                //
+                //     .and_then(|h| bodies.get(colliders.get(h)?.parent()?))
-                //     if bo.is_dynamic() {
+                // {
-                //         if bo.is_ccd_active() {
+                //     if bo.activation().time_since_can_sleep
-                //             n.set_color(point![1.0, 0.0, 0.0]);
+                //         >= RigidBodyActivation::default_time_until_sleep()
-                //         } else {
+                //     {
-                //             n.set_color(point![0.0, 1.0, 0.0]);
+                //         n.set_color(materials, point![1.0, 0.0, 0.0]);
-                //         }
+                //     }
+                //     /* else if bo.activation().energy < bo.activation().threshold {
+                //         n.set_color(materials, point![0.0, 0.0, 1.0]);
+                //     } */
+                //     else {
+                //         n.set_color(materials, point![0.0, 1.0, 0.0]);
                 //     }
                 // }
 

src_testbed/harness/mod.rs

@@ -8,7 +8,7 @@ use rapier::dynamics::{
     RigidBodySet,
 };
 use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
-use rapier::math::Vector;
+use rapier::math::{Real, Vector};
 use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
 pub mod plugin;
@@ -131,7 +131,7 @@ impl Harness {
         colliders: ColliderSet,
         impulse_joints: ImpulseJointSet,
         multibody_joints: MultibodyJointSet,
-        gravity: Vector<f32>,
+        gravity: Vector<Real>,
         hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         // println!("Num bodies: {}", bodies.len());
@@ -235,7 +235,7 @@ impl Harness {
 
         self.events.poll_all();
 
-        self.state.time += self.physics.integration_parameters.dt;
+        self.state.time += self.physics.integration_parameters.dt as f32;
         self.state.timestep_id += 1;
     }
 

src_testbed/lib.rs

@@ -1,12 +1,4 @@
 extern crate nalgebra as na;
-#[cfg(feature = "dim2")]
-extern crate parry2d as parry;
-#[cfg(feature = "dim3")]
-extern crate parry3d as parry;
-#[cfg(feature = "dim2")]
-extern crate rapier2d as rapier;
-#[cfg(feature = "dim3")]
-extern crate rapier3d as rapier;
 
 #[macro_use]
 extern crate bitflags;

src_testbed/objects/node.rs

@@ -10,7 +10,7 @@ use bevy::render::render_resource::PrimitiveTopology;
 use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{Cone, Cylinder};
-use rapier::math::Isometry;
+use rapier::math::{Isometry, Real};
 
 use crate::graphics::BevyMaterial;
 #[cfg(feature = "dim2")]
@@ -26,7 +26,7 @@ pub struct EntityWithGraphics {
     pub color: Point3<f32>,
     pub base_color: Point3<f32>,
     pub collider: Option<ColliderHandle>,
-    pub delta: Isometry<f32>,
+    pub delta: Isometry<Real>,
     pub opacity: f32,
     material: Handle<BevyMaterial>,
 }
@@ -39,8 +39,8 @@ impl EntityWithGraphics {
         prefab_meshs: &HashMap<ShapeType, Handle<Mesh>>,
         shape: &dyn Shape,
         collider: Option<ColliderHandle>,
-        collider_pos: Isometry<f32>,
+        collider_pos: Isometry<Real>,
-        delta: Isometry<f32>,
+        delta: Isometry<Real>,
         color: Point3<f32>,
         sensor: bool,
     ) -> Self {
@@ -56,16 +56,16 @@ impl EntityWithGraphics {
         let bevy_color = Color::rgba(color.x, color.y, color.z, opacity);
         let shape_pos = collider_pos * delta;
         let mut transform = Transform::from_scale(scale);
-        transform.translation.x = shape_pos.translation.vector.x;
+        transform.translation.x = shape_pos.translation.vector.x as f32;
-        transform.translation.y = shape_pos.translation.vector.y;
+        transform.translation.y = shape_pos.translation.vector.y as f32;
         #[cfg(feature = "dim3")]
         {
-            transform.translation.z = shape_pos.translation.vector.z;
+            transform.translation.z = shape_pos.translation.vector.z as f32;
             transform.rotation = Quat::from_xyzw(
-                shape_pos.rotation.i,
+                shape_pos.rotation.i as f32,
-                shape_pos.rotation.j,
+                shape_pos.rotation.j as f32,
-                shape_pos.rotation.k,
+                shape_pos.rotation.k as f32,
-                shape_pos.rotation.w,
+                shape_pos.rotation.w as f32,
             );
         }
         #[cfg(feature = "dim2")]
@@ -73,7 +73,7 @@ impl EntityWithGraphics {
             if sensor {
                 transform.translation.z = -10.0;
             }
-            transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle());
+            transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle() as f32);
         }
 
         #[cfg(feature = "dim2")]
@@ -172,21 +172,21 @@ impl EntityWithGraphics {
         if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
             if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
                 let co_pos = co.position() * self.delta;
-                pos.translation.x = co_pos.translation.vector.x;
+                pos.translation.x = co_pos.translation.vector.x as f32;
-                pos.translation.y = co_pos.translation.vector.y;
+                pos.translation.y = co_pos.translation.vector.y as f32;
                 #[cfg(feature = "dim3")]
                 {
-                    pos.translation.z = co_pos.translation.vector.z;
+                    pos.translation.z = co_pos.translation.vector.z as f32;
                     pos.rotation = Quat::from_xyzw(
-                        co_pos.rotation.i,
+                        co_pos.rotation.i as f32,
-                        co_pos.rotation.j,
+                        co_pos.rotation.j as f32,
-                        co_pos.rotation.k,
+                        co_pos.rotation.k as f32,
-                        co_pos.rotation.w,
+                        co_pos.rotation.w as f32,
                     );
                 }
                 #[cfg(feature = "dim2")]
                 {
-                    pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle());
+                    pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle() as f32);
                 }
             }
         }
@@ -266,7 +266,7 @@ impl EntityWithGraphics {
 }
 
 #[cfg(feature = "dim2")]
-fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
+fn bevy_mesh_from_polyline(vertices: Vec<Point2<Real>>) -> Mesh {
     let n = vertices.len();
     let idx = (1..n as u32 - 1).map(|i| [0, i, i + 1]).collect();
     let vtx = vertices
@@ -277,7 +277,7 @@ fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
 }
 
 #[cfg(feature = "dim2")]
-fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
+fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
     let (vtx, idx) = buffers;
     // let mut normals: Vec<[f32; 3]> = vec![];
     let mut vertices: Vec<[f32; 3]> = vec![];
@@ -287,11 +287,11 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
             let a = vtx[idx[0] as usize];
             let b = vtx[idx[1] as usize];
 
-            vertices.push([a.x, a.y, 0.0]);
+            vertices.push([a.x as f32, a.y as f32, 0.0]);
-            vertices.push([b.x, b.y, 0.0]);
+            vertices.push([b.x as f32, b.y as f32, 0.0]);
         }
     } else {
-        vertices = vtx.iter().map(|v| [v.x, v.y, 0.0]).collect();
+        vertices = vtx.iter().map(|v| [v.x as f32, v.y as f32, 0.0]).collect();
     }
 
     let indices: Vec<_> = (0..vertices.len() as u32).collect();
@@ -310,7 +310,7 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
     mesh
 }
 
-fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
+fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
     let (vtx, idx) = buffers;
     let mut normals: Vec<[f32; 3]> = vec![];
     let mut vertices: Vec<[f32; 3]> = vec![];
@@ -320,9 +320,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
         let b = vtx[idx[1] as usize];
         let c = vtx[idx[2] as usize];
 
-        vertices.push(a.into());
+        vertices.push(a.cast::<f32>().into());
-        vertices.push(b.into());
+        vertices.push(b.cast::<f32>().into());
-        vertices.push(c.into());
+        vertices.push(c.cast::<f32>().into());
     }
 
     for vtx in vertices.chunks(3) {
@@ -330,9 +330,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
         let b = Point3::from(vtx[1]);
         let c = Point3::from(vtx[2]);
         let n = (b - a).cross(&(c - a)).normalize();
-        normals.push(n.into());
+        normals.push(n.cast::<f32>().into());
-        normals.push(n.into());
+        normals.push(n.cast::<f32>().into());
-        normals.push(n.into());
+        normals.push(n.cast::<f32>().into());
     }
 
     normals
@@ -358,36 +358,36 @@ fn collider_mesh_scale(co_shape: &dyn Shape) -> Vec3 {
         #[cfg(feature = "dim2")]
         ShapeType::Cuboid => {
             let c = co_shape.as_cuboid().unwrap();
-            Vec3::new(c.half_extents.x, c.half_extents.y, 1.0)
+            Vec3::new(c.half_extents.x as f32, c.half_extents.y as f32, 1.0)
         }
         ShapeType::Ball => {
             let b = co_shape.as_ball().unwrap();
-            Vec3::new(b.radius, b.radius, b.radius)
+            Vec3::new(b.radius as f32, b.radius as f32, b.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::Cuboid => {
             let c = co_shape.as_cuboid().unwrap();
-            Vec3::from_slice(c.half_extents.as_slice())
+            Vec3::from_slice(c.half_extents.cast::<f32>().as_slice())
         }
         #[cfg(feature = "dim3")]
         ShapeType::Cylinder => {
             let c = co_shape.as_cylinder().unwrap();
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::RoundCylinder => {
             let c = &co_shape.as_round_cylinder().unwrap().base_shape;
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::Cone => {
             let c = co_shape.as_cone().unwrap();
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         #[cfg(feature = "dim3")]
         ShapeType::RoundCone => {
             let c = &co_shape.as_round_cone().unwrap().base_shape;
-            Vec3::new(c.radius, c.half_height, c.radius)
+            Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
         }
         _ => Vec3::ONE,
     }

src_testbed/physics/mod.rs

@@ -4,7 +4,7 @@ use rapier::dynamics::{
     RigidBodySet,
 };
 use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
-use rapier::math::Vector;
+use rapier::math::{Real, Vector};
 use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
 
 pub struct PhysicsSnapshot {
@@ -82,7 +82,7 @@ pub struct PhysicsState {
     pub pipeline: PhysicsPipeline,
     pub query_pipeline: QueryPipeline,
     pub integration_parameters: IntegrationParameters,
-    pub gravity: Vector<f32>,
+    pub gravity: Vector<Real>,
     pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>,
 }
 

src_testbed/testbed.rs

@@ -16,7 +16,7 @@ use rapier::dynamics::{
 use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
 #[cfg(feature = "dim3")]
 use rapier::geometry::{InteractionGroups, Ray};
-use rapier::math::Vector;
+use rapier::math::{Real, Vector};
 use rapier::pipeline::PhysicsHooks;
 
 #[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -487,7 +487,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         colliders: ColliderSet,
         impulse_joints: ImpulseJointSet,
         multibody_joints: MultibodyJointSet,
-        gravity: Vector<f32>,
+        gravity: Vector<Real>,
         hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
     ) {
         self.harness.set_world_with_params(
@@ -1129,12 +1129,15 @@ fn update_testbed(
         {
             if state.flags.contains(TestbedStateFlags::SLEEP) {
                 for (_, body) in harness.physics.bodies.iter_mut() {
-                    body.activation_mut().threshold = RigidBodyActivation::default_threshold();
+                    body.activation_mut().linear_threshold =
+                        RigidBodyActivation::default_linear_threshold();
+                    body.activation_mut().angular_threshold =
+                        RigidBodyActivation::default_angular_threshold();
                 }
             } else {
                 for (_, body) in harness.physics.bodies.iter_mut() {
                     body.wake_up(true);
-                    body.activation_mut().threshold = -1.0;
+                    body.activation_mut().linear_threshold = -1.0;
                 }
             }
         }
@@ -1226,6 +1229,7 @@ fn update_testbed(
         &harness.physics.bodies,
         &harness.physics.colliders,
         &mut gfx_components,
+        &mut *materials,
     );
 
     for plugin in &mut plugins.0 {
@@ -1299,14 +1303,14 @@ fn highlight_hovered_body(
         let ray_pt1 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
         let ray_pt2 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
         let ray_dir = ray_pt2 - ray_pt1;
-        let ray_origin = Point3::new(ray_pt1.x, ray_pt1.y, ray_pt1.z);
+        let ray_origin = Point3::new(ray_pt1.x as Real, ray_pt1.y as Real, ray_pt1.z as Real);
-        let ray_dir = Vector3::new(ray_dir.x, ray_dir.y, ray_dir.z);
+        let ray_dir = Vector3::new(ray_dir.x as Real, ray_dir.y as Real, ray_dir.z as Real);
 
         let ray = Ray::new(ray_origin, ray_dir);
         let hit = physics.query_pipeline.cast_ray(
             &physics.colliders,
             &ray,
-            f32::MAX,
+            Real::MAX,
             true,
             InteractionGroups::all(),
             None,

src_testbed/ui.rs

@@ -1,4 +1,5 @@
 use rapier::counters::Counters;
+use rapier::math::Real;
 
 use crate::harness::Harness;
 use crate::testbed::{
@@ -147,7 +148,7 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
         );
         let mut frequency = integration_parameters.inv_dt().round() as u32;
         ui.add(Slider::new(&mut frequency, 0..=240).text("frequency (Hz)"));
-        integration_parameters.set_inv_dt(frequency as f32);
+        integration_parameters.set_inv_dt(frequency as Real);
 
         let mut sleep = state.flags.contains(TestbedStateFlags::SLEEP);
         // let mut contact_points = state.flags.contains(TestbedStateFlags::CONTACT_POINTS);