From bf8e48e920e7c10241b4eda9e011e4d1258d95a6 Mon Sep 17 00:00:00 2001 From: Thierry Berger Date: Sun, 2 Feb 2025 14:53:30 +0100 Subject: [PATCH] Incorrect narrow_phase collisions after using ColliderSet::set_parent (#742) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * reproduction for case 1 (no collision) * test for wrong self intersection after Collider::set_parent * dynamics: remove new parent from contact and intersection graph ; maybe should be removed from graph_indices too? * parent testing at the same place a interaction group check, to avoid missing parent change * add more asserts in test + more correct comments * add changelog * Update CHANGELOG.md * chore: remove debug print statements * chore: improve narrow-phase test to check for re-re-parenting * fix: remove unneeded narrow-phase pair removal --------- Co-authored-by: Sébastien Crozet --- CHANGELOG.md | 3 + src/geometry/narrow_phase.rs | 325 ++++++++++++++++++++++++++++++++++- 2 files changed, 319 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index efd1905..f00bb2f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,9 @@ - Fix `KinematicCharacterController::move_shape` not respecting parameters `max_slope_climb_angle` and `min_slope_slide_angle`. - Improve ground detection reliability for `KinematicCharacterController`. (#715) - Fix wasm32 default values for physics hooks filter to be consistent with native: `COMPUTE_IMPULSES`. +- Fix changing a collider parent when ongoing collisions should be affected (#742): + - Fix collisions not being removed when a collider is parented to a rigidbody while in collision with it. + - Fix collisions not being added when the parent was removed while intersecting a (previously) sibling collider. ### Added diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 908f83e..3597847 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -463,7 +463,7 @@ impl NarrowPhase { // 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.changes.intersects(ColliderChanges::TYPE) { if co.is_sensor() { // Find the contact pairs for this collider and // push them to `pairs_to_remove`. @@ -494,6 +494,12 @@ impl NarrowPhase { } } } + + // NOTE: if a collider only changed parent, we don’t need to remove it from any + // of the graphs as re-parenting doesn’t change the sensor status of a + // collider. If needed, their collision/intersection data will be + // updated/removed automatically in the contact or intersection update + // functions. } } } @@ -510,7 +516,7 @@ impl NarrowPhase { ); } - // Add the paid removed pair to the relevant graph. + // Add the removed pair to the relevant graph. for pair in pairs_to_remove { self.add_pair(colliders, &pair.0); } @@ -593,12 +599,6 @@ impl NarrowPhase { if let (Some(co1), Some(co2)) = (colliders.get(pair.collider1), colliders.get(pair.collider2)) { - if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) && co1.parent.is_some() - { - // Same parents. Ignore collisions. - return; - } - // These colliders have no parents - continue. let (gid1, gid2) = self.graph_indices.ensure_pair_exists( @@ -723,7 +723,13 @@ impl NarrowPhase { // No update needed for these colliders. return; } - + if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) + && co1.parent.is_some() + { + // Same parents. Ignore collisions. + edge.weight.intersecting = false; + break 'emit_events; + } // TODO: avoid lookup into bodies. let mut rb_type1 = RigidBodyType::Fixed; let mut rb_type2 = RigidBodyType::Fixed; @@ -824,6 +830,12 @@ impl NarrowPhase { // No update needed for these colliders. return; } + if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) && co1.parent.is_some() + { + // Same parents. Ignore collisions. + pair.clear(); + break 'emit_events; + } let rb1 = co1.parent.map(|co_parent1| &bodies[co_parent1.handle]); let rb2 = co2.parent.map(|co_parent2| &bodies[co_parent2.handle]); @@ -1168,3 +1180,298 @@ impl NarrowPhase { } } } + +#[cfg(test)] +#[cfg(feature = "f32")] +#[cfg(feature = "dim3")] +mod test { + use na::vector; + + use crate::prelude::{ + CCDSolver, ColliderBuilder, DefaultBroadPhase, IntegrationParameters, PhysicsPipeline, + QueryPipeline, RigidBodyBuilder, + }; + + use super::*; + + /// Test for https://github.com/dimforge/rapier/issues/734. + #[test] + pub fn collider_set_parent_depenetration() { + // This tests the scenario: + // 1. Body A has two colliders attached (and overlapping), Body B has none. + // 2. One of the colliders from Body A gets re-parented to Body B. + // -> Collision is properly detected between the colliders of A and B. + let mut rigid_body_set = RigidBodySet::new(); + let mut collider_set = ColliderSet::new(); + + /* Create the ground. */ + let collider = ColliderBuilder::ball(0.5); + + /* Create body 1, which will contain both colliders at first. */ + let rigid_body_1 = RigidBodyBuilder::dynamic() + .translation(vector![0.0, 0.0, 0.0]) + .build(); + let body_1_handle = rigid_body_set.insert(rigid_body_1); + + /* Create collider 1. Parent it to rigid body 1. */ + let collider_1_handle = + collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set); + + /* Create collider 2. Parent it to rigid body 1. */ + let collider_2_handle = + collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set); + + /* Create body 2. No attached colliders yet. */ + let rigid_body_2 = RigidBodyBuilder::dynamic() + .translation(vector![0.0, 0.0, 0.0]) + .build(); + let body_2_handle = rigid_body_set.insert(rigid_body_2); + + /* Create other structures necessary for the simulation. */ + let gravity = vector![0.0, 0.0, 0.0]; + let integration_parameters = IntegrationParameters::default(); + let mut physics_pipeline = PhysicsPipeline::new(); + let mut island_manager = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut impulse_joint_set = ImpulseJointSet::new(); + let mut multibody_joint_set = MultibodyJointSet::new(); + let mut ccd_solver = CCDSolver::new(); + let mut query_pipeline = QueryPipeline::new(); + let physics_hooks = (); + let event_handler = (); + + physics_pipeline.step( + &gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + Some(&mut query_pipeline), + &physics_hooks, + &event_handler, + ); + let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos; + let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos; + assert!( + (collider_1_position.translation.vector - collider_2_position.translation.vector) + .magnitude() + < 0.5f32 + ); + + let contact_pair = narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .expect("The contact pair should exist."); + assert_eq!(contact_pair.manifolds.len(), 0); + assert!(matches!( + narrow_phase.intersection_pair(collider_1_handle, collider_2_handle), + // Interaction pair is for sensors + None, + )); + /* Parent collider 2 to body 2. */ + collider_set.set_parent(collider_2_handle, Some(body_2_handle), &mut rigid_body_set); + + physics_pipeline.step( + &gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + Some(&mut query_pipeline), + &physics_hooks, + &event_handler, + ); + + let contact_pair = narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .expect("The contact pair should exist."); + assert_eq!(contact_pair.manifolds.len(), 1); + assert!(matches!( + narrow_phase.intersection_pair(collider_1_handle, collider_2_handle), + // Interaction pair is for sensors + None, + )); + + /* Run the game loop, stepping the simulation once per frame. */ + for _ in 0..200 { + physics_pipeline.step( + &gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + Some(&mut query_pipeline), + &physics_hooks, + &event_handler, + ); + + let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos; + let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos; + println!("collider 1 position: {}", collider_1_position.translation); + println!("collider 2 position: {}", collider_2_position.translation); + } + + let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos; + let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos; + println!("collider 2 position: {}", collider_2_position.translation); + assert!( + (collider_1_position.translation.vector - collider_2_position.translation.vector) + .magnitude() + >= 0.5f32, + "colliders should no longer be penetrating." + ); + } + + /// Test for https://github.com/dimforge/rapier/issues/734. + #[test] + pub fn collider_set_parent_no_self_intersection() { + // This tests the scenario: + // 1. Body A and Body B each have one collider attached. + // -> There should be a collision detected between A and B. + // 2. The collider from Body B gets attached to Body A. + // -> There should no longer be any collision between A and B. + // 3. Re-parent one of the collider from Body A to Body B again. + // -> There should a collision again. + let mut rigid_body_set = RigidBodySet::new(); + let mut collider_set = ColliderSet::new(); + + /* Create the ground. */ + let collider = ColliderBuilder::ball(0.5); + + /* Create body 1, which will contain collider 1. */ + let rigid_body_1 = RigidBodyBuilder::dynamic() + .translation(vector![0.0, 0.0, 0.0]) + .build(); + let body_1_handle = rigid_body_set.insert(rigid_body_1); + + /* Create collider 1. Parent it to rigid body 1. */ + let collider_1_handle = + collider_set.insert_with_parent(collider.build(), body_1_handle, &mut rigid_body_set); + + /* Create body 2, which will contain collider 2 at first. */ + let rigid_body_2 = RigidBodyBuilder::dynamic() + .translation(vector![0.0, 0.0, 0.0]) + .build(); + let body_2_handle = rigid_body_set.insert(rigid_body_2); + + /* Create collider 2. Parent it to rigid body 2. */ + let collider_2_handle = + collider_set.insert_with_parent(collider.build(), body_2_handle, &mut rigid_body_set); + + /* Create other structures necessary for the simulation. */ + let gravity = vector![0.0, 0.0, 0.0]; + let integration_parameters = IntegrationParameters::default(); + let mut physics_pipeline = PhysicsPipeline::new(); + let mut island_manager = IslandManager::new(); + let mut broad_phase = DefaultBroadPhase::new(); + let mut narrow_phase = NarrowPhase::new(); + let mut impulse_joint_set = ImpulseJointSet::new(); + let mut multibody_joint_set = MultibodyJointSet::new(); + let mut ccd_solver = CCDSolver::new(); + let mut query_pipeline = QueryPipeline::new(); + let physics_hooks = (); + let event_handler = (); + + physics_pipeline.step( + &gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + Some(&mut query_pipeline), + &physics_hooks, + &event_handler, + ); + + let contact_pair = narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .expect("The contact pair should exist."); + assert_eq!( + contact_pair.manifolds.len(), + 1, + "There should be a contact manifold." + ); + + let collider_1_position = collider_set.get(collider_1_handle).unwrap().pos; + let collider_2_position = collider_set.get(collider_2_handle).unwrap().pos; + assert!( + (collider_1_position.translation.vector - collider_2_position.translation.vector) + .magnitude() + < 0.5f32 + ); + + /* Parent collider 2 to body 1. */ + collider_set.set_parent(collider_2_handle, Some(body_1_handle), &mut rigid_body_set); + physics_pipeline.step( + &gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + Some(&mut query_pipeline), + &physics_hooks, + &event_handler, + ); + + let contact_pair = narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .expect("The contact pair should no longer exist."); + assert_eq!( + contact_pair.manifolds.len(), + 0, + "Colliders with same parent should not be in contact together." + ); + + /* Parent collider 2 back to body 1. */ + collider_set.set_parent(collider_2_handle, Some(body_2_handle), &mut rigid_body_set); + physics_pipeline.step( + &gravity, + &integration_parameters, + &mut island_manager, + &mut broad_phase, + &mut narrow_phase, + &mut rigid_body_set, + &mut collider_set, + &mut impulse_joint_set, + &mut multibody_joint_set, + &mut ccd_solver, + Some(&mut query_pipeline), + &physics_hooks, + &event_handler, + ); + + let contact_pair = narrow_phase + .contact_pair(collider_1_handle, collider_2_handle) + .expect("The contact pair should exist."); + assert_eq!( + contact_pair.manifolds.len(), + 1, + "There should be a contact manifold." + ); + } +}