Implement sleeping

src/bundles.rs

@@ -54,6 +54,8 @@ pub struct RigidBodyBundle {
     pub inertia: Inertia,
     pub inv_inertia: InvInertia,
     pub local_center_of_mass: LocalCom,
+
+    pub time_sleeping: TimeSleeping,
 }
 
 impl RigidBodyBundle {

src/components/mod.rs

@@ -54,6 +54,35 @@ impl RigidBody {
     }
 }
 
+/// Indicates that a body is not simulated by the physics engine until woken up again.
+/// This is done to improve performance and to help prevent small jitter that is typically present in collisions.
+///
+/// Bodies are marked as sleeping when their linear and angular velocity is below the [`SleepingThreshold`] for a time
+/// indicated by [`DeactivationTime`]. A sleeping body is woken up when an active body interacts with it through
+/// collisions or other constraints, or when gravity changes, or when the body's
+/// position, rotation, velocity, or external forces are modified.
+///
+/// Note that sleeping can cause unrealistic behaviour in some cases.
+/// For example, removing the floor under sleeping bodies can leave them floating in the air.
+/// Sleeping can be disabled for specific entities with the [`SleepingDisabled`] component,
+/// or for all entities by setting the [`SleepingThreshold`] to a negative value.
+#[derive(Reflect, Clone, Copy, Component, Debug, Default, PartialEq, Eq, From)]
+#[reflect(Component)]
+pub struct Sleeping;
+
+/// How long the velocity of the body has been below the [`SleepingThreshold`],
+/// i.e. how long the body has been able to sleep.
+///
+/// See [`Sleeping`] for further information.
+#[derive(Reflect, Clone, Copy, Component, Debug, Default, PartialEq, From)]
+#[reflect(Component)]
+pub struct TimeSleeping(pub Scalar);
+
+/// Indicates that the body can not be deactivated by the physics engine. See [`Sleeping`] for information about sleeping.
+#[derive(Reflect, Clone, Copy, Component, Debug, Default, PartialEq, Eq, From)]
+#[reflect(Component)]
+pub struct SleepingDisabled;
+
 /// The position of a body.
 #[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, DerefMut, PartialEq, From)]
 #[reflect(Component)]
@@ -83,6 +112,10 @@ pub struct PrevPos(pub Vector);
 #[reflect(Component)]
 pub struct LinVel(pub Vector);
 
+impl LinVel {
+    pub const ZERO: LinVel = LinVel(Vector::ZERO);
+}
+
 #[cfg(all(feature = "2d", feature = "f64"))]
 impl From<Vec2> for LinVel {
     fn from(value: Vec2) -> Self {
@@ -114,6 +147,13 @@ pub struct AngVel(pub Scalar);
 #[reflect(Component)]
 pub struct AngVel(pub Vector);
 
+impl AngVel {
+    #[cfg(feature = "2d")]
+    pub const ZERO: AngVel = AngVel(0.0);
+    #[cfg(feature = "3d")]
+    pub const ZERO: AngVel = AngVel(Vector::ZERO);
+}
+
 #[cfg(all(feature = "3d", feature = "f64"))]
 impl From<Vec3> for AngVel {
     fn from(value: Vec3) -> Self {

src/lib.rs

@@ -85,6 +85,7 @@ struct FixedUpdateSet;
 ///         - Constraint projection ([`SolverPlugin`], [`SubsteppingSet::SolvePos`])
 ///         - Velocity updates ([`SolverPlugin`], [`SubsteppingSet::UpdateVel`])
 ///         - Velocity solve ([`SolverPlugin`], [`SubsteppingSet::SolveVel`])
+///     - Control physics sleeping ([`SleepingPlugin`], [`PhysicsSet::Sleeping`])
 ///     - Synchronize physics with Bevy ([`SyncPlugin`], [`PhysicsSet::Sync`])
 pub struct XpbdPlugin;
 
@@ -105,16 +106,23 @@ impl Plugin for XpbdPlugin {
             .init_resource::<SubDeltaTime>()
             .init_resource::<NumSubsteps>()
             .init_resource::<NumPosIters>()
+            .init_resource::<SleepingThreshold>()
+            .init_resource::<DeactivationTime>()
             .init_resource::<XpbdLoop>()
             .init_resource::<Gravity>()
             .register_type::<PhysicsTimestep>()
             .register_type::<DeltaTime>()
             .register_type::<SubDeltaTime>()
             .register_type::<NumSubsteps>()
             .register_type::<NumPosIters>()
+            .register_type::<SleepingThreshold>()
+            .register_type::<DeactivationTime>()
             .register_type::<XpbdLoop>()
             .register_type::<Gravity>()
             .register_type::<RigidBody>()
+            .register_type::<Sleeping>()
+            .register_type::<SleepingDisabled>()
+            .register_type::<TimeSleeping>()
             .register_type::<Pos>()
             .register_type::<Rot>()
             .register_type::<PrevPos>()
@@ -145,6 +153,7 @@ impl Plugin for XpbdPlugin {
                 PhysicsSet::Prepare,
                 PhysicsSet::BroadPhase,
                 PhysicsSet::Substeps,
+                PhysicsSet::Sleeping,
                 PhysicsSet::Sync,
             )
                 .chain(),
@@ -180,6 +189,7 @@ impl Plugin for XpbdPlugin {
             .add_plugin(BroadPhasePlugin)
             .add_plugin(IntegratorPlugin)
             .add_plugin(SolverPlugin)
+            .add_plugin(SleepingPlugin)
             .add_plugin(SyncPlugin);
 
         #[cfg(feature = "debug-render-aabbs")]

src/resources.rs

@@ -57,6 +57,43 @@ impl Default for NumPosIters {
     }
 }
 
+/// A threshold that indicates the maximum linear and angular velocity allowed for a body to be deactivated.
+///
+/// Setting a negative sleeping threshold disables sleeping entirely.
+///
+/// See [`Sleeping`] for further information about sleeping.
+#[derive(Reflect, Resource, Clone, Copy, PartialEq, PartialOrd, Debug)]
+#[reflect(Resource)]
+pub struct SleepingThreshold {
+    /// The maximum linear velocity allowed for a body to be marked as sleeping.
+    pub linear: Scalar,
+    /// The maximum angular velocity allowed for a body to be marked as sleeping.
+    pub angular: Scalar,
+}
+
+impl Default for SleepingThreshold {
+    fn default() -> Self {
+        Self {
+            linear: 0.1,
+            angular: 0.2,
+        }
+    }
+}
+
+/// How long in seconds the linear and angular velocity of a body need to be below
+/// the [`SleepingThreshold`] before the body is deactivated. Defaults to 1 second.
+///
+/// See [`Sleeping`] for further information about sleeping.
+#[derive(Reflect, Resource, Clone, Copy, PartialEq, PartialOrd, Debug)]
+#[reflect(Resource)]
+pub struct DeactivationTime(pub Scalar);
+
+impl Default for DeactivationTime {
+    fn default() -> Self {
+        Self(1.0)
+    }
+}
+
 /// The global gravitational acceleration. This is applied to dynamic bodies in the integration step.
 ///
 /// The default is an acceleration of 9.81 m/s^2 pointing down, which is approximate to the gravitational acceleration near Earth's surface.

src/steps/integrator.rs

@@ -14,16 +14,18 @@ impl Plugin for IntegratorPlugin {
     }
 }
 
+type PosIntegrationComponents = (
+    &'static RigidBody,
+    &'static mut Pos,
+    &'static mut PrevPos,
+    &'static mut LinVel,
+    &'static ExternalForce,
+    &'static Mass,
+);
+
 /// Explicitly integrates the positions and linear velocities of bodies taking only external forces like gravity into account. This acts as a prediction for the next positions of the bodies.
 fn integrate_pos(
-    mut bodies: Query<(
-        &RigidBody,
-        &mut Pos,
-        &mut PrevPos,
-        &mut LinVel,
-        &ExternalForce,
-        &Mass,
-    )>,
+    mut bodies: Query<PosIntegrationComponents, Without<Sleeping>>,
     gravity: Res<Gravity>,
     sub_dt: Res<SubDeltaTime>,
 ) {
@@ -45,20 +47,25 @@ fn integrate_pos(
     }
 }
 
+type RotIntegrationComponents = (
+    &'static RigidBody,
+    &'static mut Rot,
+    &'static mut PrevRot,
+    &'static mut AngVel,
+    &'static ExternalTorque,
+    &'static Inertia,
+    &'static InvInertia,
+);
+
 /// Explicitly integrates the rotations and angular velocities of bodies taking only external torque into account. This acts as a prediction for the next rotations of the bodies.
 #[cfg(feature = "2d")]
 fn integrate_rot(
-    mut bodies: Query<(
-        &RigidBody,
-        &mut Rot,
-        &mut PrevRot,
-        &mut AngVel,
-        &ExternalTorque,
-        &InvInertia,
-    )>,
+    mut bodies: Query<RotIntegrationComponents, Without<Sleeping>>,
     sub_dt: Res<SubDeltaTime>,
 ) {
-    for (rb, mut rot, mut prev_rot, mut ang_vel, external_torque, inv_inertia) in &mut bodies {
+    for (rb, mut rot, mut prev_rot, mut ang_vel, external_torque, _inertia, inv_inertia) in
+        &mut bodies
+    {
         prev_rot.0 = *rot;
 
         if rb.is_static() {
@@ -77,15 +84,7 @@ fn integrate_rot(
 /// Explicitly integrates the rotations and angular velocities of bodies taking only external torque into account. This acts as a prediction for the next rotations of the bodies.
 #[cfg(feature = "3d")]
 fn integrate_rot(
-    mut bodies: Query<(
-        &RigidBody,
-        &mut Rot,
-        &mut PrevRot,
-        &mut AngVel,
-        &ExternalTorque,
-        &Inertia,
-        &InvInertia,
-    )>,
+    mut bodies: Query<RotIntegrationComponents, Without<Sleeping>>,
     sub_dt: Res<SubDeltaTime>,
 ) {
     for (rb, mut rot, mut prev_rot, mut ang_vel, external_torque, inertia, inv_inertia) in

src/steps/mod.rs

@@ -3,12 +3,14 @@
 pub mod broad_phase;
 pub mod integrator;
 pub mod prepare;
+pub mod sleeping;
 pub mod solver;
 pub mod sync;
 
 pub use broad_phase::BroadPhasePlugin;
 pub use integrator::IntegratorPlugin;
 pub use prepare::PreparePlugin;
+pub use sleeping::SleepingPlugin;
 pub use solver::SolverPlugin;
 pub use sync::SyncPlugin;
 
@@ -25,8 +27,10 @@ pub enum PhysicsSet {
     ///
     /// The broad phase speeds up collision detection, as the number of accurate collision checks is greatly reduced.
     BroadPhase,
-    /// Substepping is an inner loop inside a physics step, see [`SubsteppingSet`] and [`XpbdSubstepSchedule`]
+    /// Substepping is an inner loop inside a physics step. See [`SubsteppingSet`] and [`XpbdSubstepSchedule`].
     Substeps,
+    /// The sleeping step controls when bodies are active. This improves performance and helps prevent jitter. See [`Sleeping`].
+    Sleeping,
     /// In the sync step, Bevy [`Transform`]s are synchronized with the physics world.
     Sync,
 }

src/steps/sleeping.rs

@@ -0,0 +1,109 @@
+//! Controls when bodies are active. This improves performance and helps prevent jitter. See [`Sleeping`].
+
+use crate::prelude::*;
+use bevy::prelude::*;
+
+/// Controls when bodies are active. This improves performance and helps prevent jitter.
+///
+/// Bodies are marked as [`Sleeping`] when their linear and angular velocities are below the [`SleepingThreshold`] for a duration indicated by [`DeactivationTime`].
+///
+/// Bodies are woken up when an active body or constraint interacts with them, or when gravity changes, or when the body's position, rotation, velocity, or external forces are changed.
+pub struct SleepingPlugin;
+
+impl Plugin for SleepingPlugin {
+    fn build(&self, app: &mut bevy::prelude::App) {
+        app.get_schedule_mut(XpbdSchedule)
+            .expect("add xpbd schedule first")
+            .add_systems(
+                (mark_sleeping_bodies, wake_up_bodies, gravity_wake_up_bodies)
+                    .chain()
+                    .in_set(PhysicsSet::Sleeping),
+            );
+    }
+}
+
+type SleepingQueryComponents = (
+    Entity,
+    &'static RigidBody,
+    &'static mut LinVel,
+    &'static mut AngVel,
+    &'static mut TimeSleeping,
+);
+
+/// Adds the [`Sleeping`] component to bodies whose linear and anigular velocities have been
+/// under the [`SleepingThreshold`] for a duration indicated by [`DeactivationTime`].
+fn mark_sleeping_bodies(
+    mut commands: Commands,
+    mut bodies: Query<SleepingQueryComponents, (Without<Sleeping>, Without<SleepingDisabled>)>,
+    deactivation_time: Res<DeactivationTime>,
+    sleep_threshold: Res<SleepingThreshold>,
+    dt: Res<DeltaTime>,
+) {
+    for (entity, rb, mut lin_vel, mut ang_vel, mut time_sleeping) in &mut bodies {
+        // Only dynamic bodies can sleep.
+        if !rb.is_dynamic() {
+            continue;
+        }
+
+        let lin_vel_sq = lin_vel.length_squared();
+
+        #[cfg(feature = "2d")]
+        let ang_vel_sq = ang_vel.powi(2);
+        #[cfg(feature = "3d")]
+        let ang_vel_sq = ang_vel.dot(ang_vel.0);
+
+        // Negative thresholds indicate that sleeping is disabled.
+        let lin_sleeping_threshold_sq = sleep_threshold.linear * sleep_threshold.linear.abs();
+        let ang_sleeping_threshold_sq = sleep_threshold.angular * sleep_threshold.angular.abs();
+
+        // If linear and angular velocity are below the sleeping threshold,
+        // add delta time to the time sleeping, i.e. the time that the body has remained still.
+        if lin_vel_sq < lin_sleeping_threshold_sq && ang_vel_sq < ang_sleeping_threshold_sq {
+            time_sleeping.0 += dt.0;
+        } else {
+            time_sleeping.0 = 0.0;
+        }
+
+        // If the body has been still for long enough, set it to sleep and reset velocities.
+        if time_sleeping.0 > deactivation_time.0 {
+            commands.entity(entity).insert(Sleeping);
+            *lin_vel = LinVel::ZERO;
+            *ang_vel = AngVel::ZERO;
+        }
+    }
+}
+
+type BodyWokeUpFilter = Or<(
+    Changed<Pos>,
+    Changed<Rot>,
+    Changed<LinVel>,
+    Changed<AngVel>,
+    Changed<ExternalForce>,
+    Changed<ExternalTorque>,
+)>;
+
+/// Removes the [`Sleeping`] component from sleeping bodies when properties like
+/// position, rotation, velocity and external forces are changed.
+fn wake_up_bodies(
+    mut commands: Commands,
+    mut bodies: Query<(Entity, &mut TimeSleeping), (With<Sleeping>, BodyWokeUpFilter)>,
+) {
+    for (entity, mut time_sleeping) in &mut bodies {
+        commands.entity(entity).remove::<Sleeping>();
+        time_sleeping.0 = 0.0;
+    }
+}
+
+/// Removes the [`Sleeping`] component from sleeping bodies when [`Gravity`] is changed.
+fn gravity_wake_up_bodies(
+    mut commands: Commands,
+    mut bodies: Query<(Entity, &mut TimeSleeping), With<Sleeping>>,
+    gravity: Res<Gravity>,
+) {
+    if gravity.is_changed() {
+        for (entity, mut time_sleeping) in &mut bodies {
+            commands.entity(entity).remove::<Sleeping>();
+            time_sleeping.0 = 0.0;
+        }
+    }
+}