diff options
Diffstat (limited to 'src/geometry')
| -rw-r--r-- | src/geometry/collider.rs | 64 | ||||
| -rw-r--r-- | src/geometry/contact_pair.rs | 45 | ||||
| -rw-r--r-- | src/geometry/mod.rs | 24 | ||||
| -rw-r--r-- | src/geometry/narrow_phase.rs | 18 |
4 files changed, 118 insertions, 33 deletions
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<Real> { + 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<Real>) { + 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<ContactData>; /// 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<Real>, + /// 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<Real>, + /// 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<TemporaryInteractionIndex>, out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut Vec<Vec<ContactManifoldIndex>>, ) { @@ -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)); + } } } } |