#[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,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter,
PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider,
SolverFlags,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::ncollide::query::Proximity;
use crate::pipeline::EventHandler;
use std::collections::HashMap;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
struct ColliderGraphIndices {
contact_graph_index: ColliderGraphIndex,
proximity_graph_index: ColliderGraphIndex,
}
impl ColliderGraphIndices {
fn invalid() -> Self {
Self {
contact_graph_index: InteractionGraph::<ContactPair>::invalid_graph_index(),
proximity_graph_index: InteractionGraph::<ProximityPair>::invalid_graph_index(),
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct NarrowPhase {
contact_graph: InteractionGraph<ContactPair>,
proximity_graph: InteractionGraph<ProximityPair>,
graph_indices: Coarena<ColliderGraphIndices>,
removed_colliders: Option<Subscription<RemovedCollider>>,
}
pub(crate) type ContactManifoldIndex = usize;
impl NarrowPhase {
pub fn new() -> Self {
Self {
contact_graph: InteractionGraph::new(),
proximity_graph: InteractionGraph::new(),
graph_indices: Coarena::new(),
removed_colliders: None,
}
}
pub fn contact_graph(&self) -> &InteractionGraph<ContactPair> {
&self.contact_graph
}
pub fn proximity_graph(&self) -> &InteractionGraph<ProximityPair> {
&self.proximity_graph
}
pub fn contacts_with(
&self,
collider: ColliderHandle,
) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ContactPair)>> {
let id = self.graph_indices.get(collider)?;
Some(self.contact_graph.interactions_with(id.contact_graph_index))
}
pub fn proximities_with(
&self,
collider: ColliderHandle,
) -> Option<impl Iterator<Item = (ColliderHandle, ColliderHandle, &ProximityPair)>> {
let id = self.graph_indices.get(collider)?;
Some(
self.proximity_graph
.interactions_with(id.proximity_graph_index),
)
}
pub fn contact_pair(
&self,
collider1: ColliderHandle,
collider2: ColliderHandle,
) -> Option<&ContactPair> {
let id1 = self.graph_indices.get(collider1)?;
let id2 = self.graph_indices.get(collider2)?;
self.contact_graph
.interaction_pair(id1.contact_graph_index, id2.contact_graph_index)
.map(|c| c.2)
}
pub fn proximity_pair(
&self,
collider1: ColliderHandle,
collider2: ColliderHandle,
) -> Option<&ProximityPair> {
let id1 = self.graph_indices.get(collider1)?;
let id2 = self.graph_indices.get(collider2)?;
self.proximity_graph
.interaction_pair(id1.proximity_graph_index, id2.proximity_graph_index)
.map(|c| c.2)
}
pub fn contact_pairs(&self) -> impl Iterator<Item = &ContactPair> {
self.contact_graph.interactions()
}
pub fn proximity_pairs(&self) -> impl Iterator<Item = &ProximityPair> {
self.proximity_graph.interactions()
}
pub fn maintain(&mut self, colliders: &mut ColliderSet, bodies: &mut RigidBodySet) {
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
}
let mut cursor = self.removed_colliders.take().unwrap();
let mut prox_id_remap = HashMap::new();
let mut contact_id_remap = HashMap::new();
let mut i = 0;
while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
if let Some(graph_idx) = self.graph_indices.get(collider.handle) {
let proximity_graph_id = prox_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(graph_idx.proximity_graph_index);
let contact_graph_id = contact_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(graph_idx.contact_graph_index);
self.remove_collider(
proximity_graph_id,
contact_graph_id,
colliders,
bodies,
&mut prox_id_remap,
&mut contact_id_remap,
);
}
i += 1;
}
colliders.removed_colliders.ack(&mut cursor);
self.removed_colliders = Some(cursor);
}
pub(crate) fn remove_collider<'a>(
&mut self,
proximity_graph_id: ColliderGraphIndex,
contact_graph_id: ColliderGraphIndex,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
) {
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, true)
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent, true)
}
}
if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) {
if let Some(replacement) = self.graph_indices.get_mut(replacement) {
replacement.proximity_graph_index = proximity_graph_id;
} else {
prox_id_remap.insert(replacement, proximity_graph_id);
}
}
if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
if let Some(replacement) = self.graph_indices.get_mut(replacement) {
replacement.contact_graph_index = contact_graph_id;
} else {
contact_id_remap.insert(replacement, contact_graph_id);
}
}
}
pub(crate) fn register_pairs(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
broad_phase_events: &[BroadPhasePairEvent],
events: &dyn EventHandler,
) {
for event in broad_phase_events {
match event {
BroadPhasePairEvent::AddPair(pair) => {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if co1.parent == co2.parent {
continue;
}
let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
pair.collider1,
pair.collider2,
ColliderGraphIndices::invalid(),
);
if co1.is_sensor() || co2.is_sensor() {
if !InteractionGraph::<ProximityPair>::is_graph_index_valid(
gid1.proximity_graph_index,
) {
gid1.proximity_graph_index =
self.proximity_graph.graph.add_node(pair.collider1);
}
if !InteractionGraph::<ProximityPair>::is_graph_index_valid(
gid2.proximity_graph_index,
) {
gid2.proximity_graph_index =
self.proximity_graph.graph.add_node(pair.collider2);
}
if self
.proximity_graph
.graph
.find_edge(gid1.proximity_graph_index, gid2.proximity_graph_index)
.is_none()
{
let dispatcher = DefaultProximityDispatcher;
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction =
ProximityPair::new(*pair, generator.0, generator.1);
let _ = self.proximity_graph.add_edge(
gid1.proximity_graph_index,
gid2.proximity_graph_index,
interaction,
);
}
} else {
if !InteractionGraph::<ContactPair>::is_graph_index_valid(
gid1.contact_graph_index,
) {
gid1.contact_graph_index =
self.contact_graph.graph.add_node(pair.collider1);
}
if !InteractionGraph::<ContactPair>::is_graph_index_valid(
gid2.contact_graph_index,
) {
gid2.contact_graph_index =
self.contact_graph.graph.add_node(pair.collider2);
}
if self
.contact_graph
.graph
.find_edge(gid1.contact_graph_index, gid2.contact_graph_index)
.is_none()
{
let dispatcher = DefaultContactDispatcher;
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction = ContactPair::new(*pair, generator.0, generator.1);
let _ = self.contact_graph.add_edge(
gid1.contact_graph_index,
gid2.contact_graph_index,
interaction,
);
}
}
}
}
BroadPhasePairEvent::DeletePair(pair) => {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if let (Some(gid1), Some(gid2)) = (
self.graph_indices.get(pair.collider1),
self.graph_indices.get(pair.collider2),
) {
if co1.is_sensor() || co2.is_sensor() {
let prox_pair = self.proximity_graph.remove_edge(
gid1.proximity_graph_index,
gid2.proximity_graph_index,
);
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(
gid1.contact_graph_index,
gid2.contact_graph_index,
);
if let Some(ctct) = contact_pair {
if ctct.has_any_active_contact() {
bodies.wake_up(co1.parent, true);
bodies.wake_up(co2.parent, true);
events.handle_contact_event(ContactEvent::Stopped(
pair.collider1,
pair.collider2,
))
}
}
}
}
}
}
}
}
}
pub(crate) fn compute_proximities(
&mut self,
prediction_distance: f32,
bodies: &RigidBodySet,
colliders: &ColliderSet,
pair_filter: Option<&dyn ProximityPairFilter>,
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];
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
{
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
return;
}
if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
return;
}
if let Some(filter) = pair_filter {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider1: co1,
collider2: co2,
};
if !filter.filter_proximity_pair(&context) {
return;
}
}
let dispatcher = DefaultProximityDispatcher;
if pair.detector.is_none() {
let (detector, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
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);
});
}
pub(crate) fn compute_contacts(
&mut self,
prediction_distance: f32,
bodies: &RigidBodySet,
colliders: &ColliderSet,
pair_filter: Option<&dyn ContactPairFilter>,
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];
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
{
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
return;
}
if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
return;
}
let mut solver_flags = if let Some(filter) = pair_filter {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider1: co1,
collider2: co2,
};
if let Some(solver_flags) = filter.filter_contact_pair(&context) {
solver_flags
} else {
return;
}
} else {
SolverFlags::COMPUTE_IMPULSES
};
if !co1.solver_groups.test(co2.solver_groups) {
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
}
let dispatcher = DefaultContactDispatcher;
if pair.generator.is_none() {
let (generator, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.generator = Some(generator);
if pair.generator_workspace.is_none() {
pair.generator_workspace = workspace;
}
}
let context = ContactGenerationContext {
dispatcher: &dispatcher,
prediction_distance,
colliders,
pair,
solver_flags,
};
context
.pair
.generator
.unwrap()
.generate_contacts(context, events);
});
}
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();
}
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
.solver_flags
.contains(SolverFlags::COMPUTE_IMPULSES)
&& manifold.num_active_contacts() != 0
&& (rb1.is_dynamic() || rb2.is_dynamic())
&& (!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);
}
}
}
}
}