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

Remove the narrow-phase graph indices from the collider.

Browse Source 
They are on the narrow-phase now.
This commit is contained in:
Crozet Sébastien
2020-11-19 11:07:00 +01:00
parent c641114f01
commit af39ec54d3
8 changed files with 235 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
src/data/coarena.rs Normal file
View File

@@ -0,0 +1,71 @@
use crate::data::arena::Index;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug)]
/// A container for data associated to item existing into another Arena.
pub struct Coarena<T> {
data: Vec<(u64, T)>,
}
impl<T> Coarena<T> {
/// A coarena with no element.
pub fn new() -> Self {
Self { data: Vec::new() }
}
/// Gets a specific element from the coarena, if it exists.
pub fn get(&self, index: Index) -> Option<&T> {
let (i, g) = index.into_raw_parts();
self.data
.get(i)
.and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
}
/// Gets a mutable reference to a specific element from the coarena, if it exists.
pub fn get_mut(&mut self, index: Index) -> Option<&mut T> {
let (i, g) = index.into_raw_parts();
self.data
.get_mut(i)
.and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
}
/// Ensure that elements at the two given indices exist in this coarena, and return their reference.
///
/// Missing elements are created automatically and initialized with the `default` value.
pub fn ensure_pair_exists(&mut self, a: Index, b: Index, default: T) -> (&mut T, &mut T)
where
T: Clone,
{
let (i1, g1) = a.into_raw_parts();
let (i2, g2) = b.into_raw_parts();
assert_ne!(i1, i2, "Cannot index the same object twice.");
let (elt1, elt2) = if i1 > i2 {
if self.data.len() <= i1 {
self.data.resize(i1 + 1, (u32::MAX as u64, default.clone()));
}
let (left, right) = self.data.split_at_mut(i1);
(&mut right[0], &mut left[i2])
} else {
// i2 > i1
if self.data.len() <= i2 {
self.data.resize(i2 + 1, (u32::MAX as u64, default.clone()));
}
let (left, right) = self.data.split_at_mut(i2);
(&mut left[i1], &mut right[0])
};
if elt1.0 != g1 {
*elt1 = (g1, default.clone());
}
if elt2.0 != g2 {
*elt2 = (g2, default);
}
(&mut elt1.1, &mut elt2.1)
}
}

2
src/data/mod.rs
View File

@@ -1,8 +1,10 @@
//! Data structures modified with guaranteed deterministic behavior after deserialization.
pub use self::coarena::Coarena;
pub use self::maybe_serializable_data::MaybeSerializableData;
pub mod arena;
mod coarena;
pub(crate) mod graph;
pub(crate) mod hashmap;
mod maybe_serializable_data;

32
src/dynamics/rigid_body_set.rs
View File

@@ -3,7 +3,7 @@ use rayon::prelude::*;
use crate::data::arena::Arena;
use crate::dynamics::{BodyStatus, Joint, JointSet, RigidBody};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph};
use crate::geometry::{ColliderHandle, ColliderSet, ContactPair, InteractionGraph, NarrowPhase};
use crossbeam::channel::{Receiver, Sender};
use std::ops::{Deref, DerefMut, Index, IndexMut};
@@ -452,7 +452,7 @@ impl RigidBodySet {
pub(crate) fn update_active_set_with_contacts(
&mut self,
colliders: &ColliderSet,
contact_graph: &InteractionGraph<ContactPair>,
narrow_phase: &NarrowPhase,
joint_graph: &InteractionGraph<Joint>,
min_island_size: usize,
) {
@@ -491,20 +491,22 @@ impl RigidBodySet {
fn push_contacting_colliders(
rb: &RigidBody,
colliders: &ColliderSet,
contact_graph: &InteractionGraph<ContactPair>,
narrow_phase: &NarrowPhase,
stack: &mut Vec<ColliderHandle>,
) {
for collider_handle in &rb.colliders {
let collider = &colliders[*collider_handle];
for inter in contact_graph.interactions_with(collider.contact_graph_index) {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other =
crate::utils::other_handle((inter.0, inter.1), *collider_handle);
let other_body = colliders[other].parent;
stack.push(other_body);
break;
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
let other = crate::utils::other_handle(
(inter.0, inter.1),
*collider_handle,
);
let other_body = colliders[other].parent;
stack.push(other_body);
break;
}
}
}
}
@@ -522,7 +524,7 @@ impl RigidBodySet {
continue;
}
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack);
}
// println!("Selection: {}", instant::now() - t);
@@ -565,7 +567,7 @@ impl RigidBodySet {
// Transmit the active state to all the rigid-bodies with colliders
// in contact or joined with this collider.
push_contacting_colliders(rb, colliders, contact_graph, &mut self.stack);
push_contacting_colliders(rb, colliders, narrow_phase, &mut self.stack);
for inter in joint_graph.interactions_with(rb.joint_graph_index) {
let other = crate::utils::other_handle((inter.0, inter.1), handle);

7
src/geometry/collider.rs
View File

@@ -11,6 +11,7 @@ use ncollide::bounding_volume::AABB;
use std::ops::Deref;
use std::sync::Arc;
// TODO: move this to its own file.
/// The shape of a collider.
#[derive(Clone)]
pub struct ColliderShape(pub Arc<dyn Shape>);
@@ -206,8 +207,6 @@ pub struct Collider {
pub restitution: f32,
pub(crate) collision_groups: InteractionGroups,
pub(crate) solver_groups: InteractionGroups,
pub(crate) contact_graph_index: ColliderGraphIndex,
pub(crate) proximity_graph_index: ColliderGraphIndex,
pub(crate) proxy_index: usize,
/// User-defined data associated to this rigid-body.
pub user_data: u128,
@@ -216,8 +215,6 @@ pub struct Collider {
impl Collider {
pub(crate) fn reset_internal_references(&mut self) {
self.parent = RigidBodySet::invalid_handle();
self.contact_graph_index = InteractionGraph::<Contact>::invalid_graph_index();
self.proximity_graph_index = InteractionGraph::<Proximity>::invalid_graph_index();
self.proxy_index = crate::INVALID_USIZE;
}
@@ -533,8 +530,6 @@ impl ColliderBuilder {
parent: RigidBodySet::invalid_handle(),
position: Isometry::identity(),
predicted_position: Isometry::identity(),
contact_graph_index: InteractionGraph::<Contact>::invalid_graph_index(),
proximity_graph_index: InteractionGraph::<Proximity>::invalid_graph_index(),
proxy_index: crate::INVALID_USIZE,
collision_groups: self.collision_groups,
solver_groups: self.solver_groups,

4
src/geometry/collider_set.rs
View File

@@ -11,8 +11,6 @@ pub type ColliderHandle = crate::data::arena::Index;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub(crate) struct RemovedCollider {
pub handle: ColliderHandle,
pub(crate) contact_graph_index: ColliderGraphIndex,
pub(crate) proximity_graph_index: ColliderGraphIndex,
pub(crate) proxy_index: usize,
}
@@ -105,8 +103,6 @@ impl ColliderSet {
*/
let message = RemovedCollider {
handle,
contact_graph_index: collider.contact_graph_index,
proximity_graph_index: collider.proximity_graph_index,
proxy_index: collider.proxy_index,
};

216
src/geometry/narrow_phase.rs
View File

@@ -22,17 +22,35 @@ use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGrap
//#[cfg(feature = "simd-is-enabled")]
//use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::ncollide::query::Proximity;
use crate::pipeline::EventHandler;
use std::collections::HashMap;
//use simba::simd::SimdValue;
#[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(),
}
}
}
/// The narrow-phase responsible for computing precise contact information between colliders.
#[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>>,
// ball_ball: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
// shape_shape: Vec<usize>, // Workspace: Vec<*mut ContactPair>,
@@ -48,6 +66,7 @@ impl NarrowPhase {
Self {
contact_graph: InteractionGraph::new(),
proximity_graph: InteractionGraph::new(),
graph_indices: Coarena::new(),
removed_colliders: None,
// ball_ball: Vec::new(),
// shape_shape: Vec::new(),
@@ -66,6 +85,27 @@ impl NarrowPhase {
&self.proximity_graph
}
/// All the contacts involving the given collider.
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))
}
/// All the proximities involving the given collider.
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),
)
}
// #[cfg(feature = "parallel")]
// pub fn contact_pairs(&self) -> &[ContactPair] {
// &self.contact_graph.interactions
@@ -94,29 +134,30 @@ impl NarrowPhase {
// by the contact/proximity graphs when a node is removed.
let mut prox_id_remap = HashMap::new();
let mut contact_id_remap = HashMap::new();
let mut i = 0;
for i in 0.. {
if let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
let proximity_graph_id = prox_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(collider.proximity_graph_index);
let contact_graph_id = contact_id_remap
.get(&collider.handle)
.copied()
.unwrap_or(collider.contact_graph_index);
while let Some(collider) = colliders.removed_colliders.read_ith(&cursor, i) {
let graph_idx = self.graph_indices.get(collider.handle).unwrap();
self.remove_collider(
proximity_graph_id,
contact_graph_id,
colliders,
bodies,
&mut prox_id_remap,
&mut contact_id_remap,
);
} else {
break;
}
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);
@@ -146,7 +187,7 @@ impl NarrowPhase {
// We have to manage the fact that one other collider will
// have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self.proximity_graph.remove_node(proximity_graph_id) {
if let Some(replacement) = colliders.get_mut(replacement) {
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);
@@ -154,7 +195,7 @@ impl NarrowPhase {
}
if let Some(replacement) = self.contact_graph.remove_node(contact_graph_id) {
if let Some(replacement) = colliders.get_mut(replacement) {
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);
@@ -172,69 +213,87 @@ impl NarrowPhase {
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)
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if co1.parent == co2.parent {
// Same parents. Ignore collisions.
continue;
}
if co1.is_sensor() || co2.is_sensor() {
let gid1 = co1.proximity_graph_index;
let gid2 = co2.proximity_graph_index;
let (gid1, gid2) = self.graph_indices.ensure_pair_exists(
pair.collider1,
pair.collider2,
ColliderGraphIndices::invalid(),
);
if co1.is_sensor() || co2.is_sensor() {
// 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 =
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) {
co2.proximity_graph_index =
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, gid2).is_none() {
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(
co1.proximity_graph_index,
co2.proximity_graph_index,
gid1.proximity_graph_index,
gid2.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 =
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) {
co2.contact_graph_index =
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, gid2).is_none() {
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(
co1.contact_graph_index,
co2.contact_graph_index,
gid1.contact_graph_index,
gid2.contact_graph_index,
interaction,
);
}
@@ -243,41 +302,50 @@ impl NarrowPhase {
}
BroadPhasePairEvent::DeletePair(pair) => {
if let (Some(co1), Some(co2)) =
colliders.get2_mut_internal(pair.collider1, pair.collider2)
(colliders.get(pair.collider1), colliders.get(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);
// TODO: could we just unwrap here?
// Don't we have the guarantee that we will get a `AddPair` before a `DeletePair`?
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,
);
// 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)
// 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);
} else {
let contact_pair = self.contact_graph.remove_edge(
gid1.contact_graph_index,
gid2.contact_graph_index,
);
// Emit a contact stopped event if we had a contact before removing the edge.
// Also wake up the dynamic bodies that were in contact.
if let Some(ctct) = contact_pair {
if ctct.has_any_active_contact() {
bodies.wake_up(co1.parent, true);
bodies.wake_up(co2.parent, true);
// Emit a contact stopped event if we had a contact before removing the edge.
// Also wake up the dynamic bodies that were in contact.
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,
))
events.handle_contact_event(ContactEvent::Stopped(
pair.collider1,
pair.collider2,
))
}
}
}
}

2
src/pipeline/collision_pipeline.rs
View File

@@ -74,7 +74,7 @@ impl CollisionPipeline {
bodies.update_active_set_with_contacts(
colliders,
narrow_phase.contact_graph(),
narrow_phase,
self.empty_joints.joint_graph(),
0,
);

2
src/pipeline/physics_pipeline.rs
View File

@@ -130,7 +130,7 @@ impl PhysicsPipeline {
self.counters.stages.island_construction_time.start();
bodies.update_active_set_with_contacts(
colliders,
narrow_phase.contact_graph(),
narrow_phase,
joints.joint_graph(),
integration_parameters.min_island_size,
);