Skip to content

Commit

Permalink
Split physics systems into plugins, refactor
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@Jondolf
Jondolf committed Feb 27, 2023
1 parent 54bdd56 commit 3bbc15f
Show file tree
Hide file tree
Showing 10 changed files with 546 additions and 446 deletions.
44 changes: 22 additions & 22 deletions src/constraints/penetration.rs
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
use super::{Constraint, PositionConstraint};
use crate::{components::*, resources::Contact, Vector};
use crate::{components::*, narrow_phase::Collision, Vector};

use bevy::prelude::*;

Expand All @@ -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
Expand All @@ -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,
Expand All @@ -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);
Expand All @@ -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,
);
Expand All @@ -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);
Expand All @@ -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,
);
Expand All @@ -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;

Expand All @@ -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,
);
}

Expand Down
107 changes: 19 additions & 88 deletions src/lib.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,23 +8,24 @@ pub mod bundles;
pub mod components;
pub mod constraints;
pub mod resources;
pub mod systems;
pub mod steps;

pub mod prelude {
pub use crate::{
bundles::*,
components::*,
constraints::{joints::*, *},
resources::*,
steps::*,
*,
};
}

mod utils;

use bevy::{ecs::schedule::ShouldRun, prelude::*};
use parry::math::Isometry;
use prelude::*;
use systems::*;

#[cfg(feature = "2d")]
pub type Vector = Vec2;
Expand All @@ -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::<NumSubsteps>()
.init_resource::<NumPosIters>()
.init_resource::<SubDeltaTime>()
.init_resource::<XpbdLoop>()
.init_resource::<Gravity>()
.init_resource::<PenetrationConstraints>()
.init_resource::<BroadCollisionPairs>()
.register_type::<RigidBody>()
.register_type::<Pos>()
.register_type::<Rot>()
Expand All @@ -74,73 +65,21 @@ impl Plugin for XpbdPlugin {
.register_type::<InvMass>()
.register_type::<Inertia>()
.register_type::<InvInertia>()
.register_type::<LocalCom>()
.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::<FixedJoint>)
.with_system(clear_joint_lagrange::<RevoluteJoint>)
.with_system(clear_joint_lagrange::<SphericalJoint>)
.with_system(clear_joint_lagrange::<PrismaticJoint>),
)
.with_system_set(
SystemSet::new()
.label(Step::SolvePos)
.after(Step::Integrate)
.with_system(solve_pos)
.with_system(joint_constraints::<FixedJoint>)
.with_system(joint_constraints::<RevoluteJoint>)
.with_system(joint_constraints::<SphericalJoint>)
.with_system(joint_constraints::<PrismaticJoint>),
)
.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::<FixedJoint>)
.with_system(joint_damping::<RevoluteJoint>)
.with_system(joint_damping::<SphericalJoint>)
.with_system(joint_damping::<PrismaticJoint>),
)
.with_system(
sync_transforms
.with_run_criteria(last_substep)
.after(Step::SolveVel),
),
);
.register_type::<LocalCom>();

// 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);
}
}

Expand Down Expand Up @@ -228,11 +167,3 @@ fn first_substep(state: Res<XpbdLoop>) -> ShouldRun {
ShouldRun::No
}
}

fn last_substep(substeps: Res<NumSubsteps>, state: Res<XpbdLoop>) -> ShouldRun {
if state.current_substep == substeps.0 - 1 {
ShouldRun::Yes
} else {
ShouldRun::No
}
}
30 changes: 1 addition & 29 deletions src/resources.rs
View file
Original file line number Diff line number Diff line change
@@ -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);
Expand Down Expand Up @@ -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<PenetrationConstraint>);

#[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,
}
44 changes: 44 additions & 0 deletions src/steps/broad_phase.rs
View file
Original file line number Diff line number Diff line change
@@ -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::<BroadCollisionPairs>()
.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<BroadCollisionPairs>,
) {
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));
}
}
}
Loading

0 comments on commit 3bbc15f

Please sign in to comment.