Merge pull request #277 from dimforge/solver-fixes

Fix some solver issues

43 files changed, 936 insertions, 229 deletions

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