First public release of Rapier.
This commit is contained in:
Sébastien Crozet
483
src/geometry/narrow_phase.rs
Normal file
483
src/geometry/narrow_phase.rs
Normal file
@@ -0,0 +1,483 @@
|
||||
#[cfg(feature = "parallel")]
|
||||
use rayon::prelude::*;
|
||||
|
||||
use crate::dynamics::RigidBodySet;
|
||||
use crate::geometry::contact_generator::{
|
||||
ContactDispatcher, ContactGenerationContext, DefaultContactDispatcher,
|
||||
};
|
||||
use crate::geometry::proximity_detector::{
|
||||
DefaultProximityDispatcher, ProximityDetectionContext, ProximityDispatcher,
|
||||
};
|
||||
//#[cfg(feature = "simd-is-enabled")]
|
||||
//use crate::geometry::{
|
||||
// contact_generator::ContactGenerationContextSimd,
|
||||
// proximity_detector::ProximityDetectionContextSimd, WBall,
|
||||
//};
|
||||
use crate::geometry::{
|
||||
BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair,
|
||||
};
|
||||
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
|
||||
//#[cfg(feature = "simd-is-enabled")]
|
||||
//use crate::math::{SimdFloat, SIMD_WIDTH};
|
||||
use crate::ncollide::query::Proximity;
|
||||
use crate::pipeline::EventHandler;
|
||||
//use simba::simd::SimdValue;
|
||||
|
||||
/// The narrow-phase responsible for computing precise contact information between colliders.
|
||||
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
|
||||
pub struct NarrowPhase {
|
||||
contact_graph: InteractionGraph<ContactPair>,
|
||||
proximity_graph: InteractionGraph<ProximityPair>,
|
||||
// ball_ball: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
|
||||
// shape_shape: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
|
||||
// ball_ball_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>,
|
||||
// shape_shape_prox: Vec<usize>, // Workspace: Vec<*mut ProximityPair>,
|
||||
}
|
||||
|
||||
pub(crate) type ContactManifoldIndex = usize;
|
||||
|
||||
impl NarrowPhase {
|
||||
/// Creates a new empty narrow-phase.
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
contact_graph: InteractionGraph::new(),
|
||||
proximity_graph: InteractionGraph::new(),
|
||||
// ball_ball: Vec::new(),
|
||||
// shape_shape: Vec::new(),
|
||||
// ball_ball_prox: Vec::new(),
|
||||
// shape_shape_prox: Vec::new(),
|
||||
}
|
||||
}
|
||||
|
||||
/// The contact graph containing all contact pairs and their contact information.
|
||||
pub fn contact_graph(&self) -> &InteractionGraph<ContactPair> {
|
||||
&self.contact_graph
|
||||
}
|
||||
|
||||
/// The proximity graph containing all proximity pairs and their proximity information.
|
||||
pub fn proximity_graph(&self) -> &InteractionGraph<ProximityPair> {
|
||||
&self.proximity_graph
|
||||
}
|
||||
|
||||
// #[cfg(feature = "parallel")]
|
||||
// pub fn contact_pairs(&self) -> &[ContactPair] {
|
||||
// &self.contact_graph.interactions
|
||||
// }
|
||||
|
||||
// pub fn contact_pairs_mut(&mut self) -> &mut [ContactPair] {
|
||||
// &mut self.contact_graph.interactions
|
||||
// }
|
||||
|
||||
// #[cfg(feature = "parallel")]
|
||||
// pub(crate) fn contact_pairs_vec_mut(&mut self) -> &mut Vec<ContactPair> {
|
||||
// &mut self.contact_graph.interactions
|
||||
// }
|
||||
|
||||
pub(crate) fn remove_colliders(
|
||||
&mut self,
|
||||
handles: &[ColliderHandle],
|
||||
colliders: &mut ColliderSet,
|
||||
bodies: &mut RigidBodySet,
|
||||
) {
|
||||
for handle in handles {
|
||||
if let Some(collider) = colliders.get(*handle) {
|
||||
let proximity_graph_id = collider.proximity_graph_index;
|
||||
let contact_graph_id = collider.contact_graph_index;
|
||||
|
||||
// Wake up every body in contact with the deleted collider.
|
||||
for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) {
|
||||
if let Some(parent) = colliders.get(a).map(|c| c.parent) {
|
||||
bodies.wake_up(parent)
|
||||
}
|
||||
|
||||
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
|
||||
bodies.wake_up(parent)
|
||||
}
|
||||
}
|
||||
|
||||
// We have to manage the fact that one other collider will
|
||||
// hive its graph index changed because of the node's swap-remove.
|
||||
if let Some(replacement) = self
|
||||
.proximity_graph
|
||||
.remove_node(proximity_graph_id)
|
||||
.and_then(|h| colliders.get_mut(h))
|
||||
{
|
||||
replacement.proximity_graph_index = proximity_graph_id;
|
||||
}
|
||||
|
||||
if let Some(replacement) = self
|
||||
.contact_graph
|
||||
.remove_node(contact_graph_id)
|
||||
.and_then(|h| colliders.get_mut(h))
|
||||
{
|
||||
replacement.contact_graph_index = contact_graph_id;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn register_pairs(
|
||||
&mut self,
|
||||
colliders: &mut ColliderSet,
|
||||
broad_phase_events: &[BroadPhasePairEvent],
|
||||
events: &dyn EventHandler,
|
||||
) {
|
||||
for event in broad_phase_events {
|
||||
match event {
|
||||
BroadPhasePairEvent::AddPair(pair) => {
|
||||
// println!("Adding pair: {:?}", *pair);
|
||||
if let (Some(co1), Some(co2)) =
|
||||
colliders.get2_mut_internal(pair.collider1, pair.collider2)
|
||||
{
|
||||
if co1.is_sensor() || co2.is_sensor() {
|
||||
let gid1 = co1.proximity_graph_index;
|
||||
let gid2 = co2.proximity_graph_index;
|
||||
|
||||
// NOTE: the collider won't have a graph index as long
|
||||
// as it does not interact with anything.
|
||||
if !InteractionGraph::<ProximityPair>::is_graph_index_valid(gid1) {
|
||||
co1.proximity_graph_index =
|
||||
self.proximity_graph.graph.add_node(pair.collider1);
|
||||
}
|
||||
|
||||
if !InteractionGraph::<ProximityPair>::is_graph_index_valid(gid2) {
|
||||
co2.proximity_graph_index =
|
||||
self.proximity_graph.graph.add_node(pair.collider2);
|
||||
}
|
||||
|
||||
if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() {
|
||||
let dispatcher = DefaultProximityDispatcher;
|
||||
let generator = dispatcher.dispatch(co1.shape(), co2.shape());
|
||||
let interaction =
|
||||
ProximityPair::new(*pair, generator.0, generator.1);
|
||||
let _ = self.proximity_graph.add_edge(
|
||||
co1.proximity_graph_index,
|
||||
co2.proximity_graph_index,
|
||||
interaction,
|
||||
);
|
||||
}
|
||||
} else {
|
||||
// NOTE: same code as above, but for the contact graph.
|
||||
// TODO: refactor both pieces of code somehow?
|
||||
let gid1 = co1.contact_graph_index;
|
||||
let gid2 = co2.contact_graph_index;
|
||||
|
||||
// NOTE: the collider won't have a graph index as long
|
||||
// as it does not interact with anything.
|
||||
if !InteractionGraph::<ContactPair>::is_graph_index_valid(gid1) {
|
||||
co1.contact_graph_index =
|
||||
self.contact_graph.graph.add_node(pair.collider1);
|
||||
}
|
||||
|
||||
if !InteractionGraph::<ContactPair>::is_graph_index_valid(gid2) {
|
||||
co2.contact_graph_index =
|
||||
self.contact_graph.graph.add_node(pair.collider2);
|
||||
}
|
||||
|
||||
if self.contact_graph.graph.find_edge(gid1, gid2).is_none() {
|
||||
let dispatcher = DefaultContactDispatcher;
|
||||
let generator = dispatcher.dispatch(co1.shape(), co2.shape());
|
||||
let interaction = ContactPair::new(*pair, generator.0, generator.1);
|
||||
let _ = self.contact_graph.add_edge(
|
||||
co1.contact_graph_index,
|
||||
co2.contact_graph_index,
|
||||
interaction,
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
BroadPhasePairEvent::DeletePair(pair) => {
|
||||
if let (Some(co1), Some(co2)) =
|
||||
colliders.get2_mut_internal(pair.collider1, pair.collider2)
|
||||
{
|
||||
if co1.is_sensor() || co2.is_sensor() {
|
||||
let prox_pair = self
|
||||
.proximity_graph
|
||||
.remove_edge(co1.proximity_graph_index, co2.proximity_graph_index);
|
||||
|
||||
// Emit a proximity lost event if we had a proximity before removing the edge.
|
||||
if let Some(prox) = prox_pair {
|
||||
if prox.proximity != Proximity::Disjoint {
|
||||
let prox_event = ProximityEvent::new(
|
||||
pair.collider1,
|
||||
pair.collider2,
|
||||
prox.proximity,
|
||||
Proximity::Disjoint,
|
||||
);
|
||||
events.handle_proximity_event(prox_event)
|
||||
}
|
||||
}
|
||||
} else {
|
||||
let contact_pair = self
|
||||
.contact_graph
|
||||
.remove_edge(co1.contact_graph_index, co2.contact_graph_index);
|
||||
|
||||
// Emit a contact stopped event if we had a proximity before removing the edge.
|
||||
if let Some(ctct) = contact_pair {
|
||||
if ctct.has_any_active_contact() {
|
||||
events.handle_contact_event(ContactEvent::Stopped(
|
||||
pair.collider1,
|
||||
pair.collider2,
|
||||
))
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn compute_proximities(
|
||||
&mut self,
|
||||
prediction_distance: f32,
|
||||
bodies: &RigidBodySet,
|
||||
colliders: &ColliderSet,
|
||||
events: &dyn EventHandler,
|
||||
) {
|
||||
par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| {
|
||||
let pair = &mut edge.weight;
|
||||
let co1 = &colliders[pair.pair.collider1];
|
||||
let co2 = &colliders[pair.pair.collider2];
|
||||
|
||||
// FIXME: avoid lookup into bodies.
|
||||
let rb1 = &bodies[co1.parent];
|
||||
let rb2 = &bodies[co2.parent];
|
||||
|
||||
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
|
||||
{
|
||||
// No need to update this contact because nothing moved.
|
||||
return;
|
||||
}
|
||||
|
||||
let dispatcher = DefaultProximityDispatcher;
|
||||
if pair.detector.is_none() {
|
||||
// We need a redispatch for this detector.
|
||||
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
|
||||
let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
|
||||
pair.detector = Some(detector);
|
||||
pair.detector_workspace = workspace;
|
||||
}
|
||||
|
||||
let context = ProximityDetectionContext {
|
||||
dispatcher: &dispatcher,
|
||||
prediction_distance,
|
||||
colliders,
|
||||
pair,
|
||||
};
|
||||
|
||||
context
|
||||
.pair
|
||||
.detector
|
||||
.unwrap()
|
||||
.detect_proximity(context, events);
|
||||
});
|
||||
|
||||
/*
|
||||
// First, group pairs.
|
||||
// NOTE: the transmutes here are OK because the Vec are all cleared
|
||||
// before we leave this method.
|
||||
// We do this in order to avoid reallocating those vecs each time
|
||||
// we compute the contacts. Unsafe is necessary because we can't just
|
||||
// store a Vec<&mut ProximityPair> into the NarrowPhase struct without
|
||||
// polluting the World with lifetimes.
|
||||
let ball_ball_prox: &mut Vec<&mut ProximityPair> =
|
||||
unsafe { std::mem::transmute(&mut self.ball_ball_prox) };
|
||||
let shape_shape_prox: &mut Vec<&mut ProximityPair> =
|
||||
unsafe { std::mem::transmute(&mut self.shape_shape_prox) };
|
||||
|
||||
let bodies = &bodies.bodies;
|
||||
|
||||
// FIXME: don't iterate through all the interactions.
|
||||
for pair in &mut self.proximity_graph.interactions {
|
||||
let co1 = &colliders[pair.pair.collider1];
|
||||
let co2 = &colliders[pair.pair.collider2];
|
||||
|
||||
// FIXME: avoid lookup into bodies.
|
||||
let rb1 = &bodies[co1.parent];
|
||||
let rb2 = &bodies[co2.parent];
|
||||
|
||||
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
|
||||
{
|
||||
// No need to update this proximity because nothing moved.
|
||||
continue;
|
||||
}
|
||||
|
||||
match (co1.shape(), co2.shape()) {
|
||||
(Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair),
|
||||
_ => shape_shape_prox.push(pair),
|
||||
}
|
||||
}
|
||||
|
||||
par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| {
|
||||
let context = ProximityDetectionContextSimd {
|
||||
dispatcher: &DefaultProximityDispatcher,
|
||||
prediction_distance,
|
||||
colliders,
|
||||
pairs,
|
||||
};
|
||||
context.pairs[0]
|
||||
.detector
|
||||
.detect_proximity_simd(context, events);
|
||||
});
|
||||
|
||||
par_iter_mut!(shape_shape_prox).for_each(|pair| {
|
||||
let context = ProximityDetectionContext {
|
||||
dispatcher: &DefaultProximityDispatcher,
|
||||
prediction_distance,
|
||||
colliders,
|
||||
pair,
|
||||
};
|
||||
|
||||
context.pair.detector.detect_proximity(context, events);
|
||||
});
|
||||
|
||||
ball_ball_prox.clear();
|
||||
shape_shape_prox.clear();
|
||||
*/
|
||||
}
|
||||
|
||||
pub(crate) fn compute_contacts(
|
||||
&mut self,
|
||||
prediction_distance: f32,
|
||||
bodies: &RigidBodySet,
|
||||
colliders: &ColliderSet,
|
||||
events: &dyn EventHandler,
|
||||
) {
|
||||
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
|
||||
let pair = &mut edge.weight;
|
||||
let co1 = &colliders[pair.pair.collider1];
|
||||
let co2 = &colliders[pair.pair.collider2];
|
||||
|
||||
// FIXME: avoid lookup into bodies.
|
||||
let rb1 = &bodies[co1.parent];
|
||||
let rb2 = &bodies[co2.parent];
|
||||
|
||||
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
|
||||
{
|
||||
// No need to update this contact because nothing moved.
|
||||
return;
|
||||
}
|
||||
|
||||
let dispatcher = DefaultContactDispatcher;
|
||||
if pair.generator.is_none() {
|
||||
// We need a redispatch for this generator.
|
||||
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
|
||||
let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
|
||||
pair.generator = Some(generator);
|
||||
pair.generator_workspace = workspace;
|
||||
}
|
||||
|
||||
let context = ContactGenerationContext {
|
||||
dispatcher: &dispatcher,
|
||||
prediction_distance,
|
||||
colliders,
|
||||
pair,
|
||||
};
|
||||
|
||||
context
|
||||
.pair
|
||||
.generator
|
||||
.unwrap()
|
||||
.generate_contacts(context, events);
|
||||
});
|
||||
|
||||
/*
|
||||
// First, group pairs.
|
||||
// NOTE: the transmutes here are OK because the Vec are all cleared
|
||||
// before we leave this method.
|
||||
// We do this in order to avoid reallocating those vecs each time
|
||||
// we compute the contacts. Unsafe is necessary because we can't just
|
||||
// store a Vec<&mut ContactPair> into the NarrowPhase struct without
|
||||
// polluting the World with lifetimes.
|
||||
let ball_ball: &mut Vec<&mut ContactPair> =
|
||||
unsafe { std::mem::transmute(&mut self.ball_ball) };
|
||||
let shape_shape: &mut Vec<&mut ContactPair> =
|
||||
unsafe { std::mem::transmute(&mut self.shape_shape) };
|
||||
|
||||
let bodies = &bodies.bodies;
|
||||
|
||||
// FIXME: don't iterate through all the interactions.
|
||||
for pair in &mut self.contact_graph.interactions {
|
||||
let co1 = &colliders[pair.pair.collider1];
|
||||
let co2 = &colliders[pair.pair.collider2];
|
||||
|
||||
// FIXME: avoid lookup into bodies.
|
||||
let rb1 = &bodies[co1.parent];
|
||||
let rb2 = &bodies[co2.parent];
|
||||
|
||||
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
|
||||
{
|
||||
// No need to update this contact because nothing moved.
|
||||
continue;
|
||||
}
|
||||
|
||||
match (co1.shape(), co2.shape()) {
|
||||
(Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair),
|
||||
_ => shape_shape.push(pair),
|
||||
}
|
||||
}
|
||||
|
||||
par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| {
|
||||
let context = ContactGenerationContextSimd {
|
||||
dispatcher: &DefaultContactDispatcher,
|
||||
prediction_distance,
|
||||
colliders,
|
||||
pairs,
|
||||
};
|
||||
context.pairs[0]
|
||||
.generator
|
||||
.generate_contacts_simd(context, events);
|
||||
});
|
||||
|
||||
par_iter_mut!(shape_shape).for_each(|pair| {
|
||||
let context = ContactGenerationContext {
|
||||
dispatcher: &DefaultContactDispatcher,
|
||||
prediction_distance,
|
||||
colliders,
|
||||
pair,
|
||||
};
|
||||
|
||||
context.pair.generator.generate_contacts(context, events);
|
||||
});
|
||||
|
||||
ball_ball.clear();
|
||||
shape_shape.clear();
|
||||
*/
|
||||
}
|
||||
|
||||
/// Retrieve all the interactions with at least one contact point, happening between two active bodies.
|
||||
// NOTE: this is very similar to the code from JointSet::select_active_interactions.
|
||||
pub(crate) fn sort_and_select_active_contacts<'a>(
|
||||
&'a mut self,
|
||||
bodies: &RigidBodySet,
|
||||
out_manifolds: &mut Vec<&'a mut ContactManifold>,
|
||||
out: &mut Vec<Vec<ContactManifoldIndex>>,
|
||||
) {
|
||||
for out_island in &mut out[..bodies.num_islands()] {
|
||||
out_island.clear();
|
||||
}
|
||||
|
||||
// FIXME: don't iterate through all the interactions.
|
||||
for inter in self.contact_graph.graph.edges.iter_mut() {
|
||||
for manifold in &mut inter.weight.manifolds {
|
||||
let rb1 = &bodies[manifold.body_pair.body1];
|
||||
let rb2 = &bodies[manifold.body_pair.body2];
|
||||
if manifold.num_active_contacts() != 0
|
||||
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
|
||||
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
|
||||
{
|
||||
let island_index = if !rb1.is_dynamic() {
|
||||
rb2.active_island_id
|
||||
} else {
|
||||
rb1.active_island_id
|
||||
};
|
||||
|
||||
out[island_index].push(out_manifolds.len());
|
||||
out_manifolds.push(manifold);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user