Merge pull request #68 from Jondolf/scheduling-improvements

Improve scheduling and system sets

crates/bevy_xpbd_2d/examples/chain_2d.rs

@@ -1,7 +1,7 @@
 #![allow(clippy::unnecessary_cast)]
 
 use bevy::{prelude::*, sprite::MaterialMesh2dBundle, window::PrimaryWindow};
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_2d/examples/collision_layers.rs

@@ -1,7 +1,7 @@
 #![allow(clippy::unnecessary_cast)]
 
 use bevy::{prelude::*, sprite::MaterialMesh2dBundle};
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_2d/examples/fixed_joint_2d.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_2d/examples/move_marbles.rs

@@ -1,7 +1,7 @@
 #![allow(clippy::unnecessary_cast)]
 
 use bevy::{prelude::*, sprite::MaterialMesh2dBundle};
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_2d/examples/prismatic_joint_2d.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_2d/examples/ray_caster.rs

@@ -6,7 +6,7 @@
 #![allow(clippy::unnecessary_cast)]
 
 use bevy::{prelude::*, sprite::MaterialMesh2dBundle};
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::*;
 
 fn main() {

crates/bevy_xpbd_2d/examples/revolute_joint_2d.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_2d::prelude::*;
+use bevy_xpbd_2d::{math::*, prelude::*};
 use examples_common_2d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_3d/examples/chain_3d.rs

@@ -1,7 +1,7 @@
 #![allow(clippy::unnecessary_cast)]
 
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*};
 use examples_common_3d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_3d/examples/cubes.rs

@@ -1,7 +1,7 @@
 #![allow(clippy::unnecessary_cast)]
 
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*};
 use examples_common_3d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_3d/examples/custom_broad_phase.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*, PhysicsSchedule, PhysicsStepSet};
 
 // Required for AABB intersection check. This might be abstracted away at some point.
 use bevy_xpbd_3d::parry::bounding_volume::BoundingVolume;
@@ -77,7 +77,7 @@ impl Plugin for BruteForceBroadPhasePlugin {
             .expect("add PhysicsSchedule first");
 
         // Add the broad phase system into the broad phase set
-        physics_schedule.add_systems(collect_collision_pairs.in_set(PhysicsSet::BroadPhase));
+        physics_schedule.add_systems(collect_collision_pairs.in_set(PhysicsStepSet::BroadPhase));
     }
 }
 

crates/bevy_xpbd_3d/examples/custom_constraint.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*, SubstepSchedule, SubstepSet};
 
 fn main() {
     let mut app = App::new();

crates/bevy_xpbd_3d/examples/fixed_joint_3d.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*};
 use examples_common_3d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_3d/examples/prismatic_joint_3d.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*};
 use examples_common_3d::XpbdExamplePlugin;
 
 fn main() {

crates/bevy_xpbd_3d/examples/revolute_joint_3d.rs

@@ -1,5 +1,5 @@
 use bevy::prelude::*;
-use bevy_xpbd_3d::prelude::*;
+use bevy_xpbd_3d::{math::*, prelude::*};
 use examples_common_3d::XpbdExamplePlugin;
 
 fn main() {

src/components/rotation.rs

@@ -64,8 +64,8 @@ pub struct Rotation(pub Quaternion);
 impl Rotation {
     /// Rotates the rotation by a 3D vector.
     #[cfg(feature = "2d")]
-    pub fn rotate_vec3(&self, vec: crate::Vector3) -> crate::Vector3 {
-        crate::Vector3::new(
+    pub fn rotate_vec3(&self, vec: Vector3) -> Vector3 {
+        Vector3::new(
             vec.x * self.cos() - vec.y * self.sin(),
             vec.x * self.sin() + vec.y * self.cos(),
             vec.z,

src/constraints/mod.rs

@@ -44,14 +44,15 @@
 //!     lagrange: f32,
 //! }
 //!
+//! #[cfg(feature = "f32")]
 //! impl XpbdConstraint<2> for CustomConstraint {
 //!     fn entities(&self) -> [Entity; 2] {
 //!         [self.entity1, self.entity2]
 //!     }
 //!     fn clear_lagrange_multipliers(&mut self) {
 //!         self.lagrange = 0.0;
 //!     }
-//!     fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) {
+//!     fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: f32) {
 //!         // Constraint solving logic goes here
 //!     }
 //! }

src/lib.rs

@@ -275,7 +275,7 @@
 //! [external torque](ExternalTorque).
 //!
 //! In Bevy XPBD, the simulation loop is handled by various plugins. The [`PhysicsSetupPlugin`] sets up
-//! the Bevy schedules[^1][^2] and sets[^3][^4], the [`BroadPhasePlugin`] manages the broad phase, the [`IntegratorPlugin`] handles
+//! the Bevy schedules[^1][^2] and sets[^3][^4][^5], the [`BroadPhasePlugin`] manages the broad phase, the [`IntegratorPlugin`] handles
 //! XPBD integration, and so on. You can find all of the plugins and their responsibilities [here](PhysicsPlugins).
 //!
 //! ### See also
@@ -317,7 +317,9 @@
 //!
 //! [^3]: [`PhysicsSet`]
 //!
-//! [^4]: [`SubstepSet`]
+//! [^4]: [`PhysicsStepSet`]
+//!
+//! [^5]: [`SubstepSet`]
 
 #![allow(rustdoc::invalid_rust_codeblocks)]
 #![warn(missing_docs)]
@@ -353,11 +355,11 @@ pub mod prelude {
         collision::*,
         components::*,
         constraints::{joints::*, *},
-        math::*,
         plugins::*,
         resources::*,
-        *,
+        PhysicsSet,
     };
+    pub(crate) use crate::{math::*, *};
     pub use bevy_xpbd_derive::*;
 }
 pub use prelude::setup::{pause, resume};
@@ -373,4 +375,123 @@ use bevy::{
     prelude::*,
 };
 use parry::math::Isometry;
-use prelude::*;
+
+/// Responsible for advancing the physics simulation. This is run in [`PhysicsSet::StepSimulation`].
+///
+/// See [`PhysicsStepSet`] for the system sets that are run in this schedule.
+#[derive(Debug, Hash, PartialEq, Eq, Clone, ScheduleLabel)]
+pub struct PhysicsSchedule;
+
+/// The substepping schedule that runs in [`PhysicsStepSet::Substeps`].
+/// The number of substeps per physics step is configured through the [`SubstepCount`] resource.
+///
+/// See [`SubstepSet`] for the system sets that are run in this schedule.
+#[derive(Debug, Hash, PartialEq, Eq, Clone, ScheduleLabel)]
+pub struct SubstepSchedule;
+
+/// High-level system sets for the main phases of the physics engine.
+/// You can use these to schedule your own systems before or after physics is run without
+/// having to worry about implementation details.
+///
+/// 1. `Prepare`: Responsible for initializing [rigid bodies](RigidBody) and [colliders](Collider) and
+/// updating several components.
+/// 2. `StepSimulation`: Responsible for advancing the simulation by running the steps in [`PhysicsStepSet`].
+/// 3. `Sync`: Responsible for synchronizing physics components with other data, like writing [`Position`]
+/// and [`Rotation`] components to `Transform`s.
+///
+/// ## See also
+///
+/// - [`PhysicsSchedule`]: Responsible for advancing the simulation in [`PhysicsSet::StepSimulation`].
+/// - [`PhysicsStepSet`]: System sets for the steps of the actual physics simulation loop, like
+/// the broad phase and the substepping loop.
+/// - [`SubstepSchedule`]: Responsible for running the substepping loop in [`PhysicsStepSet::Substeps`].
+/// - [`SubstepSet`]: System sets for the steps of the substepping loop, like position integration and
+/// the constraint solver.
+#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)]
+pub enum PhysicsSet {
+    /// Responsible for initializing [rigid bodies](RigidBody) and [colliders](Collider) and
+    /// updating several components.
+    ///
+    /// See [`PreparePlugin`].
+    Prepare,
+    /// Responsible for advancing the simulation by running the steps in [`PhysicsStepSet`].
+    /// Systems in this set are run in the [`PhysicsSchedule`].
+    StepSimulation,
+    /// Responsible for synchronizing physics components with other data, like writing [`Position`]
+    /// and [`Rotation`] components to `Transform`s.
+    ///
+    /// See [`SyncPlugin`].
+    Sync,
+}
+
+/// System sets for the main steps in the physics simulation loop. These are typically run in the [`PhysicsSchedule`].
+///
+/// 1. Broad phase
+/// 2. Substeps
+///     1. Integrate
+///     2. Solve positional and angular constraints
+///     3. Update velocities
+///     4. Solve velocity constraints (dynamic friction and restitution)
+/// 3. Sleeping
+/// 4. Spatial queries (ray casting and shape casting)
+#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)]
+pub enum PhysicsStepSet {
+    /// Responsible for collecting pairs of potentially colliding entities into [`BroadCollisionPairs`] using
+    /// [AABB](ColliderAabb) intersection tests.
+    ///
+    /// See [`BroadPhasePlugin`].
+    BroadPhase,
+    /// Responsible for substepping, which is an inner loop inside a physics step.
+    ///
+    /// See [`SubstepSet`] and [`SubstepSchedule`].
+    Substeps,
+    /// Responsible for controlling when bodies should be deactivated and marked as [`Sleeping`].
+    ///
+    /// See [`SleepingPlugin`].
+    Sleeping,
+    /// Responsible for spatial queries like [ray casting](`RayCaster`) and shape casting.
+    ///
+    /// See [`SpatialQueryPlugin`].
+    SpatialQuery,
+}
+
+/// System sets for the the steps in the inner substepping loop. These are typically run in the [`SubstepSchedule`].
+///
+/// 1. Integrate
+/// 2. Solve positional and angular constraints
+/// 3. Update velocities
+/// 4. Solve velocity constraints (dynamic friction and restitution)
+#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)]
+pub enum SubstepSet {
+    /// Responsible for integrating Newton's 2nd law of motion,
+    /// applying forces and moving entities according to their velocities.
+    ///
+    /// See [`IntegratorPlugin`].
+    Integrate,
+    /// The [solver] iterates through [constraints] and solves them.
+    /// This step is also responsible for narrow phase collision detection,
+    /// as it creates a [`PenetrationConstraint`] for each contact.
+    ///
+    /// **Note**: If you want to [create your own constraints](constraints#custom-constraints),
+    /// you should add them in [`SubstepSet::SolveUserConstraints`]
+    /// to avoid system order ambiguities.
+    ///
+    /// See [`SolverPlugin`].
+    SolveConstraints,
+    /// The [solver] iterates through custom [constraints] created by the user and solves them.
+    ///
+    /// You can [create new constraints](constraints#custom-constraints) by implementing [`XpbdConstraint`]
+    /// for a component and adding the [constraint system](solve_constraint) to this set.
+    ///
+    /// See [`SolverPlugin`].
+    SolveUserConstraints,
+    /// Responsible for updating velocities after [constraint](constraints) solving.
+    ///
+    /// See [`SolverPlugin`].
+    UpdateVelocities,
+    /// Responsible for applying dynamic friction, restitution and joint damping at the end of thei
+    /// substepping loop.
+    ///
+    /// See [`SolverPlugin`].
+    SolveVelocities,
+}

src/plugins/broad_phase.rs

@@ -12,7 +12,7 @@ use bevy::prelude::*;
 ///
 /// Currently, the broad phase uses the [sweep and prune](https://en.wikipedia.org/wiki/Sweep_and_prune) algorithm.
 ///
-/// The broad phase systems run in [`PhysicsSet::BroadPhase`].
+/// The broad phase systems run in [`PhysicsStepSet::BroadPhase`].
 pub struct BroadPhasePlugin;
 
 impl Plugin for BroadPhasePlugin {
@@ -25,16 +25,66 @@ impl Plugin for BroadPhasePlugin {
 
         physics_schedule.add_systems(
             (
+                update_aabb,
                 update_aabb_intervals,
                 add_new_aabb_intervals,
                 collect_collision_pairs,
             )
                 .chain()
-                .in_set(PhysicsSet::BroadPhase),
+                .in_set(PhysicsStepSet::BroadPhase),
         );
     }
 }
 
+type AABBChanged = Or<(
+    Changed<Position>,
+    Changed<Rotation>,
+    Changed<LinearVelocity>,
+    Changed<AngularVelocity>,
+)>;
+
+/// Updates the Axis-Aligned Bounding Boxes of all colliders. A safety margin will be added to account for sudden accelerations.
+#[allow(clippy::type_complexity)]
+fn update_aabb(
+    mut bodies: Query<
+        (
+            &Collider,
+            &mut ColliderAabb,
+            &Position,
+            &Rotation,
+            Option<&LinearVelocity>,
+            Option<&AngularVelocity>,
+        ),
+        AABBChanged,
+    >,
+    dt: Res<DeltaTime>,
+) {
+    // Safety margin multiplier bigger than DELTA_TIME to account for sudden accelerations
+    let safety_margin_factor = 2.0 * dt.0;
+
+    for (collider, mut aabb, pos, rot, lin_vel, ang_vel) in &mut bodies {
+        let lin_vel_len = lin_vel.map_or(0.0, |v| v.length());
+
+        #[cfg(feature = "2d")]
+        let ang_vel_len = ang_vel.map_or(0.0, |v| v.0.abs());
+        #[cfg(feature = "3d")]
+        let ang_vel_len = ang_vel.map_or(0.0, |v| v.0.length());
+
+        let computed_aabb = collider
+            .get_shape()
+            .compute_aabb(&utils::make_isometry(pos.0, rot));
+        let half_extents = Vector::from(computed_aabb.half_extents());
+        let center = Vector::from(computed_aabb.center());
+
+        // Add a safety margin.
+        let safety_margin = safety_margin_factor * (lin_vel_len + ang_vel_len);
+        let extended_half_extents = half_extents + safety_margin;
+
+        aabb.mins.coords = (center - extended_half_extents).into();
+        aabb.maxs.coords = (center + extended_half_extents).into();
+    }
+}
+
 /// Entities with [`ColliderAabb`]s sorted along an axis by their extents.
 #[derive(Resource, Default)]
 struct AabbIntervals(Vec<(Entity, ColliderAabb, RigidBody, CollisionLayers)>);