diff options
Diffstat (limited to 'src/geometry/narrow_phase.rs')
| -rw-r--r-- | src/geometry/narrow_phase.rs | 593 |
1 files changed, 389 insertions, 204 deletions
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index de199ec..bb402da 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -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); + 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 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(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 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, - )); + 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 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 (rb1.is_sleeping() && rb2.is_static()) - || (rb2.is_sleeping() && rb1.is_static()) - || (rb1.is_sleeping() && rb2.is_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 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 (rb1.is_sleeping() && rb2.is_static()) - || (rb2.is_sleeping() && rb1.is_static()) - || (rb1.is_sleeping() && rb2.is_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) + }; + + let (active_island_id2, status2, sleeping2) = + if let Some(handle2) = manifold.data.rigid_body2 { |