From 3bbc15feda0f8112e5679d60036c8ab1dde3afd3 Mon Sep 17 00:00:00 2001 From: Jondolf Date: Mon, 27 Feb 2023 19:09:42 +0200 Subject: [PATCH] Split physics systems into plugins, refactor The massive systems file is now gone. Systems have been organized into plugins according to the steps in the physics simulation loop. - PreparePlugin: Update and synchronize components, prepare things - BroadPhasePlugin: Collect pairs of potentially colliding entities - NarrowPhasePlugin: Detect collisions precisely, collect collision data - Integrator: Integrate positions and velocities, apply external forces - Solver: Solve constraints, i.e. joints and collision response This should also make it possible for users to replace parts of the engine with their own plugins. I also renamed contacts to collisions. --- src/constraints/penetration.rs | 44 ++-- src/lib.rs | 107 ++------- src/resources.rs | 30 +-- src/steps/broad_phase.rs | 44 ++++ src/steps/integrator.rs | 119 ++++++++++ src/steps/mod.rs | 35 +++ src/steps/narrow_phase.rs | 125 +++++++++++ src/steps/prepare.rs | 109 ++++++++++ src/{systems.rs => steps/solver.rs} | 327 ++++++---------------------- src/utils.rs | 52 +---- 10 files changed, 546 insertions(+), 446 deletions(-) create mode 100644 src/steps/broad_phase.rs create mode 100644 src/steps/integrator.rs create mode 100644 src/steps/mod.rs create mode 100644 src/steps/narrow_phase.rs create mode 100644 src/steps/prepare.rs rename src/{systems.rs => steps/solver.rs} (50%) diff --git a/src/constraints/penetration.rs b/src/constraints/penetration.rs index 8c623155..cbe28dbe 100644 --- a/src/constraints/penetration.rs +++ b/src/constraints/penetration.rs @@ -1,5 +1,5 @@ use super::{Constraint, PositionConstraint}; -use crate::{components::*, resources::Contact, Vector}; +use crate::{components::*, narrow_phase::Collision, Vector}; use bevy::prelude::*; @@ -12,7 +12,7 @@ pub(crate) struct PenetrationConstraint { pub entity_a: Entity, /// Entity "b" in the constraint. pub entity_b: Entity, - pub contact_data: Contact, + pub collision_data: Collision, /// Lagrange multiplier for the normal force pub normal_lagrange: f32, /// Lagrange multiplier for the tangential force @@ -24,11 +24,11 @@ pub(crate) struct PenetrationConstraint { } impl PenetrationConstraint { - pub fn new(entity_a: Entity, entity_b: Entity, contact_data: Contact) -> Self { + pub fn new(entity_a: Entity, entity_b: Entity, collision_data: Collision) -> Self { Self { entity_a, entity_b, - contact_data, + collision_data, normal_lagrange: 0.0, tangential_lagrange: 0.0, compliance: 0.0, @@ -44,19 +44,19 @@ impl PenetrationConstraint { body2: &mut RigidBodyQueryItem, sub_dt: f32, ) { - // Compute contact positions on the two bodies. + // Compute collision positions on the two bodies. // Subtracting the local center of mass from the position seems to be important for some shapes, although it's not shown in the paper. Something may be wrong elsewhere? - let p_a = body1.pos.0 - body1.local_com.0 + body1.rot.rotate(self.contact_data.local_r_a); - let p_b = body2.pos.0 - body2.local_com.0 + body2.rot.rotate(self.contact_data.local_r_b); + let p_a = body1.pos.0 - body1.local_com.0 + body1.rot.rotate(self.collision_data.local_r_a); + let p_b = body2.pos.0 - body2.local_com.0 + body2.rot.rotate(self.collision_data.local_r_b); - let d = (p_a - p_b).dot(self.contact_data.normal); + let d = (p_a - p_b).dot(self.collision_data.normal); - // Penetration under 0, skip contact + // Penetration under 0, skip collision if d <= 0.0 { return; } - let n = self.contact_data.normal; + let n = self.collision_data.normal; let inv_inertia1 = body1.inv_inertia.rotated(&body1.rot); let inv_inertia2 = body2.inv_inertia.rotated(&body2.rot); @@ -69,8 +69,8 @@ impl PenetrationConstraint { self.normal_lagrange, n, d, - self.contact_data.world_r_a, - self.contact_data.world_r_b, + self.collision_data.world_r_a, + self.collision_data.world_r_b, self.compliance, sub_dt, ); @@ -84,12 +84,12 @@ impl PenetrationConstraint { inv_inertia2, delta_lagrange_n, n, - self.contact_data.world_r_a, - self.contact_data.world_r_b, + self.collision_data.world_r_a, + self.collision_data.world_r_b, ); - let p_a = body1.pos.0 - body1.local_com.0 + body1.rot.rotate(self.contact_data.local_r_a); - let p_b = body2.pos.0 - body2.local_com.0 + body2.rot.rotate(self.contact_data.local_r_b); + let p_a = body1.pos.0 - body1.local_com.0 + body1.rot.rotate(self.collision_data.local_r_a); + let p_b = body2.pos.0 - body2.local_com.0 + body2.rot.rotate(self.collision_data.local_r_b); let inv_inertia1 = body1.inv_inertia.rotated(&body1.rot); let inv_inertia2 = body2.inv_inertia.rotated(&body2.rot); @@ -102,8 +102,8 @@ impl PenetrationConstraint { self.tangential_lagrange, n, d, - self.contact_data.world_r_a, - self.contact_data.world_r_b, + self.collision_data.world_r_a, + self.collision_data.world_r_b, self.compliance, sub_dt, ); @@ -116,9 +116,9 @@ impl PenetrationConstraint { if lagrange_t > static_friction_coefficient * lagrange_n { let prev_p_a = body1.prev_pos.0 - body1.local_com.0 - + body1.prev_rot.rotate(self.contact_data.local_r_a); + + body1.prev_rot.rotate(self.collision_data.local_r_a); let prev_p_b = body2.prev_pos.0 - body2.local_com.0 - + body2.prev_rot.rotate(self.contact_data.local_r_b); + + body2.prev_rot.rotate(self.collision_data.local_r_b); let delta_p = (p_a - prev_p_a) - (p_b - prev_p_b); let delta_p_t = delta_p - delta_p.dot(n) * n; @@ -131,8 +131,8 @@ impl PenetrationConstraint { inv_inertia2, delta_lagrange_t, delta_p_t.normalize_or_zero(), - self.contact_data.world_r_a, - self.contact_data.world_r_b, + self.collision_data.world_r_a, + self.collision_data.world_r_b, ); } diff --git a/src/lib.rs b/src/lib.rs index 57b65f84..cd1332ab 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -8,7 +8,7 @@ pub mod bundles; pub mod components; pub mod constraints; pub mod resources; -pub mod systems; +pub mod steps; pub mod prelude { pub use crate::{ @@ -16,6 +16,7 @@ pub mod prelude { components::*, constraints::{joints::*, *}, resources::*, + steps::*, *, }; } @@ -23,8 +24,8 @@ pub mod prelude { mod utils; use bevy::{ecs::schedule::ShouldRun, prelude::*}; +use parry::math::Isometry; use prelude::*; -use systems::*; #[cfg(feature = "2d")] pub type Vector = Vec2; @@ -37,26 +38,16 @@ pub const DELTA_TIME: f32 = 1.0 / 60.0; #[derive(Debug, Hash, PartialEq, Eq, Clone, StageLabel)] struct FixedUpdateStage; -#[derive(SystemLabel, Clone, Copy, Debug, PartialEq, Eq, Hash)] -pub enum Step { - CollectCollisionPairs, - Integrate, - SolvePos, - UpdateVel, - SolveVel, -} - pub struct XpbdPlugin; impl Plugin for XpbdPlugin { fn build(&self, app: &mut App) { + // Init resources and register component types app.init_resource::() .init_resource::() .init_resource::() .init_resource::() .init_resource::() - .init_resource::() - .init_resource::() .register_type::() .register_type::() .register_type::() @@ -74,73 +65,21 @@ impl Plugin for XpbdPlugin { .register_type::() .register_type::() .register_type::() - .register_type::() - .add_stage_before( - CoreStage::Update, - FixedUpdateStage, - SystemStage::parallel() - .with_run_criteria(run_criteria) - .with_system_set( - SystemSet::new() - .before(Step::CollectCollisionPairs) - .with_system(update_sub_delta_time) - .with_system(update_aabb) - .with_system(update_mass_props), - ) - .with_system( - collect_collision_pairs - .label(Step::CollectCollisionPairs) - .with_run_criteria(first_substep) - .before(Step::Integrate), - ) - .with_system_set( - SystemSet::new() - .label(Step::Integrate) - .with_system(integrate_pos) - .with_system(integrate_rot), - ) - .with_system_set( - SystemSet::new() - .before(Step::SolvePos) - .with_system(clear_penetration_constraint_lagrange) - .with_system(clear_joint_lagrange::) - .with_system(clear_joint_lagrange::) - .with_system(clear_joint_lagrange::) - .with_system(clear_joint_lagrange::), - ) - .with_system_set( - SystemSet::new() - .label(Step::SolvePos) - .after(Step::Integrate) - .with_system(solve_pos) - .with_system(joint_constraints::) - .with_system(joint_constraints::) - .with_system(joint_constraints::) - .with_system(joint_constraints::), - ) - .with_system_set( - SystemSet::new() - .label(Step::UpdateVel) - .after(Step::SolvePos) - .with_system(update_lin_vel) - .with_system(update_ang_vel), - ) - .with_system_set( - SystemSet::new() - .label(Step::SolveVel) - .after(Step::UpdateVel) - .with_system(solve_vel) - .with_system(joint_damping::) - .with_system(joint_damping::) - .with_system(joint_damping::) - .with_system(joint_damping::), - ) - .with_system( - sync_transforms - .with_run_criteria(last_substep) - .after(Step::SolveVel), - ), - ); + .register_type::(); + + // Add stages + app.add_stage_before( + CoreStage::Update, + FixedUpdateStage, + SystemStage::parallel().with_run_criteria(run_criteria), + ); + + // Add plugins for physics simulation loop + app.add_plugin(PreparePlugin) + .add_plugin(BroadPhasePlugin) + .add_plugin(NarrowPhasePlugin) + .add_plugin(IntegratorPlugin) + .add_plugin(SolverPlugin); } } @@ -228,11 +167,3 @@ fn first_substep(state: Res) -> ShouldRun { ShouldRun::No } } - -fn last_substep(substeps: Res, state: Res) -> ShouldRun { - if state.current_substep == substeps.0 - 1 { - ShouldRun::Yes - } else { - ShouldRun::No - } -} diff --git a/src/resources.rs b/src/resources.rs index 52a410b9..84db9c3c 100644 --- a/src/resources.rs +++ b/src/resources.rs @@ -1,6 +1,4 @@ -use crate::{constraints::penetration::PenetrationConstraint, Vector, DELTA_TIME}; - -use bevy::prelude::*; +use crate::{Vector, DELTA_TIME}; /// Number of substeps used in XPBD simulation pub struct NumSubsteps(pub u32); @@ -37,29 +35,3 @@ impl Default for Gravity { Self(Vector::Y * -9.81) } } - -/// Stores penetration constraints for potentially colliding entity pairs. -#[derive(Debug, Default)] -pub(crate) struct PenetrationConstraints(pub Vec); - -#[derive(Default, Debug)] -pub struct BroadCollisionPairs(pub Vec<(Entity, Entity)>); - -/// Data related to a contact between two bodies. -#[derive(Clone, Copy, Debug, PartialEq)] -pub struct Contact { - pub entity_a: Entity, - pub entity_b: Entity, - /// Local contact point a in local coordinates - pub local_r_a: Vector, - /// Local contact point b in local coordinates - pub local_r_b: Vector, - /// Local contact point a in world coordinates - pub world_r_a: Vector, - /// Local contact point b in world coordinates - pub world_r_b: Vector, - /// Contact normal from contact point a to b - pub normal: Vector, - /// Penetration depth - pub penetration: f32, -} diff --git a/src/steps/broad_phase.rs b/src/steps/broad_phase.rs new file mode 100644 index 00000000..45128723 --- /dev/null +++ b/src/steps/broad_phase.rs @@ -0,0 +1,44 @@ +use crate::prelude::*; +use bevy::prelude::*; +use parry::bounding_volume::BoundingVolume; + +pub struct BroadPhasePlugin; + +impl Plugin for BroadPhasePlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .label(PhysicsStep::BroadPhase) + .with_run_criteria(first_substep) + .with_system(collect_collision_pairs), + ); + } +} + +#[derive(Default, Debug)] +pub struct BroadCollisionPairs(pub Vec<(Entity, Entity)>); + +/// Collects bodies that are potentially colliding. +fn collect_collision_pairs( + bodies: Query<(Entity, &ColliderAabb, &RigidBody)>, + mut broad_collision_pairs: ResMut, +) { + brute_force_collision_pairs(bodies, &mut broad_collision_pairs.0); +} + +/// Collects bodies that are potentially colliding using a brute force algorithm (better one coming soon). +fn brute_force_collision_pairs( + bodies: Query<(Entity, &ColliderAabb, &RigidBody)>, + broad_collision_pairs: &mut Vec<(Entity, Entity)>, +) { + broad_collision_pairs.clear(); + + for [(ent_a, aabb_a, rb_a), (ent_b, aabb_b, rb_b)] in bodies.iter_combinations() { + // At least one of the bodies is dynamic and their AABBs intersect + if (rb_a.is_dynamic() || rb_b.is_dynamic()) && aabb_a.intersects(&aabb_b.0) { + broad_collision_pairs.push((ent_a, ent_b)); + } + } +} diff --git a/src/steps/integrator.rs b/src/steps/integrator.rs new file mode 100644 index 00000000..198e9eee --- /dev/null +++ b/src/steps/integrator.rs @@ -0,0 +1,119 @@ +use crate::prelude::*; +use bevy::prelude::*; + +pub struct IntegratorPlugin; + +impl Plugin for IntegratorPlugin { + fn build(&self, app: &mut App) { + app.add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .label(PhysicsStep::Integrate) + .after(PhysicsStep::BroadPhase) + .with_system(integrate_pos) + .with_system(integrate_rot), + ); + } +} + +/// Applies forces and predicts the next position and velocity for all dynamic bodies. +fn integrate_pos( + mut bodies: Query<( + &RigidBody, + &mut Pos, + &mut PrevPos, + &mut LinVel, + &ExternalForce, + &Mass, + )>, + gravity: Res, + sub_dt: Res, +) { + for (rb, mut pos, mut prev_pos, mut lin_vel, external_force, mass) in &mut bodies { + prev_pos.0 = pos.0; + + if rb.is_static() { + continue; + } + + // Apply gravity and other external forces + if rb.is_dynamic() { + let gravitation_force = mass.0 * gravity.0; + let external_forces = gravitation_force + external_force.0; + lin_vel.0 += sub_dt.0 * external_forces / mass.0; + } + + pos.0 += sub_dt.0 * lin_vel.0; + } +} + +/// Integrates rotations for all dynamic bodies. +#[cfg(feature = "2d")] +fn integrate_rot( + mut bodies: Query<( + &RigidBody, + &mut Rot, + &mut PrevRot, + &mut AngVel, + &ExternalTorque, + &InvInertia, + )>, + sub_dt: Res, +) { + for (rb, mut rot, mut prev_rot, mut ang_vel, external_torque, inv_inertia) in &mut bodies { + prev_rot.0 = *rot; + + if rb.is_static() { + continue; + } + + // Apply external torque + if rb.is_dynamic() { + ang_vel.0 += sub_dt.0 * inv_inertia.0 * external_torque.0; + } + + *rot += Rot::from_radians(sub_dt.0 * ang_vel.0); + } +} + +/// Integrates rotations for all dynamic bodies. +#[cfg(feature = "3d")] +fn integrate_rot( + mut bodies: Query<( + &RigidBody, + &mut Rot, + &mut PrevRot, + &mut AngVel, + &ExternalTorque, + &Inertia, + &InvInertia, + )>, + sub_dt: Res, +) { + 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() { + continue; + } + + // Apply external torque + if rb.is_dynamic() { + let delta_ang_vel = sub_dt.0 + * inv_inertia.rotated(&rot).0 + * (external_torque.0 - ang_vel.cross(inertia.rotated(&rot).0 * ang_vel.0)); + ang_vel.0 += delta_ang_vel; + } + + let q = Quat::from_vec4(ang_vel.extend(0.0)) * rot.0; + let (x, y, z, w) = ( + rot.x + sub_dt.0 * 0.5 * q.x, + rot.y + sub_dt.0 * 0.5 * q.y, + rot.z + sub_dt.0 * 0.5 * q.z, + rot.w + sub_dt.0 * 0.5 * q.w, + ); + rot.0 = Quat::from_xyzw(x, y, z, w).normalize(); + } +} diff --git a/src/steps/mod.rs b/src/steps/mod.rs new file mode 100644 index 00000000..162d34ef --- /dev/null +++ b/src/steps/mod.rs @@ -0,0 +1,35 @@ +pub mod broad_phase; +pub mod integrator; +pub mod narrow_phase; +pub mod prepare; +pub mod solver; + +pub use broad_phase::BroadPhasePlugin; +pub use integrator::IntegratorPlugin; +pub use narrow_phase::NarrowPhasePlugin; +pub use prepare::PreparePlugin; +pub use solver::SolverPlugin; + +use bevy::prelude::SystemLabel; + +/// The main steps in the physics simulation loop. +#[derive(SystemLabel, Clone, Copy, Debug, PartialEq, Eq, Hash)] +pub enum PhysicsStep { + /// In the preparation step, necessary preparations and updates will be run before the rest of the physics simulation loop. + /// For example, bevy `Transform`s are synchronized with the physics world, AABBs are updated etc. + Prepare, + /// During the broad phase, pairs of potentially colliding entities will be collected into the [`BroadCollisionPairs`] resource using simple AABB intersection checks. These will be further checked for collision in the [`PhysicsStep::NarrowPhase`]. + /// + /// The broad phase speeds up collision detection, as the number of accurate collision checks is greatly reduced. + BroadPhase, + /// During the narrow phase, pairs of entities collected in the [`PhysicsStep::BroadPhase`] will be checked for collisions. The data for each collision is stored in the [`Collisions`] resource. + NarrowPhase, + /// In the integration step, the position and velocity of each particle and body is explicitly integrated, taking only external forces like gravity (and forces applied by the user) into account. + Integrate, + /// The position solving step iterates through constraints, and moves particles and bodies accordingly. This is used for things like joints and resolving collisions through non-penetration constraints. + SolvePos, + /// In the velocity update step, new velocities are derived for all particles and bodies after the position solving step. + UpdateVel, + /// During the velocity solving step, a velocity update caused by properties like restitution and friction will be applied to all particles and bodies. + SolveVel, +} diff --git a/src/steps/narrow_phase.rs b/src/steps/narrow_phase.rs new file mode 100644 index 00000000..f2ae1aec --- /dev/null +++ b/src/steps/narrow_phase.rs @@ -0,0 +1,125 @@ +use crate::{broad_phase::BroadCollisionPairs, prelude::*}; +use bevy::{prelude::*, utils::HashMap}; +use parry::math::Isometry; + +pub struct NarrowPhasePlugin; + +impl Plugin for NarrowPhasePlugin { + fn build(&self, app: &mut App) { + app.init_resource::().add_system_to_stage( + FixedUpdateStage, + collect_collisions + .label(PhysicsStep::NarrowPhase) + .after(PhysicsStep::Integrate), + ); + } +} + +#[derive(Default, Debug)] +pub struct Collisions(pub HashMap<(Entity, Entity), Collision>); + +/// Data related to a collision between two bodies. +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct Collision { + pub entity_a: Entity, + pub entity_b: Entity, + /// Local contact point a in local coordinates + pub local_r_a: Vector, + /// Local contact point b in local coordinates + pub local_r_b: Vector, + /// Local contact point a in world coordinates + pub world_r_a: Vector, + /// Local contact point b in world coordinates + pub world_r_b: Vector, + /// Contact normal from contact point a to b + pub normal: Vector, + /// Penetration depth + pub penetration: f32, +} + +/// Iterates through broad phase collision pairs, checks which ones are actually colliding, and stores the collision data in the [`Collisions`] resource. +fn collect_collisions( + bodies: Query<(RigidBodyQuery, &ColliderShape)>, + broad_collision_pairs: Res, + mut collisions: ResMut, +) { + collisions.0.clear(); + + for (ent_a, ent_b) in broad_collision_pairs.0.iter().cloned() { + // Get components for entity a and b + if let Ok([(body1, collider_shape_a), (body2, collider_shape_b)]) = + bodies.get_many([ent_a, ent_b]) + { + // No need to solve collisions if neither of the bodies is dynamic + if !body1.rb.is_dynamic() && !body2.rb.is_dynamic() { + continue; + } + + // Detect if the entities are actually colliding and get collision data + if let Some(collision) = get_collision( + ent_a, + ent_b, + body1.pos.0, + body2.pos.0, + body1.local_com.0, + body2.local_com.0, + body1.rot, + body2.rot, + &collider_shape_a.0, + &collider_shape_b.0, + ) { + collisions.0.insert((ent_a, ent_b), collision); + } + } + } +} + +/// Computes one pair of collision points between two shapes. +#[allow(clippy::too_many_arguments)] +fn get_collision( + ent_a: Entity, + ent_b: Entity, + pos_a: Vector, + pos_b: Vector, + local_com_a: Vector, + local_com_b: Vector, + rot_a: &Rot, + rot_b: &Rot, + shape_a: &Shape, + shape_b: &Shape, +) -> Option { + if let Some(collision) = parry::query::contact( + &make_isometry(pos_a, rot_a), + shape_a.0.as_ref(), + &make_isometry(pos_b, rot_b), + shape_b.0.as_ref(), + 0.0, + ) + .unwrap() + { + let world_r_a = Vector::from(collision.point1) - pos_a + local_com_a; + let world_r_b = Vector::from(collision.point2) - pos_b + local_com_b; + + return Some(Collision { + entity_a: ent_a, + entity_b: ent_b, + local_r_a: rot_a.inv().rotate(world_r_a), + local_r_b: rot_b.inv().rotate(world_r_b), + world_r_a, + world_r_b, + normal: Vector::from(collision.normal1), + penetration: -collision.dist, + }); + } + None +} + +#[cfg(feature = "2d")] +fn make_isometry(pos: Vector, rot: &Rot) -> Isometry { + Isometry::::new(pos.into(), (*rot).into()) +} + +#[cfg(feature = "3d")] +fn make_isometry(pos: Vector, rot: &Rot) -> Isometry { + Isometry::::new(pos.into(), rot.to_scaled_axis().into()) +} diff --git a/src/steps/prepare.rs b/src/steps/prepare.rs new file mode 100644 index 00000000..7af47a22 --- /dev/null +++ b/src/steps/prepare.rs @@ -0,0 +1,109 @@ +use crate::prelude::*; +use bevy::prelude::*; + +pub struct PreparePlugin; + +impl Plugin for PreparePlugin { + fn build(&self, app: &mut bevy::prelude::App) { + app.add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .before(PhysicsStep::BroadPhase) + .label(PhysicsStep::Prepare) + .with_run_criteria(first_substep) + .with_system(sync_transforms) + .with_system(update_sub_delta_time) + .with_system(update_aabb) + .with_system(update_mass_props), + ); + } +} +/// Copies positions from the physics world to bevy Transforms +#[cfg(feature = "2d")] +fn sync_transforms(mut bodies: Query<(&mut Transform, &Pos, &Rot)>) { + for (mut transform, pos, rot) in &mut bodies { + transform.translation = pos.extend(0.0); + transform.rotation = (*rot).into(); + } +} + +/// Copies positions from the physics world to bevy Transforms +#[cfg(feature = "3d")] +fn sync_transforms(mut bodies: Query<(&mut Transform, &Pos, &Rot)>) { + for (mut transform, pos, rot) in &mut bodies { + transform.translation = pos.0; + transform.rotation = rot.0; + } +} + +type AABBChanged = Or<(Changed, Changed, Changed)>; + +/// Updates the Axis-Aligned Bounding Boxes of all colliders. A safety margin will be added to account for sudden accelerations. +fn update_aabb(mut bodies: Query<(ColliderQuery, &Pos, &Rot, Option<&LinVel>), AABBChanged>) { + // Safety margin multiplier bigger than DELTA_TIME to account for sudden accelerations + let safety_margin_factor = 2.0 * DELTA_TIME; + + for (mut collider, pos, rot, lin_vel) in &mut bodies { + let lin_vel = lin_vel.map_or(Vector::ZERO, |v| v.0); + + let half_extents = Vector::from( + collider + .shape + .compute_aabb(&Isometry::new(pos.0.into(), (*rot).into())) + .half_extents(), + ); + // Add a safety margin. + let scaled_half_extents = (half_extents + safety_margin_factor * lin_vel.length()) * 1.0; + + collider.aabb.mins.coords = (pos.0 - scaled_half_extents).into(); + collider.aabb.maxs.coords = (pos.0 + scaled_half_extents).into(); + } +} + +fn update_sub_delta_time(substeps: Res, mut sub_dt: ResMut) { + sub_dt.0 = DELTA_TIME / substeps.0 as f32; +} + +type MassPropsChanged = Or<( + Changed, + Changed, + Changed, + Changed, + Changed, + Changed, +)>; + +/// Updates each body's mass properties whenever their dependant mass properties or the body's [`Collider`] change. +/// +/// Also updates the collider's mass properties if the body has a collider. +fn update_mass_props( + mut bodies: Query<(MassPropsQueryMut, Option), MassPropsChanged>, +) { + for (mut mass_props, collider) in &mut bodies { + if mass_props.mass.is_changed() && mass_props.mass.0 >= f32::EPSILON { + mass_props.inv_mass.0 = 1.0 / mass_props.mass.0; + } + + if let Some(mut collider) = collider { + // Subtract previous collider mass props from the body's mass props + mass_props -= collider.prev_mass_props.0; + + // Update previous and current collider mass props + collider.prev_mass_props.0 = *collider.mass_props; + *collider.mass_props = ColliderMassProperties::from_shape_and_density( + &collider.shape.0, + collider.mass_props.density, + ); + + // Add new collider mass props to the body's mass props + mass_props += *collider.mass_props; + } + + if mass_props.mass.0 < f32::EPSILON { + mass_props.mass.0 = f32::EPSILON; + } + if mass_props.inv_mass.0 < f32::EPSILON { + mass_props.inv_mass.0 = f32::EPSILON; + } + } +} diff --git a/src/systems.rs b/src/steps/solver.rs similarity index 50% rename from src/systems.rs rename to src/steps/solver.rs index 0e7daafe..8a1370b9 100644 --- a/src/systems.rs +++ b/src/steps/solver.rs @@ -1,137 +1,64 @@ -//! XPBD systems and core logic. -//! -//! The math and physics are primarily from [Matthias Müller's paper titled "Detailed Rigid Body Simulation with Extended Position Based Dynamics"](https://matthias-research.github.io/pages/publications/PBDBodies.pdf). - use crate::{ - components::*, - constraints::{penetration::PenetrationConstraint, Constraint, PositionConstraint}, - prelude::Joint, - resources::*, - utils::*, - *, + narrow_phase::{Collision, Collisions}, + prelude::*, + utils::{get_dynamic_friction, get_restitution}, }; - use bevy::prelude::*; -use parry::{bounding_volume::BoundingVolume, math::Isometry}; - -/// Collects bodies that are potentially colliding and adds non-penetration constraints for them. -pub(crate) fn collect_collision_pairs( - bodies: Query<(Entity, &ColliderAabb, &RigidBody)>, - mut broad_collision_pairs: ResMut, -) { - broad_collision_pairs.0.clear(); - - for [(ent_a, aabb_a, rb_a), (ent_b, aabb_b, rb_b)] in bodies.iter_combinations() { - // At least one of the bodies is dynamic and their AABBs intersect - if (rb_a.is_dynamic() || rb_b.is_dynamic()) && aabb_a.intersects(&aabb_b.0) { - broad_collision_pairs.0.push((ent_a, ent_b)); - } - } -} - -/// Applies forces and predicts the next position and velocity for all dynamic bodies. -pub(crate) fn integrate_pos( - mut bodies: Query<( - &RigidBody, - &mut Pos, - &mut PrevPos, - &mut LinVel, - &ExternalForce, - &Mass, - )>, - gravity: Res, - sub_dt: Res, -) { - for (rb, mut pos, mut prev_pos, mut lin_vel, external_force, mass) in &mut bodies { - prev_pos.0 = pos.0; - - if rb.is_static() { - continue; - } - - // Apply gravity and other external forces - if rb.is_dynamic() { - let gravitation_force = mass.0 * gravity.0; - let external_forces = gravitation_force + external_force.0; - lin_vel.0 += sub_dt.0 * external_forces / mass.0; - } - - pos.0 += sub_dt.0 * lin_vel.0; - } -} - -/// Integrates rotations for all dynamic bodies. -#[cfg(feature = "2d")] -pub(crate) fn integrate_rot( - mut bodies: Query<( - &RigidBody, - &mut Rot, - &mut PrevRot, - &mut AngVel, - &ExternalTorque, - &InvInertia, - )>, - sub_dt: Res, -) { - for (rb, mut rot, mut prev_rot, mut ang_vel, external_torque, inv_inertia) in &mut bodies { - prev_rot.0 = *rot; - - if rb.is_static() { - continue; - } - - // Apply external torque - if rb.is_dynamic() { - ang_vel.0 += sub_dt.0 * inv_inertia.0 * external_torque.0; - } - - *rot += Rot::from_radians(sub_dt.0 * ang_vel.0); +use constraints::penetration::PenetrationConstraint; + +pub struct SolverPlugin; + +impl Plugin for SolverPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .before(PhysicsStep::SolvePos) + .with_system(clear_penetration_constraint_lagrange) + .with_system(clear_joint_lagrange::) + .with_system(clear_joint_lagrange::) + .with_system(clear_joint_lagrange::) + .with_system(clear_joint_lagrange::), + ) + .add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .label(PhysicsStep::SolvePos) + .after(PhysicsStep::NarrowPhase) + .with_system(solve_pos) + .with_system(joint_constraints::) + .with_system(joint_constraints::) + .with_system(joint_constraints::) + .with_system(joint_constraints::), + ) + .add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .label(PhysicsStep::UpdateVel) + .after(PhysicsStep::SolvePos) + .with_system(update_lin_vel) + .with_system(update_ang_vel), + ) + .add_system_set_to_stage( + FixedUpdateStage, + SystemSet::new() + .label(PhysicsStep::SolveVel) + .after(PhysicsStep::UpdateVel) + .with_system(solve_vel) + .with_system(joint_damping::) + .with_system(joint_damping::) + .with_system(joint_damping::) + .with_system(joint_damping::), + ); } } -/// Integrates rotations for all dynamic bodies. -#[cfg(feature = "3d")] -pub(crate) fn integrate_rot( - mut bodies: Query<( - &RigidBody, - &mut Rot, - &mut PrevRot, - &mut AngVel, - &ExternalTorque, - &Inertia, - &InvInertia, - )>, - sub_dt: Res, -) { - 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() { - continue; - } - - // Apply external torque - if rb.is_dynamic() { - let delta_ang_vel = sub_dt.0 - * inv_inertia.rotated(&rot).0 - * (external_torque.0 - ang_vel.cross(inertia.rotated(&rot).0 * ang_vel.0)); - ang_vel.0 += delta_ang_vel; - } - - let q = Quat::from_vec4(ang_vel.extend(0.0)) * rot.0; - let (x, y, z, w) = ( - rot.x + sub_dt.0 * 0.5 * q.x, - rot.y + sub_dt.0 * 0.5 * q.y, - rot.z + sub_dt.0 * 0.5 * q.z, - rot.w + sub_dt.0 * 0.5 * q.w, - ); - rot.0 = Quat::from_xyzw(x, y, z, w).normalize(); - } -} +/// Stores penetration constraints for colliding entity pairs. +#[derive(Debug, Default)] +pub(crate) struct PenetrationConstraints(pub Vec); -pub(crate) fn clear_penetration_constraint_lagrange( +fn clear_penetration_constraint_lagrange( mut penetration_constraints: ResMut, ) { for constraint in &mut penetration_constraints.0 { @@ -139,56 +66,37 @@ pub(crate) fn clear_penetration_constraint_lagrange( } } -pub(crate) fn clear_joint_lagrange(mut joints: Query<&mut T>) { +fn clear_joint_lagrange(mut joints: Query<&mut T>) { for mut joint in &mut joints { joint.clear_lagrange_multipliers(); } } /// Solves position constraints for dynamic-dynamic interactions. -pub(crate) fn solve_pos( - mut bodies: Query<(RigidBodyQuery, &ColliderShape)>, - broad_collision_pairs: Res, +fn solve_pos( + mut bodies: Query, + collisions: Res, mut penetration_constraints: ResMut, sub_dt: Res, ) { penetration_constraints.0.clear(); // Handle non-penetration constraints - for (ent_a, ent_b) in broad_collision_pairs.0.iter().cloned() { - // Get components for entity a and b - if let Ok([(mut body1, collider_shape_a), (mut body2, collider_shape_b)]) = - bodies.get_many_mut([ent_a, ent_b]) - { + for ((ent_a, ent_b), collision) in collisions.0.iter() { + if let Ok([mut body1, mut body2]) = bodies.get_many_mut([*ent_a, *ent_b]) { // No need to solve collisions if neither of the bodies is dynamic if !body1.rb.is_dynamic() && !body2.rb.is_dynamic() { continue; } - // Detect if the entities are actually colliding and get contact data - if let Some(contact) = get_contact( - ent_a, - ent_b, - body1.pos.0, - body2.pos.0, - body1.local_com.0, - body2.local_com.0, - &body1.rot, - &body2.rot, - &collider_shape_a.0, - &collider_shape_b.0, - ) { - let mut constraint = PenetrationConstraint::new(ent_a, ent_b, contact); - - constraint.constrain(&mut body1, &mut body2, sub_dt.0); - - penetration_constraints.0.push(constraint); - } + let mut constraint = PenetrationConstraint::new(*ent_a, *ent_b, *collision); + constraint.constrain(&mut body1, &mut body2, sub_dt.0); + penetration_constraints.0.push(constraint); } } } -pub(crate) fn joint_constraints( +fn joint_constraints( mut bodies: Query, mut joints: Query<&mut T, Without>, num_pos_iters: Res, @@ -210,7 +118,7 @@ pub(crate) fn joint_constraints( } /// Updates the linear velocity of all dynamic bodies based on the change in position from the previous step. -pub(crate) fn update_lin_vel( +fn update_lin_vel( mut bodies: Query<(&RigidBody, &Pos, &PrevPos, &mut LinVel, &mut PreSolveLinVel)>, sub_dt: Res, ) { @@ -230,7 +138,7 @@ pub(crate) fn update_lin_vel( /// Updates the angular velocity of all dynamic bodies based on the change in rotation from the previous step. #[cfg(feature = "2d")] -pub(crate) fn update_ang_vel( +fn update_ang_vel( mut bodies: Query<(&RigidBody, &Rot, &PrevRot, &mut AngVel, &mut PreSolveAngVel)>, sub_dt: Res, ) { @@ -249,7 +157,7 @@ pub(crate) fn update_ang_vel( /// Updates the angular velocity of all dynamic bodies based on the change in rotation from the previous step. #[cfg(feature = "3d")] -pub(crate) fn update_ang_vel( +fn update_ang_vel( mut bodies: Query<(&RigidBody, &Rot, &PrevRot, &mut AngVel, &mut PreSolveAngVel)>, sub_dt: Res, ) { @@ -274,21 +182,21 @@ pub(crate) fn update_ang_vel( /// Solves the new velocities of dynamic bodies after dynamic-dynamic collisions. #[allow(clippy::type_complexity)] -pub(crate) fn solve_vel( +fn solve_vel( mut bodies: Query, penetration_constraints: Res, gravity: Res, sub_dt: Res, ) { for constraint in penetration_constraints.0.iter().cloned() { - let Contact { + let Collision { entity_a, entity_b, world_r_a: r_a, world_r_b: r_b, normal, .. - } = constraint.contact_data; + } = constraint.collision_data; if let Ok([mut body1, mut body2]) = bodies.get_many_mut([entity_a, entity_b]) { if !body1.rb.is_dynamic() && !body2.rb.is_dynamic() { @@ -365,7 +273,7 @@ pub(crate) fn solve_vel( } } -pub(crate) fn joint_damping( +fn joint_damping( mut bodies: Query<(&RigidBody, &mut LinVel, &mut AngVel, &InvMass)>, joints: Query<&T, Without>, sub_dt: Res, @@ -425,96 +333,3 @@ fn compute_delta_ang_vel(inv_inertia: f32, r: Vector, p: Vector) -> f32 { fn compute_delta_ang_vel(inv_inertia: Mat3, r: Vector, p: Vector) -> Vector { inv_inertia * r.cross(p) } - -/// Copies positions from the physics world to bevy Transforms -#[cfg(feature = "2d")] -pub(crate) fn sync_transforms(mut bodies: Query<(&mut Transform, &Pos, &Rot)>) { - for (mut transform, pos, rot) in &mut bodies { - transform.translation = pos.extend(0.0); - transform.rotation = (*rot).into(); - } -} - -/// Copies positions from the physics world to bevy Transforms -#[cfg(feature = "3d")] -pub(crate) fn sync_transforms(mut bodies: Query<(&mut Transform, &Pos, &Rot)>) { - for (mut transform, pos, rot) in &mut bodies { - transform.translation = pos.0; - transform.rotation = rot.0; - } -} - -type AABBChanged = Or<(Changed, Changed, Changed)>; - -/// Updates the Axis-Aligned Bounding Boxes of all colliders. A safety margin will be added to account for sudden accelerations. -pub(crate) fn update_aabb( - mut bodies: Query<(ColliderQuery, &Pos, &Rot, Option<&LinVel>), AABBChanged>, -) { - // Safety margin multiplier bigger than DELTA_TIME to account for sudden accelerations - let safety_margin_factor = 2.0 * DELTA_TIME; - - for (mut collider, pos, rot, lin_vel) in &mut bodies { - let lin_vel = lin_vel.map_or(Vector::ZERO, |v| v.0); - - let half_extents = Vector::from( - collider - .shape - .compute_aabb(&Isometry::new(pos.0.into(), (*rot).into())) - .half_extents(), - ); - // Add a safety margin. - // Todo: The 1.5 multiplier shouldn't be needed, but without it bodies sometimes overlap too much and get launched. I'll return to this at some point, as it isn't ideal and it hurts performance. - let scaled_half_extents = (half_extents + safety_margin_factor * lin_vel.length()) * 1.5; - - collider.aabb.mins.coords = (pos.0 - scaled_half_extents).into(); - collider.aabb.maxs.coords = (pos.0 + scaled_half_extents).into(); - } -} - -pub(crate) fn update_sub_delta_time(substeps: Res, mut sub_dt: ResMut) { - sub_dt.0 = DELTA_TIME / substeps.0 as f32; -} - -type MassPropsChanged = Or<( - Changed, - Changed, - Changed, - Changed, - Changed, - Changed, -)>; - -/// Updates each body's mass properties whenever their dependant mass properties or the body's [`Collider`] change. -/// -/// Also updates the collider's mass properties if the body has a collider. -pub(crate) fn update_mass_props( - mut bodies: Query<(MassPropsQueryMut, Option), MassPropsChanged>, -) { - for (mut mass_props, collider) in &mut bodies { - if mass_props.mass.is_changed() && mass_props.mass.0 >= f32::EPSILON { - mass_props.inv_mass.0 = 1.0 / mass_props.mass.0; - } - - if let Some(mut collider) = collider { - // Subtract previous collider mass props from the body's mass props - mass_props -= collider.prev_mass_props.0; - - // Update previous and current collider mass props - collider.prev_mass_props.0 = *collider.mass_props; - *collider.mass_props = ColliderMassProperties::from_shape_and_density( - &collider.shape.0, - collider.mass_props.density, - ); - - // Add new collider mass props to the body's mass props - mass_props += *collider.mass_props; - } - - if mass_props.mass.0 < f32::EPSILON { - mass_props.mass.0 = f32::EPSILON; - } - if mass_props.inv_mass.0 < f32::EPSILON { - mass_props.inv_mass.0 = f32::EPSILON; - } - } -} diff --git a/src/utils.rs b/src/utils.rs index fb69dd48..4e0a5100 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,56 +1,6 @@ use crate::prelude::*; -use bevy::prelude::*; -use parry::math::Isometry; - -/// Computes one pair of contact points between two shapes. -#[allow(clippy::too_many_arguments)] -pub(crate) fn get_contact( - ent_a: Entity, - ent_b: Entity, - pos_a: Vector, - pos_b: Vector, - local_com_a: Vector, - local_com_b: Vector, - rot_a: &Rot, - rot_b: &Rot, - shape_a: &Shape, - shape_b: &Shape, -) -> Option { - if let Some(contact) = parry::query::contact( - &make_isometry(pos_a, rot_a), - shape_a.0.as_ref(), - &make_isometry(pos_b, rot_b), - shape_b.0.as_ref(), - 0.0, - ) - .unwrap() - { - let world_r_a = Vector::from(contact.point1) - pos_a + local_com_a; - let world_r_b = Vector::from(contact.point2) - pos_b + local_com_b; - - return Some(Contact { - entity_a: ent_a, - entity_b: ent_b, - local_r_a: rot_a.inv().rotate(world_r_a), - local_r_b: rot_b.inv().rotate(world_r_b), - world_r_a, - world_r_b, - normal: Vector::from(contact.normal1), - penetration: -contact.dist, - }); - } - None -} - -#[cfg(feature = "2d")] -pub(crate) fn make_isometry(pos: Vector, rot: &Rot) -> Isometry { - Isometry::::new(pos.into(), (*rot).into()) -} - #[cfg(feature = "3d")] -pub(crate) fn make_isometry(pos: Vector, rot: &Rot) -> Isometry { - Isometry::::new(pos.into(), rot.to_scaled_axis().into()) -} +use bevy::prelude::*; #[cfg(feature = "3d")] pub(crate) fn get_rotated_inertia_tensor(inertia_tensor: Mat3, rot: Quat) -> Mat3 {