nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add contact force events generated above a user-defined threshold

Browse Source
This commit is contained in:
Sébastien Crozet
2022-06-24 19:00:34 +02:00
parent d6b6189861
commit c9d8277377
8 changed files with 255 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
src/geometry/collider.rs
View File

@@ -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,
}
}
}

45
src/geometry/contact_pair.rs
View File

@@ -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))
}
}

24
src/geometry/mod.rs
View File

@@ -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;

18
src/geometry/narrow_phase.rs
View File

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

91
src/pipeline/event_handler.rs
View File

@@ -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<CollisionEvent>,
collision_event_sender: Sender<CollisionEvent>,
contact_force_event_sender: Sender<CollisionForceEvent>,
}
impl ChannelEventCollector {
/// Initialize a new collision event handler from crossbeam channel senders.
pub fn new(event_sender: Sender<CollisionEvent>) -> Self {
Self { event_sender }
pub fn new(
collision_event_sender: Sender<CollisionEvent>,
contact_force_event_sender: Sender<CollisionForceEvent>,
) -> 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);
}
}

47
src/pipeline/physics_pipeline.rs
View File

@@ -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<TemporaryInteractionIndex>,
manifold_indices: Vec<Vec<ContactManifoldIndex>>,
joint_constraint_indices: Vec<Vec<ContactManifoldIndex>>,
broadphase_collider_pairs: Vec<ColliderPair>,
@@ -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.

9
src_testbed/harness/mod.rs
View File

@@ -86,10 +86,13 @@ type Callbacks =
#[allow(dead_code)]
impl Harness {
pub fn new_empty() -> Self {
let event_channel = crossbeam::channel::unbounded();
let event_handler = ChannelEventCollector::new(event_channel.0);
let collision_event_channel = crossbeam::channel::unbounded();
let contact_force_event_channel = crossbeam::channel::unbounded();
let event_handler =
ChannelEventCollector::new(collision_event_channel.0, contact_force_event_channel.0);
let events = PhysicsEvents {
events: event_channel.1,
collision_events: collision_event_channel.1,
contact_force_events: contact_force_event_channel.1,
};
let physics = PhysicsState::new();
let state = RunState::new();

8
src_testbed/physics/mod.rs
View File

@@ -3,7 +3,7 @@ use rapier::dynamics::{
CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, CollisionEvent, NarrowPhase};
use rapier::geometry::{BroadPhase, ColliderSet, CollisionEvent, CollisionForceEvent, NarrowPhase};
use rapier::math::{Real, Vector};
use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
@@ -107,11 +107,13 @@ impl PhysicsState {
}
pub struct PhysicsEvents {
pub events: Receiver<CollisionEvent>,
pub collision_events: Receiver<CollisionEvent>,
pub contact_force_events: Receiver<CollisionForceEvent>,
}
impl PhysicsEvents {
pub fn poll_all(&self) {
while let Ok(_) = self.events.try_recv() {}
while let Ok(_) = self.collision_events.try_recv() {}
while let Ok(_) = self.contact_force_events.try_recv() {}
}
}