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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

595
src/geometry/narrow_phase.rs
View File

@@ -1,14 +1,16 @@
#[cfg(feature = "parallel")]
use rayon::prelude::*;
use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet};
use crate::geometry::collider::ColliderChanges;
use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut, ComponentSetOption};
use crate::dynamics::CoefficientCombineRule;
use crate::dynamics::{
IslandManager, RigidBodyActivation, RigidBodyDominance, RigidBodyIds, RigidBodyType,
};
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet,
BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderGroups, ColliderHandle,
ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
ContactData, ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
IntersectionEvent, RemovedCollider, SolverContact, SolverFlags,
IntersectionEvent, SolverContact, SolverFlags,
};
use crate::math::{Real, Vector};
use crate::pipeline::{
@@ -54,7 +56,6 @@ pub struct NarrowPhase {
contact_graph: InteractionGraph<ColliderHandle, ContactPair>,
intersection_graph: InteractionGraph<ColliderHandle, bool>,
graph_indices: Coarena<ColliderGraphIndices>,
removed_colliders: Option<Subscription<RemovedCollider>>,
}
pub(crate) type ContactManifoldIndex = usize;
@@ -75,7 +76,6 @@ impl NarrowPhase {
contact_graph: InteractionGraph::new(),
intersection_graph: InteractionGraph::new(),
graph_indices: Coarena::new(),
removed_colliders: None,
}
}
@@ -172,75 +172,79 @@ impl NarrowPhase {
// }
/// Maintain the narrow-phase internal state by taking collider removal into account.
pub fn handle_user_changes(
pub fn handle_user_changes<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
colliders: &mut Colliders,
bodies: &mut Bodies,
events: &dyn EventHandler,
) {
// Ensure we already subscribed.
if self.removed_colliders.is_none() {
self.removed_colliders = Some(colliders.removed_colliders.subscribe());
}
let cursor = self.removed_colliders.take().unwrap();
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>,
{
// TODO: avoid these hash-maps.
// They are necessary to handle the swap-remove done internally
// by the contact/intersection graphs when a node is removed.
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) {
for collider in removed_colliders {
// NOTE: if the collider does not have any graph indices currently, there is nothing
// to remove in the narrow-phase for this collider.
if let Some(graph_idx) = self.graph_indices.get(collider.handle.0) {
if let Some(graph_idx) = self.graph_indices.get(collider.0) {
let intersection_graph_id = prox_id_remap
.get(&collider.handle)
.get(collider)
.copied()
.unwrap_or(graph_idx.intersection_graph_index);
let contact_graph_id = contact_id_remap
.get(&collider.handle)
.get(collider)
.copied()
.unwrap_or(graph_idx.contact_graph_index);
self.remove_collider(
intersection_graph_id,
contact_graph_id,
islands,
colliders,
bodies,
&mut prox_id_remap,
&mut contact_id_remap,
);
}
i += 1;
}
colliders.removed_colliders.ack(&cursor);
self.removed_colliders = Some(cursor);
self.handle_modified_colliders(colliders, bodies, events);
self.handle_modified_colliders(islands, modified_colliders, colliders, bodies, events);
}
pub(crate) fn remove_collider(
pub(crate) fn remove_collider<Bodies, Colliders>(
&mut self,
intersection_graph_id: ColliderGraphIndex,
contact_graph_id: ColliderGraphIndex,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
colliders: &mut Colliders,
bodies: &mut Bodies,
prox_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
contact_id_remap: &mut HashMap<ColliderHandle, ColliderGraphIndex>,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSetOption<ColliderParent>,
{
// 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, true)
if let Some(parent) = colliders.get(a.0).map(|c| c.handle) {
islands.wake_up(bodies, parent, true)
}
if let Some(parent) = colliders.get(b).map(|c| c.parent) {
bodies.wake_up(parent, true)
if let Some(parent) = colliders.get(b.0).map(|c| c.handle) {
islands.wake_up(bodies, parent, true)
}
}
@@ -263,78 +267,104 @@ impl NarrowPhase {
}
}
pub(crate) fn handle_modified_colliders(
pub(crate) fn handle_modified_colliders<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
modified_colliders: &[ColliderHandle],
colliders: &Colliders,
bodies: &mut Bodies,
events: &dyn EventHandler,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>,
{
let mut pairs_to_remove = vec![];
colliders.foreach_modified_colliders(|handle, collider| {
if collider.changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider.
return;
}
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
if let Some(gid) = self.graph_indices.get(handle.0) {
// For each modified colliders, we need to wake-up the bodies it is in contact with
// so that the narrow-phase properly takes into account the change in, e.g.,
// collision groups. Waking up the modified collider's parent isn't enough because
// it could be a static or kinematic body which don't propagate the wake-up state.
bodies.wake_up(collider.parent, true);
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
let other_handle = if handle == inter.0 { inter.1 } else { inter.0 };
if let Some(other_collider) = colliders.get(other_handle) {
bodies.wake_up(other_collider.parent, true);
}
if let Some(co_changes) = co_changes {
if co_changes.needs_narrow_phase_update() {
// No flag relevant to the narrow-phase is enabled for this collider.
return;
}
// For each collider which had their sensor status modified, we need
// to transfer their contact/intersection graph edges to the intersection/contact graph.
// To achieve this we will remove the relevant contact/intersection pairs form the
// contact/intersection graphs, and then add them into the other graph.
if collider.changes.contains(ColliderChanges::SENSOR) {
if collider.is_sensor() {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove`.
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromContactGraph,
));
if let Some(gid) = self.graph_indices.get(handle.0) {
// For each modified colliders, we need to wake-up the bodies it is in contact with
// so that the narrow-phase properly takes into account the change in, e.g.,
// collision groups. Waking up the modified collider's parent isn't enough because
// it could be a static or kinematic body which don't propagate the wake-up state.
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
let (co_changes, co_type): (&ColliderChanges, &ColliderType) =
colliders.index_bundle(handle.0);
if let Some(co_parent) = co_parent {
islands.wake_up(bodies, co_parent.handle, true);
}
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
let other_handle = if *handle == inter.0 { inter.1 } else { inter.0 };
let other_parent: Option<&ColliderParent> = colliders.get(other_handle.0);
if let Some(other_parent) = other_parent {
islands.wake_up(bodies, other_parent.handle, true);
}
} else {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove` if both involved
// colliders are not sensors.
for inter in self
.intersection_graph
.interactions_with(gid.intersection_graph_index)
.filter(|(h1, h2, _)| {
!colliders[*h1].is_sensor() && !colliders[*h2].is_sensor()
})
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromIntersectionGraph,
));
}
// For each collider which had their sensor status modified, we need
// to transfer their contact/intersection graph edges to the intersection/contact graph.
// To achieve this we will remove the relevant contact/intersection pairs form the
// contact/intersection graphs, and then add them into the other graph.
if co_changes.contains(ColliderChanges::TYPE) {
if co_type.is_sensor() {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove`.
for inter in self
.contact_graph
.interactions_with(gid.contact_graph_index)
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromContactGraph,
));
}
} else {
// Find the contact pairs for this collider and
// push them to `pairs_to_remove` if both involved
// colliders are not sensors.
for inter in self
.intersection_graph
.interactions_with(gid.intersection_graph_index)
.filter(|(h1, h2, _)| {
let co_type1: &ColliderType = colliders.index(h1.0);
let co_type2: &ColliderType = colliders.index(h2.0);
!co_type1.is_sensor() && !co_type2.is_sensor()
})
{
pairs_to_remove.push((
ColliderPair::new(inter.0, inter.1),
PairRemovalMode::FromIntersectionGraph,
));
}
}
}
}
}
});
}
// Remove the pair from the relevant graph.
for pair in &pairs_to_remove {
self.remove_pair(colliders, bodies, &pair.0, events, pair.1);
self.remove_pair(islands, colliders, bodies, &pair.0, events, pair.1);
}
// Add the paid removed pair to the relevant graph.
@@ -343,17 +373,24 @@ impl NarrowPhase {
}
}
fn remove_pair(
fn remove_pair<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
colliders: &Colliders,
bodies: &mut Bodies,
pair: &ColliderPair,
events: &dyn EventHandler,
mode: PairRemovalMode,
) {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
// 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)) = (
@@ -361,7 +398,8 @@ impl NarrowPhase {
self.graph_indices.get(pair.collider2.0),
) {
if mode == PairRemovalMode::FromIntersectionGraph
|| (mode == PairRemovalMode::Auto && (co1.is_sensor() || co2.is_sensor()))
|| (mode == PairRemovalMode::Auto
&& (co_type1.is_sensor() || co_type2.is_sensor()))
{
let was_intersecting = self
.intersection_graph
@@ -382,8 +420,18 @@ impl NarrowPhase {
// 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);
let co_parent1: Option<&ColliderParent> =
colliders.get(pair.collider1.0);
let co_parent2: Option<&ColliderParent> =
colliders.get(pair.collider2.0);
if let Some(co_parent1) = co_parent1 {
islands.wake_up(bodies, co_parent1.handle, true);
}
if let Some(co_parent2) = co_parent2 {
islands.wake_up(bodies, co_parent2.handle, true);
}
events.handle_contact_event(ContactEvent::Stopped(
pair.collider1,
@@ -396,11 +444,18 @@ impl NarrowPhase {
}
}
fn add_pair(&mut self, colliders: &mut ColliderSet, pair: &ColliderPair) {
if let (Some(co1), Some(co2)) =
(colliders.get(pair.collider1), colliders.get(pair.collider2))
{
if co1.parent == co2.parent {
fn add_pair<Colliders>(&mut self, colliders: &Colliders, pair: &ColliderPair)
where
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
let co_type1: Option<&ColliderType> = colliders.get(pair.collider1.0);
let co_type2: Option<&ColliderType> = colliders.get(pair.collider2.0);
if let (Some(co_type1), Some(co_type2)) = (co_type1, co_type2) {
let co_parent1: Option<&ColliderParent> = colliders.get(pair.collider1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(pair.collider2.0);
if co_parent1.map(|p| p.handle) == co_parent2.map(|p| p.handle) {
// Same parents. Ignore collisions.
return;
}
@@ -411,7 +466,7 @@ impl NarrowPhase {
ColliderGraphIndices::invalid(),
);
if co1.is_sensor() || co2.is_sensor() {
if co_type1.is_sensor() || co_type2.is_sensor() {
// NOTE: the collider won't have a graph index as long
// as it does not interact with anything.
if !InteractionGraph::<(), ()>::is_graph_index_valid(gid1.intersection_graph_index)
@@ -469,33 +524,56 @@ impl NarrowPhase {
}
}
pub(crate) fn register_pairs(
pub(crate) fn register_pairs<Bodies, Colliders>(
&mut self,
colliders: &mut ColliderSet,
bodies: &mut RigidBodySet,
islands: &mut IslandManager,
colliders: &Colliders,
bodies: &mut Bodies,
broad_phase_events: &[BroadPhasePairEvent],
events: &dyn EventHandler,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
Colliders: ComponentSet<ColliderType> + ComponentSetOption<ColliderParent>,
{
for event in broad_phase_events {
match event {
BroadPhasePairEvent::AddPair(pair) => {
self.add_pair(colliders, pair);
}
BroadPhasePairEvent::DeletePair(pair) => {
self.remove_pair(colliders, bodies, pair, events, PairRemovalMode::Auto);
self.remove_pair(
islands,
colliders,
bodies,
pair,
events,
PairRemovalMode::Auto,
);
}
}
}
}
pub(crate) fn compute_intersections(
pub(crate) fn compute_intersections<Bodies, Colliders>(
&mut self,
bodies: &RigidBodySet,
colliders: &ColliderSet,
hooks: &dyn PhysicsHooks,
bodies: &Bodies,
colliders: &Colliders,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
events: &dyn EventHandler,
) {
if !colliders.contains_any_modified_collider() {
) where
Bodies: ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderGroups>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>,
{
if modified_colliders.is_empty() {
return;
}
@@ -507,35 +585,66 @@ impl NarrowPhase {
par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
let handle1 = nodes[edge.source().index()].weight;
let handle2 = nodes[edge.target().index()].weight;
let co1 = &colliders[handle1];
let co2 = &colliders[handle2];
if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
let co_parent1: Option<&ColliderParent> = colliders.get(handle1.0);
let (co_changes1, co_groups1, co_shape1, co_pos1): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
) = colliders.index_bundle(handle1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(handle2.0);
let (co_changes2, co_groups2, co_shape2, co_pos2): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
) = colliders.index_bundle(handle2.0);
if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
{
// No update needed for these colliders.
return;
}
// TODO: avoid lookup into bodies.
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
let (mut sleeping1, mut status1) = (true, RigidBodyType::Static);
let (mut sleeping2, mut status2) = (true, RigidBodyType::Static);
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
if let Some(co_parent1) = co_parent1 {
let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent1.handle.0);
status1 = *rb_type1;
sleeping1 = rb_activation1.sleeping;
}
if let Some(co_parent2) = co_parent2 {
let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent2.handle.0);
status2 = *rb_type2;
sleeping2 = rb_activation2.sleeping;
}
if (sleeping1 && status2.is_static())
|| (sleeping2 && status1.is_static())
|| (sleeping1 && sleeping2)
{
// No need to update this intersection because nothing moved.
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
if !co_groups1
.collision_groups
.test(co_groups2.collision_groups)
{
// The intersection is not allowed.
return;
}
if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR)
&& !rb1.is_dynamic()
&& !rb2.is_dynamic()
&& !status1.is_dynamic()
&& !status2.is_dynamic()
{
// Default filtering rule: no intersection between two non-dynamic bodies.
return;
@@ -543,12 +652,12 @@ impl NarrowPhase {
if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: handle1,
collider_handle2: handle2,
collider1: co1,
collider2: co2,
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: handle1,
collider2: handle2,
};
if !hooks.filter_intersection_pair(&context) {
@@ -557,10 +666,10 @@ impl NarrowPhase {
}
}
let pos12 = co1.position().inv_mul(co2.position());
let pos12 = co_pos1.inv_mul(co_pos2);
if let Ok(intersection) =
query_dispatcher.intersection_test(&pos12, co1.shape(), co2.shape())
query_dispatcher.intersection_test(&pos12, &**co_shape1, &**co_shape2)
{
if intersection != edge.weight {
edge.weight = intersection;
@@ -574,15 +683,26 @@ impl NarrowPhase {
});
}
pub(crate) fn compute_contacts(
pub(crate) fn compute_contacts<Bodies, Colliders>(
&mut self,
prediction_distance: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
hooks: &dyn PhysicsHooks,
bodies: &Bodies,
colliders: &Colliders,
modified_colliders: &[ColliderHandle],
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
events: &dyn EventHandler,
) {
if !colliders.contains_any_modified_collider() {
) where
Bodies: ComponentSet<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSet<ColliderChanges>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderGroups>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderMaterial>,
{
if modified_colliders.is_empty() {
return;
}
@@ -592,35 +712,68 @@ impl NarrowPhase {
// TODO: don't iterate on all the edges.
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];
if !co1.changes.needs_narrow_phase_update() && !co2.changes.needs_narrow_phase_update()
let co_parent1: Option<&ColliderParent> = colliders.get(pair.pair.collider1.0);
let (co_changes1, co_groups1, co_shape1, co_pos1, co_material1): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
&ColliderMaterial,
) = colliders.index_bundle(pair.pair.collider1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(pair.pair.collider2.0);
let (co_changes2, co_groups2, co_shape2, co_pos2, co_material2): (
&ColliderChanges,
&ColliderGroups,
&ColliderShape,
&ColliderPosition,
&ColliderMaterial,
) = colliders.index_bundle(pair.pair.collider2.0);
if !co_changes1.needs_narrow_phase_update() && !co_changes2.needs_narrow_phase_update()
{
// No update needed for these colliders.
return;
}
// TODO: avoid lookup into bodies.
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
let (mut sleeping1, mut status1) = (true, RigidBodyType::Static);
let (mut sleeping2, mut status2) = (true, RigidBodyType::Static);
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
if let Some(co_parent1) = co_parent1 {
let (rb_type1, rb_activation1): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent1.handle.0);
status1 = *rb_type1;
sleeping1 = rb_activation1.sleeping;
}
if let Some(co_parent2) = co_parent2 {
let (rb_type2, rb_activation2): (&RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(co_parent2.handle.0);
status2 = *rb_type2;
sleeping2 = rb_activation2.sleeping;
}
if (sleeping1 && status2.is_static())
|| (sleeping2 && status1.is_static())
|| (sleeping1 && sleeping2)
{
// No need to update this contact because nothing moved.
// No need to update this intersection because nothing moved.
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
if !co_groups1
.collision_groups
.test(co_groups2.collision_groups)
{
// The collision is not allowed.
return;
}
if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
&& !rb1.is_dynamic()
&& !rb2.is_dynamic()
&& !status1.is_dynamic()
&& !status2.is_dynamic()
{
// Default filtering rule: no contact between two non-dynamic bodies.
return;
@@ -629,12 +782,12 @@ impl NarrowPhase {
let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
{
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: pair.pair.collider1,
collider_handle2: pair.pair.collider2,
collider1: co1,
collider2: co2,
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.pair.collider1,
collider2: pair.pair.collider2,
};
if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
@@ -644,25 +797,25 @@ impl NarrowPhase {
return;
}
} else {
co1.solver_flags | co2.solver_flags
co_material1.solver_flags | co_material2.solver_flags
};
if !co1.solver_groups.test(co2.solver_groups) {
if !co_groups1.solver_groups.test(co_groups2.solver_groups) {
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
}
if co1.changes.contains(ColliderChanges::SHAPE)
|| co2.changes.contains(ColliderChanges::SHAPE)
if co_changes1.contains(ColliderChanges::SHAPE)
|| co_changes2.contains(ColliderChanges::SHAPE)
{
// The shape changed so the workspace is no longer valid.
pair.workspace = None;
}
let pos12 = co1.position().inv_mul(co2.position());
let pos12 = co_pos1.inv_mul(co_pos2);
let _ = query_dispatcher.contact_manifolds(
&pos12,
co1.shape(),
co2.shape(),
&**co_shape1,
&**co_shape2,
prediction_distance,
&mut pair.manifolds,
&mut pair.workspace,
@@ -671,25 +824,34 @@ impl NarrowPhase {
let mut has_any_active_contact = false;
let friction = CoefficientCombineRule::combine(
co1.friction,
co2.friction,
co1.flags.friction_combine_rule_value(),
co2.flags.friction_combine_rule_value(),
co_material1.friction,
co_material2.friction,
co_material1.friction_combine_rule as u8,
co_material2.friction_combine_rule as u8,
);
let restitution = CoefficientCombineRule::combine(
co1.restitution,
co2.restitution,
co1.flags.restitution_combine_rule_value(),
co2.flags.restitution_combine_rule_value(),
co_material1.restitution,
co_material2.restitution,
co_material1.restitution_combine_rule as u8,
co_material2.restitution_combine_rule as u8,
);
let zero = RigidBodyDominance(0); // The value doesn't matter, it will be MAX because of the effective groups.
let dominance1 = co_parent1
.map(|p1| *bodies.index(p1.handle.0))
.unwrap_or(zero);
let dominance2 = co_parent2
.map(|p2| *bodies.index(p2.handle.0))
.unwrap_or(zero);
for manifold in &mut pair.manifolds {
let world_pos1 = manifold.subshape_pos1.prepend_to(co1.position());
let world_pos1 = manifold.subshape_pos1.prepend_to(co_pos1);
manifold.data.solver_contacts.clear();
manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent());
manifold.data.rigid_body1 = co_parent1.map(|p| p.handle);
manifold.data.rigid_body2 = co_parent2.map(|p| p.handle);
manifold.data.solver_flags = solver_flags;
manifold.data.relative_dominance =
rb1.effective_dominance_group() - rb2.effective_dominance_group();
dominance1.effective_group(&status1) - dominance2.effective_group(&status2);
manifold.data.normal = world_pos1 * manifold.local_n1;
// Generate solver contacts.
@@ -732,12 +894,12 @@ impl NarrowPhase {
let mut modifiable_normal = manifold.data.normal;
let mut context = ContactModificationContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: pair.pair.collider1,
collider_handle2: pair.pair.collider2,
collider1: co1,
collider2: co2,
bodies,
colliders,
rigid_body1: co_parent1.map(|p| p.handle),
rigid_body2: co_parent2.map(|p| p.handle),
collider1: pair.pair.collider1,
collider2: pair.pair.collider2,
manifold,
solver_contacts: &mut modifiable_solver_contacts,
normal: &mut modifiable_normal,
@@ -772,38 +934,61 @@ impl NarrowPhase {
/// 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 select_active_contacts<'a>(
pub(crate) fn select_active_contacts<'a, Bodies>(
&'a mut self,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
out_manifolds: &mut Vec<&'a mut ContactManifold>,
out: &mut Vec<Vec<ContactManifoldIndex>>,
) {
for out_island in &mut out[..bodies.num_islands()] {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyActivation>,
{
for out_island in &mut out[..islands.num_islands()] {
out_island.clear();
}
// TODO: 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.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if manifold
.data
.solver_flags
.contains(SolverFlags::COMPUTE_IMPULSES)
&& manifold.data.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
};
let (active_island_id1, status1, sleeping1) =
if let Some(handle1) = manifold.data.rigid_body1 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(handle1.0);
(data.0.active_island_id, *data.1, data.2.sleeping)
} else {
(0, RigidBodyType::Static, true)
};
out[island_index].push(out_manifolds.len());
out_manifolds.push(manifold);
let (active_island_id2, status2, sleeping2) =
if let Some(handle2) = manifold.data.rigid_body2 {
let data: (&RigidBodyIds, &RigidBodyType, &RigidBodyActivation) =
bodies.index_bundle(handle2.0);
(data.0.active_island_id, *data.1, data.2.sleeping)
} else {
(0, RigidBodyType::Static, true)
};
if (status1.is_dynamic() || status2.is_dynamic())
&& (!status1.is_dynamic() || !sleeping1)
&& (!status2.is_dynamic() || !sleeping2)
{
let island_index = if !status1.is_dynamic() {
active_island_id2
} else {
active_island_id1
};
out[island_index].push(out_manifolds.len());
out_manifolds.push(manifold);
}
}
}
}