From d6b61898612d05e12b52d9636e9bb21dccdca4bb Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Fri, 24 Jun 2022 15:11:37 +0200 Subject: Properly wake-up rigid-bodies that must be awaken because of joints --- src/pipeline/physics_pipeline.rs | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'src') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 19749be..16f2830 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -371,6 +371,16 @@ impl PhysicsPipeline { hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { + // Apply some of delayed wake-ups. + for handle in impulse_joints + .to_wake_up + .drain(..) + .chain(multibody_joints.to_wake_up.drain(..)) + { + islands.wake_up(bodies, handle, true); + } + + // Apply modifications. let modified_bodies = bodies.take_modified(); let mut modified_colliders = colliders.take_modified(); let mut removed_colliders = colliders.take_removed(); -- cgit From c9d8277377681a6c5162abe4e8f17a058eebcfd4 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Fri, 24 Jun 2022 19:00:34 +0200 Subject: Add contact force events generated above a user-defined threshold --- src/geometry/collider.rs | 64 ++++++++++++++-------------- src/geometry/contact_pair.rs | 45 ++++++++++++++++++++ src/geometry/mod.rs | 24 +++++++++++ src/geometry/narrow_phase.rs | 18 +++++++- src/pipeline/event_handler.rs | 91 +++++++++++++++++++++++++++++++++++++--- src/pipeline/physics_pipeline.rs | 47 +++++++++++++++++---- 6 files changed, 244 insertions(+), 45 deletions(-) (limited to 'src') diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 8d9b005..2f2e4ac 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -26,6 +26,7 @@ pub struct Collider { pub(crate) material: ColliderMaterial, pub(crate) flags: ColliderFlags, pub(crate) bf_data: ColliderBroadPhaseData, + pub(crate) contact_force_event_threshold: Real, /// User-defined data associated to this collider. pub user_data: u128, } @@ -124,6 +125,11 @@ impl Collider { self.material.restitution_combine_rule = rule; } + /// Sets the total force magnitude beyond which a contact force event can be emitted. + pub fn set_contact_force_event_threshold(&mut self, threshold: Real) { + self.contact_force_event_threshold = threshold; + } + /// Sets whether or not this is a sensor collider. pub fn set_sensor(&mut self, is_sensor: bool) { if is_sensor != self.is_sensor() { @@ -283,6 +289,11 @@ impl Collider { ColliderMassProps::MassProperties(mass_properties) => **mass_properties, } } + + /// The total force magnitude beyond which a contact force event can be emitted. + pub fn contact_force_event_threshold(&self) -> Real { + self.contact_force_event_threshold + } } /// A structure responsible for building a new collider. @@ -321,6 +332,8 @@ pub struct ColliderBuilder { pub collision_groups: InteractionGroups, /// The solver groups for the collider being built. pub solver_groups: InteractionGroups, + /// The total force magnitude beyond which a contact force event can be emitted. + pub contact_force_event_threshold: Real, } impl ColliderBuilder { @@ -342,6 +355,7 @@ impl ColliderBuilder { active_collision_types: ActiveCollisionTypes::default(), active_hooks: ActiveHooks::empty(), active_events: ActiveEvents::empty(), + contact_force_event_threshold: Real::MAX, } } @@ -681,6 +695,12 @@ impl ColliderBuilder { self } + /// Sets the total force magnitude beyond which a contact force event can be emitted. + pub fn contact_force_event_threshold(mut self, threshold: Real) -> Self { + self.contact_force_event_threshold = threshold; + self + } + /// Sets the initial translation of the collider to be created. /// /// If the collider will be attached to a rigid-body, this sets the translation relative to the @@ -725,34 +745,6 @@ impl ColliderBuilder { /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { - let (changes, pos, bf_data, shape, coll_type, material, flags, mprops) = self.components(); - Collider { - shape, - mprops, - material, - parent: None, - changes, - pos, - bf_data, - flags, - coll_type, - user_data: self.user_data, - } - } - - /// Builds all the components required by a collider. - pub fn components( - &self, - ) -> ( - ColliderChanges, - ColliderPosition, - ColliderBroadPhaseData, - ColliderShape, - ColliderType, - ColliderMaterial, - ColliderFlags, - ColliderMassProps, - ) { let mass_info = if let Some(mp) = self.mass_properties { ColliderMassProps::MassProperties(Box::new(mp)) } else { @@ -785,9 +777,19 @@ impl ColliderBuilder { ColliderType::Solid }; - ( - changes, pos, bf_data, shape, coll_type, material, flags, mprops, - ) + Collider { + shape, + mprops, + material, + parent: None, + changes, + pos, + bf_data, + flags, + coll_type, + contact_force_event_threshold: self.contact_force_event_threshold, + user_data: self.user_data, + } } } diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 551ffde..37f157e 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -141,6 +141,36 @@ impl ContactPair { self.workspace = None; } + /// The sum of all the impulses applied by contacts on this contact pair. + pub fn total_impulse(&self) -> Vector { + self.manifolds + .iter() + .map(|m| m.total_impulse() * m.data.normal) + .sum() + } + + /// The sum of the magnitudes of the contacts on this contact pair. + pub fn total_impulse_magnitude(&self) -> Real { + self.manifolds + .iter() + .fold(0.0, |a, m| a + m.total_impulse()) + } + + /// The magnitude and (unit) direction of the maximum impulse on this contact pair. + pub fn max_impulse(&self) -> (Real, Vector) { + let mut result = (0.0, Vector::zeros()); + + for m in &self.manifolds { + let impulse = m.total_impulse(); + + if impulse > result.0 { + result = (impulse, m.data.normal); + } + } + + result + } + /// Finds the contact with the smallest signed distance. /// /// If the colliders involved in this contact pair are penetrating, then @@ -316,3 +346,18 @@ impl ContactManifoldData { self.solver_contacts.len() } } + +pub trait ContactManifoldExt { + fn total_impulse(&self) -> Real; + fn max_impulse(&self) -> Real; +} + +impl ContactManifoldExt for ContactManifold { + fn total_impulse(&self) -> Real { + self.points.iter().map(|pt| pt.data.impulse).sum() + } + + fn max_impulse(&self) -> Real { + self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse)) + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 34d3707..48d71f8 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -16,6 +16,8 @@ pub use self::collider_set::ColliderSet; pub use parry::query::TrackedContact; +use crate::math::{Real, Vector}; + /// A contact between two colliders. pub type Contact = parry::query::TrackedContact; /// A contact manifold between two colliders. @@ -116,6 +118,28 @@ impl CollisionEvent { } } +#[derive(Copy, Clone, PartialEq, Debug, Default)] +/// Event occurring when the sum of the magnitudes of the contact forces +/// between two colliders exceed a threshold. +pub struct CollisionForceEvent { + /// The first collider involved in the contact. + pub collider1: ColliderHandle, + /// The second collider involved in the contact. + pub collider2: ColliderHandle, + /// The sum of all the forces between the two colliders. + pub total_force: Vector, + /// The sum of the magnitudes of each force between the two colliders. + /// + /// Note that this is **not** the same as the magnitude of `self.total_force`. + /// Here we are summing the magnitude of all the forces, instead of taking + /// the magnitude of their sum. + pub total_force_magnitude: Real, + /// The world-space (unit) direction of the force with strongest magnitude. + pub max_force_direction: Vector, + /// The magnitude of the largest force at a contact point of this contact pair. + pub max_force_magnitude: Real, +} + pub(crate) use self::broad_phase_multi_sap::SAPProxyIndex; pub(crate) use self::narrow_phase::ContactManifoldIndex; pub(crate) use parry::partitioning::QBVH; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index e7b9a34..a1256b8 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -1,6 +1,7 @@ #[cfg(feature = "parallel")] use rayon::prelude::*; +use crate::data::graph::EdgeIndex; use crate::data::Coarena; use crate::dynamics::{ CoefficientCombineRule, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType, @@ -8,7 +9,7 @@ use crate::dynamics::{ use crate::geometry::{ BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, ContactPair, - InteractionGraph, IntersectionPair, SolverContact, SolverFlags, + InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex, }; use crate::math::{Real, Vector}; use crate::pipeline::{ @@ -164,6 +165,11 @@ impl NarrowPhase { }) } + /// Returns the contact pair at the given temporary index. + pub fn contact_pair_at_index(&self, id: TemporaryInteractionIndex) -> &ContactPair { + &self.contact_graph.graph.edges[id.index()].weight + } + /// The contact pair involving two specific colliders. /// /// It is strongly recommended to use the [`NarrowPhase::contact_pair`] method instead. This @@ -975,6 +981,7 @@ impl NarrowPhase { &'a mut self, islands: &IslandManager, bodies: &RigidBodySet, + out_contact_pairs: &mut Vec, out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut Vec>, ) { @@ -983,7 +990,9 @@ impl NarrowPhase { } // TODO: don't iterate through all the interactions. - for inter in self.contact_graph.graph.edges.iter_mut() { + for (pair_id, inter) in self.contact_graph.graph.edges.iter_mut().enumerate() { + let mut push_pair = false; + for manifold in &mut inter.weight.manifolds { if manifold .data @@ -1027,9 +1036,14 @@ impl NarrowPhase { out[island_index].push(out_manifolds.len()); out_manifolds.push(manifold); + push_pair = true; } } } + + if push_pair { + out_contact_pairs.push(EdgeIndex::new(pair_id as u32)); + } } } } diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs index 24b415c..55ede93 100644 --- a/src/pipeline/event_handler.rs +++ b/src/pipeline/event_handler.rs @@ -1,5 +1,6 @@ use crate::dynamics::RigidBodySet; -use crate::geometry::{ColliderSet, CollisionEvent, ContactPair}; +use crate::geometry::{ColliderSet, CollisionEvent, CollisionForceEvent, ContactPair}; +use crate::math::Real; use crossbeam::channel::Sender; bitflags::bitflags! { @@ -25,6 +26,8 @@ pub trait EventHandler: Send + Sync { /// Handle a collision event. /// /// A collision event is emitted when the state of intersection between two colliders changes. + /// At least one of the involved colliders must have the `ActiveEvents::COLLISION_EVENTS` flag + /// set. /// /// # Parameters /// * `event` - The collision event. @@ -40,6 +43,26 @@ pub trait EventHandler: Send + Sync { event: CollisionEvent, contact_pair: Option<&ContactPair>, ); + + /// Handle a force event. + /// + /// A force event is generated whenever the total force magnitude applied between two + /// colliders is `> Collider::contact_force_event_threshold` value of any of these + /// colliders. + /// + /// The "total force magnitude" here means "the sum of the magnitudes of the forces applied at + /// all the contact points in a contact pair". Therefore, if the contact pair involves two + /// forces `{0.0, 1.0, 0.0}` and `{0.0, -1.0, 0.0}`, then the total force magnitude tested + /// against the `contact_force_event_threshold` is `2.0` even if the sum of these forces is actually the + /// zero vector. + fn handle_contact_force_event( + &self, + dt: Real, + bodies: &RigidBodySet, + colliders: &ColliderSet, + contact_pair: &ContactPair, + total_force_magnitude: Real, + ); } impl EventHandler for () { @@ -51,17 +74,34 @@ impl EventHandler for () { _contact_pair: Option<&ContactPair>, ) { } + + fn handle_contact_force_event( + &self, + _dt: Real, + _bodies: &RigidBodySet, + _colliders: &ColliderSet, + _contact_pair: &ContactPair, + _total_force_magnitude: Real, + ) { + } } /// A collision event handler that collects events into a crossbeam channel. pub struct ChannelEventCollector { - event_sender: Sender, + collision_event_sender: Sender, + contact_force_event_sender: Sender, } impl ChannelEventCollector { /// Initialize a new collision event handler from crossbeam channel senders. - pub fn new(event_sender: Sender) -> Self { - Self { event_sender } + pub fn new( + collision_event_sender: Sender, + contact_force_event_sender: Sender, + ) -> Self { + Self { + collision_event_sender, + contact_force_event_sender, + } } } @@ -73,6 +113,47 @@ impl EventHandler for ChannelEventCollector { event: CollisionEvent, _: Option<&ContactPair>, ) { - let _ = self.event_sender.send(event); + let _ = self.collision_event_sender.send(event); + } + + fn handle_contact_force_event( + &self, + dt: Real, + _bodies: &RigidBodySet, + _colliders: &ColliderSet, + contact_pair: &ContactPair, + total_force_magnitude: Real, + ) { + let mut result = CollisionForceEvent { + collider1: contact_pair.collider1, + collider2: contact_pair.collider2, + total_force_magnitude, + ..CollisionForceEvent::default() + }; + + for m in &contact_pair.manifolds { + let mut total_manifold_impulse = 0.0; + for pt in m.contacts() { + total_manifold_impulse += pt.data.impulse; + + if pt.data.impulse > result.max_force_magnitude { + result.max_force_magnitude = pt.data.impulse; + result.max_force_direction = m.data.normal; + } + } + + result.total_force += m.data.normal * total_manifold_impulse; + } + + let inv_dt = crate::utils::inv(dt); + // NOTE: convert impulses to forces. Note that we + // don’t need to convert the `total_force_magnitude` + // because it’s an input of this function already + // assumed to be a force instead of an impulse. + result.total_force *= inv_dt; + result.max_force_direction *= inv_dt; + result.max_force_magnitude *= inv_dt; + + let _ = self.contact_force_event_sender.send(result); } } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 16f2830..ea0f239 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -11,10 +11,10 @@ use crate::dynamics::{ use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, - ContactManifoldIndex, NarrowPhase, + ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex, }; use crate::math::{Real, Vector}; -use crate::pipeline::{EventHandler, PhysicsHooks}; +use crate::pipeline::{ActiveEvents, EventHandler, PhysicsHooks}; use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; /// The physics pipeline, responsible for stepping the whole physics simulation. @@ -31,6 +31,7 @@ use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; pub struct PhysicsPipeline { /// Counters used for benchmarking only. pub counters: Counters, + contact_pair_indices: Vec, manifold_indices: Vec>, joint_constraint_indices: Vec>, broadphase_collider_pairs: Vec, @@ -55,11 +56,12 @@ impl PhysicsPipeline { pub fn new() -> PhysicsPipeline { PhysicsPipeline { counters: Counters::new(true), - solvers: Vec::new(), - manifold_indices: Vec::new(), - joint_constraint_indices: Vec::new(), - broadphase_collider_pairs: Vec::new(), - broad_phase_events: Vec::new(), + solvers: vec![], + contact_pair_indices: vec![], + manifold_indices: vec![], + joint_constraint_indices: vec![], + broadphase_collider_pairs: vec![], + broad_phase_events: vec![], } } @@ -148,6 +150,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, impulse_joints: &mut ImpulseJointSet, multibody_joints: &mut MultibodyJointSet, + events: &dyn EventHandler, ) { self.counters.stages.island_construction_time.resume(); islands.update_active_set_with_contacts( @@ -175,6 +178,7 @@ impl PhysicsPipeline { narrow_phase.select_active_contacts( islands, bodies, + &mut self.contact_pair_indices, &mut manifolds, &mut self.manifold_indices, ); @@ -275,6 +279,34 @@ impl PhysicsPipeline { }); }); } + + // Generate contact force events if needed. + let inv_dt = crate::utils::inv(integration_parameters.dt); + for pair_id in self.contact_pair_indices.drain(..) { + let pair = narrow_phase.contact_pair_at_index(pair_id); + let co1 = &colliders[pair.collider1]; + let co2 = &colliders[pair.collider2]; + let threshold = co1 + .contact_force_event_threshold + .min(co2.contact_force_event_threshold); + + if threshold < Real::MAX { + let total_magnitude = pair.total_impulse_magnitude() * inv_dt; + + // NOTE: the strict inequality is important here, so we don’t + // trigger an event if the force is 0.0 and the threshold is 0.0. + if total_magnitude > threshold { + events.handle_contact_force_event( + integration_parameters.dt, + bodies, + colliders, + pair, + total_magnitude, + ); + } + } + } + self.counters.stages.solver_time.pause(); } @@ -507,6 +539,7 @@ impl PhysicsPipeline { colliders, impulse_joints, multibody_joints, + events, ); // If CCD is enabled, execute the CCD motion clamping. -- cgit From b9f76e2fda96fad6fb0f77dc2ef2314aca84f5a4 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Fri, 1 Jul 2022 11:57:24 +0200 Subject: Warning fixes --- src/geometry/contact_pair.rs | 3 +++ src/pipeline/physics_pipeline.rs | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 37f157e..3bef40d 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -347,8 +347,11 @@ impl ContactManifoldData { } } +/// Additional methods for the contact manifold. pub trait ContactManifoldExt { + /// Computes the sum of all the impulses applied by contacts from this contact manifold. fn total_impulse(&self) -> Real; + /// Computes the maximum impulse applied by contacts from this contact manifold. fn max_impulse(&self) -> Real; } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index ea0f239..d1c3b65 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -14,7 +14,7 @@ use crate::geometry::{ ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex, }; use crate::math::{Real, Vector}; -use crate::pipeline::{ActiveEvents, EventHandler, PhysicsHooks}; +use crate::pipeline::{EventHandler, PhysicsHooks}; use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; /// The physics pipeline, responsible for stepping the whole physics simulation. -- cgit