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

CCD: take collision groups into account

Browse Source
This commit is contained in:
Crozet Sébastien
2021-06-01 14:56:24 +02:00
parent 5ef81cda40
commit dbb3c8f43b
6 changed files with 107 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

75
src/dynamics/ccd/ccd_solver.rs
View File

@@ -10,7 +10,7 @@ use crate::geometry::{
use crate::math::Real; use crate::math::Real;
use crate::parry::utils::SortedPair; use crate::parry::utils::SortedPair;
use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode}; use crate::pipeline::{EventHandler, QueryPipeline, QueryPipelineMode};
use crate::prelude::{ActiveEvents, ColliderFlags}; use crate::prelude::{ActiveEvents, ColliderFlags, ColliderGroups};
use parry::query::{DefaultQueryDispatcher, QueryDispatcher}; use parry::query::{DefaultQueryDispatcher, QueryDispatcher};
use parry::utils::hashmap::HashMap; use parry::utils::hashmap::HashMap;
use std::collections::BinaryHeap; use std::collections::BinaryHeap;
@@ -140,7 +140,8 @@ impl CCDSolver {
Colliders: ComponentSetOption<ColliderParent> Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition> + ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape> + ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>, + ComponentSet<ColliderType>
+ ComponentSet<ColliderGroups>,
{ {
// Update the query pipeline. // Update the query pipeline.
self.query_pipeline.update_with_mode( self.query_pipeline.update_with_mode(
@@ -201,16 +202,21 @@ impl CCDSolver {
{ {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0); let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0); let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0);
let co_type1: &ColliderType = colliders.index(ch1.0); let co_type1: &ColliderType = colliders.index(ch1.0);
let co_type2: &ColliderType = colliders.index(ch1.0); let co_type2: &ColliderType = colliders.index(ch1.0);
let bh1 = co_parent1.map(|p| p.handle); let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle); let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 || (co_type1.is_sensor() || co_type2.is_sensor()) { // Ignore self-intersection and sensors and apply collision groups filter.
// Ignore self-intersection and sensors. if bh1 == bh2 // Ignore self-intersection.
|| (co_type1.is_sensor() || co_type2.is_sensor()) // Ignore sensors.
|| !c1.3.collision_groups.test(c2.3.collision_groups) // Apply collision groups.
|| !c1.3.solver_groups.test(c2.3.solver_groups)
// Apply solver groups.
{
return true; return true;
} }
@@ -226,8 +232,8 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(), self.query_pipeline.query_dispatcher(),
*ch1, *ch1,
*ch2, *ch2,
(c1.0, c1.1, c1.2, co_parent1), (c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, co_parent2), (c2.0, c2.1, c2.2, c2.3, co_parent2),
Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)), Some((rb_pos1, rb_vels1, rb_mprops1, rb_ccd1)),
b2, b2,
None, None,
@@ -274,7 +280,8 @@ impl CCDSolver {
+ ComponentSet<ColliderPosition> + ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape> + ComponentSet<ColliderShape>
+ ComponentSet<ColliderType> + ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>, + ComponentSet<ColliderFlags>
+ ComponentSet<ColliderGroups>,
{ {
let mut frozen = HashMap::<_, Real>::default(); let mut frozen = HashMap::<_, Real>::default();
let mut all_toi = BinaryHeap::new(); let mut all_toi = BinaryHeap::new();
@@ -336,14 +343,15 @@ impl CCDSolver {
{ {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0); let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0); let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0);
let bh1 = co_parent1.map(|p| p.handle); let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle); let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 { // Ignore self-intersections and apply groups filter.
// Ignore self-intersection. if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups)
{
return true; return true;
} }
@@ -360,8 +368,8 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(), self.query_pipeline.query_dispatcher(),
*ch1, *ch1,
*ch2, *ch2,
(c1.0, c1.1, c1.2, co_parent1), (c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, co_parent2), (c2.0, c2.1, c2.2, c2.3, co_parent2),
b1, b1,
b2, b2,
None, None,
@@ -400,7 +408,7 @@ impl CCDSolver {
// NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this // NOTE: all static bodies (and kinematic bodies?) should be considered as "frozen", this
// may avoid some resweeps. // may avoid some resweeps.
let mut intersections_to_check = vec![]; let mut pseudo_intersections_to_check = vec![];
while let Some(toi) = all_toi.pop() { while let Some(toi) = all_toi.pop() {
assert!(toi.toi <= dt); assert!(toi.toi <= dt);
@@ -422,7 +430,7 @@ impl CCDSolver {
continue; continue;
} }
if toi.is_intersection_test { if toi.is_pseudo_intersection_test {
// NOTE: this test is redundant with the previous `if !should_freeze && ...` // NOTE: this test is redundant with the previous `if !should_freeze && ...`
// but let's keep it to avoid tricky regressions if we end up swapping both // but let's keep it to avoid tricky regressions if we end up swapping both
// `if` for some reasons in the future. // `if` for some reasons in the future.
@@ -430,7 +438,7 @@ impl CCDSolver {
// This is only an intersection so we don't have to freeze and there is no // This is only an intersection so we don't have to freeze and there is no
// need to resweep. However we will need to see if we have to generate // need to resweep. However we will need to see if we have to generate
// intersection events, so push the TOI for further testing. // intersection events, so push the TOI for further testing.
intersections_to_check.push(toi); pseudo_intersections_to_check.push(toi);
} }
continue; continue;
} }
@@ -462,14 +470,14 @@ impl CCDSolver {
.colliders_with_aabb_intersecting_aabb(&aabb, |ch2| { .colliders_with_aabb_intersecting_aabb(&aabb, |ch2| {
let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0); let co_parent1: Option<&ColliderParent> = colliders.get(ch1.0);
let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0); let co_parent2: Option<&ColliderParent> = colliders.get(ch2.0);
let c1: (_, _, _) = colliders.index_bundle(ch1.0); let c1: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch1.0);
let c2: (_, _, _) = colliders.index_bundle(ch2.0); let c2: (_, _, _, &ColliderGroups) = colliders.index_bundle(ch2.0);
let bh1 = co_parent1.map(|p| p.handle); let bh1 = co_parent1.map(|p| p.handle);
let bh2 = co_parent2.map(|p| p.handle); let bh2 = co_parent2.map(|p| p.handle);
if bh1 == bh2 { // Ignore self-intersection and apply groups filter.
// Ignore self-intersection. if bh1 == bh2 || !c1.3.collision_groups.test(c2.3.collision_groups) {
return true; return true;
} }
@@ -498,8 +506,8 @@ impl CCDSolver {
self.query_pipeline.query_dispatcher(), self.query_pipeline.query_dispatcher(),
*ch1, *ch1,
*ch2, *ch2,
(c1.0, c1.1, c1.2, co_parent1), (c1.0, c1.1, c1.2, c1.3, co_parent1),
(c2.0, c2.1, c2.2, co_parent2), (c2.0, c2.1, c2.2, c2.3, co_parent2),
b1, b1,
b2, b2,
frozen1.copied(), frozen1.copied(),
@@ -516,7 +524,7 @@ impl CCDSolver {
} }
} }
for toi in intersections_to_check { for toi in pseudo_intersections_to_check {
// See if the intersection is still active once the bodies // See if the intersection is still active once the bodies
// reach their final positions. // reach their final positions.
// - If the intersection is still active, don't report it yet. It will be // - If the intersection is still active, don't report it yet. It will be
@@ -524,17 +532,30 @@ impl CCDSolver {
// - If the intersection isn't active anymore, and it wasn't intersecting // - If the intersection isn't active anymore, and it wasn't intersecting
// before, then we need to generate one interaction-start and one interaction-stop // before, then we need to generate one interaction-start and one interaction-stop
// events because it will never be detected by the narrow-phase because of tunneling. // events because it will never be detected by the narrow-phase because of tunneling.
let (co_pos1, co_shape1, co_flags1): ( let (co_type1, co_pos1, co_shape1, co_flags1): (
&ColliderType,
&ColliderPosition, &ColliderPosition,
&ColliderShape, &ColliderShape,
&ColliderFlags, &ColliderFlags,
) = colliders.index_bundle(toi.c1.0); ) = colliders.index_bundle(toi.c1.0);
let (co_pos2, co_shape2, co_flags2): ( let (co_type2, co_pos2, co_shape2, co_flags2): (
&ColliderType,
&ColliderPosition, &ColliderPosition,
&ColliderShape, &ColliderShape,
&ColliderFlags, &ColliderFlags,
) = colliders.index_bundle(toi.c2.0); ) = colliders.index_bundle(toi.c2.0);
if !co_type1.is_sensor() && !co_type2.is_sensor() {
// TODO: this happens if we found a TOI between two non-sensor
// colliders with mismatching solver_flags. It is not clear
// what we should do in this case: we could report a
// contact started/contact stopped event for example. But in
// that case, what contact pair should be pass to these events?
// For now we just ignore this special case. Let's wait for an actual
// use-case to come up before we determine what we want to do here.
continue;
}
let co_next_pos1 = if let Some(b1) = toi.b1 { let co_next_pos1 = if let Some(b1) = toi.b1 {
let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap(); let co_parent1: &ColliderParent = colliders.get(toi.c1.0).unwrap();
let (rb_pos1, rb_vels1, rb_mprops1): ( let (rb_pos1, rb_vels1, rb_mprops1): (

24
src/dynamics/ccd/toi_entry.rs
View File

@@ -2,7 +2,7 @@ use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity, RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
}; };
use crate::geometry::{ use crate::geometry::{
ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType, ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
}; };
use crate::math::Real; use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher}; use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -14,7 +14,9 @@ pub struct TOIEntry {
pub b1: Option<RigidBodyHandle>, pub b1: Option<RigidBodyHandle>,
pub c2: ColliderHandle, pub c2: ColliderHandle,
pub b2: Option<RigidBodyHandle>, pub b2: Option<RigidBodyHandle>,
pub is_intersection_test: bool, // We call this "pseudo" intersection because this also
// includes colliders pairs with mismatching solver_groups.
pub is_pseudo_intersection_test: bool,
pub timestamp: usize, pub timestamp: usize,
} }
@@ -25,7 +27,7 @@ impl TOIEntry {
b1: Option<RigidBodyHandle>, b1: Option<RigidBodyHandle>,
c2: ColliderHandle, c2: ColliderHandle,
b2: Option<RigidBodyHandle>, b2: Option<RigidBodyHandle>,
is_intersection_test: bool, is_pseudo_intersection_test: bool,
timestamp: usize, timestamp: usize,
) -> Self { ) -> Self {
Self { Self {
@@ -34,7 +36,7 @@ impl TOIEntry {
b1, b1,
c2, c2,
b2, b2,
is_intersection_test, is_pseudo_intersection_test,
timestamp, timestamp,
} }
} }
@@ -47,12 +49,14 @@ impl TOIEntry {
&ColliderType, &ColliderType,
&ColliderShape, &ColliderShape,
&ColliderPosition, &ColliderPosition,
&ColliderGroups,
Option<&ColliderParent>, Option<&ColliderParent>,
), ),
c2: ( c2: (
&ColliderType, &ColliderType,
&ColliderShape, &ColliderShape,
&ColliderPosition, &ColliderPosition,
&ColliderGroups,
Option<&ColliderParent>, Option<&ColliderParent>,
), ),
b1: Option<( b1: Option<(
@@ -78,8 +82,8 @@ impl TOIEntry {
return None; return None;
} }
let (co_type1, co_shape1, co_pos1, co_parent1) = c1; let (co_type1, co_shape1, co_pos1, co_groups1, co_parent1) = c1;
let (co_type2, co_shape2, co_pos2, co_parent2) = c2; let (co_type2, co_shape2, co_pos2, co_groups2, co_parent2) = c2;
let linvel1 = let linvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero()); frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
@@ -104,7 +108,9 @@ impl TOIEntry {
// keep it since more conservatism is good at this stage. // keep it since more conservatism is good at this stage.
let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness()) let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
+ smallest_contact_dist.max(0.0); + smallest_contact_dist.max(0.0);
let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor(); let is_pseudo_intersection_test = co_type1.is_sensor()
|| co_type2.is_sensor()
|| !co_groups1.solver_groups.test(co_groups2.solver_groups);
if (end_time - start_time) * vel12 < thickness { if (end_time - start_time) * vel12 < thickness {
return None; return None;
@@ -135,7 +141,7 @@ impl TOIEntry {
// If the TOI search involves two non-sensor colliders then // If the TOI search involves two non-sensor colliders then
// we don't want to stop the TOI search at the first penetration // we don't want to stop the TOI search at the first penetration
// because the colliders may be in a separating trajectory. // because the colliders may be in a separating trajectory.
let stop_at_penetration = is_intersection_test; let stop_at_penetration = is_pseudo_intersection_test;
let res_toi = query_dispatcher let res_toi = query_dispatcher
.nonlinear_time_of_impact( .nonlinear_time_of_impact(
@@ -157,7 +163,7 @@ impl TOIEntry {
co_parent1.map(|p| p.handle), co_parent1.map(|p| p.handle),
ch2, ch2,
co_parent2.map(|p| p.handle), co_parent2.map(|p| p.handle),
is_intersection_test, is_pseudo_intersection_test,
0, 0,
)) ))
} }

1
src/geometry/collider.rs
View File

@@ -33,7 +33,6 @@ pub struct Collider {
impl Collider { impl Collider {
pub(crate) fn reset_internal_references(&mut self) { pub(crate) fn reset_internal_references(&mut self) {
self.co_parent = None;
self.co_bf_data.proxy_index = crate::INVALID_U32; self.co_bf_data.proxy_index = crate::INVALID_U32;
self.co_changes = ColliderChanges::all(); self.co_changes = ColliderChanges::all();
} }

16
src/geometry/collider_set.rs
View File

@@ -134,6 +134,7 @@ impl ColliderSet {
// Make sure the internal links are reset, they may not be // Make sure the internal links are reset, they may not be
// if this rigid-body was obtained by cloning another one. // if this rigid-body was obtained by cloning another one.
coll.reset_internal_references(); coll.reset_internal_references();
coll.co_parent = None;
let handle = ColliderHandle(self.colliders.insert(coll)); let handle = ColliderHandle(self.colliders.insert(coll));
self.modified_colliders.push(handle); self.modified_colliders.push(handle);
handle handle
@@ -147,12 +148,17 @@ impl ColliderSet {
bodies: &mut RigidBodySet, bodies: &mut RigidBodySet,
) -> ColliderHandle { ) -> ColliderHandle {
// Make sure the internal links are reset, they may not be // Make sure the internal links are reset, they may not be
// if this rigid-body was obtained by cloning another one. // if this collider was obtained by cloning another one.
coll.reset_internal_references(); coll.reset_internal_references();
coll.co_parent = Some(ColliderParent {
handle: parent_handle, if let Some(prev_parent) = &mut coll.co_parent {
pos_wrt_parent: coll.co_pos.0, prev_parent.handle = parent_handle;
}); } else {
coll.co_parent = Some(ColliderParent {
handle: parent_handle,
pos_wrt_parent: coll.co_pos.0,
});
}
// NOTE: we use `get_mut` instead of `get_mut_internal` so that the // NOTE: we use `get_mut` instead of `get_mut_internal` so that the
// modification flag is updated properly. // modification flag is updated properly.

36
src/pipeline/physics_pipeline.rs
View File

@@ -367,7 +367,8 @@ impl PhysicsPipeline {
+ ComponentSet<ColliderPosition> + ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape> + ComponentSet<ColliderShape>
+ ComponentSet<ColliderType> + ComponentSet<ColliderType>
+ ComponentSet<ColliderFlags>, + ComponentSet<ColliderFlags>
+ ComponentSet<ColliderGroups>,
{ {
self.counters.ccd.toi_computation_time.start(); self.counters.ccd.toi_computation_time.start();
// Handle CCD // Handle CCD
@@ -430,7 +431,10 @@ impl PhysicsPipeline {
islands: &IslandManager, islands: &IslandManager,
bodies: &mut Bodies, bodies: &mut Bodies,
) where ) where
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyPosition>, Bodies: ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyMassProps>,
{ {
// Update kinematic bodies velocities. // Update kinematic bodies velocities.
// TODO: what is the best place for this? It should at least be // TODO: what is the best place for this? It should at least be
@@ -438,9 +442,31 @@ impl PhysicsPipeline {
// there to determine if this kinematic body should wake-up dynamic // there to determine if this kinematic body should wake-up dynamic
// bodies it is touching. // bodies it is touching.
for handle in islands.active_kinematic_bodies() { for handle in islands.active_kinematic_bodies() {
let ppos: &RigidBodyPosition = bodies.index(handle.0); let (rb_type, rb_pos, rb_vel, rb_mprops): (
let new_vel = ppos.interpolate_velocity(integration_parameters.inv_dt()); &RigidBodyType,
bodies.set_internal(handle.0, new_vel); &RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
match rb_type {
RigidBodyType::KinematicPositionBased => {
let rb_pos: &RigidBodyPosition = bodies.index(handle.0);
let new_vel = rb_pos.interpolate_velocity(integration_parameters.inv_dt());
bodies.set_internal(handle.0, new_vel);
}
RigidBodyType::KinematicVelocityBased => {
let new_pos = rb_vel.integrate(
integration_parameters.dt,
&rb_pos.position,
// NOTE: we don't use the `world_com` here because it is not
// really updated for kinematic bodies.
&(rb_pos.position * rb_mprops.local_mprops.local_com),
);
bodies.set_internal(handle.0, RigidBodyPosition::from(new_pos));
}
_ => {}
}
} }
} }

3
src/pipeline/user_changes.rs
View File

@@ -100,7 +100,8 @@ pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
// the active_dynamic_set. // the active_dynamic_set.
changes.set(RigidBodyChanges::SLEEP, true); changes.set(RigidBodyChanges::SLEEP, true);
} }
RigidBodyType::Kinematic => { RigidBodyType::KinematicVelocityBased
| RigidBodyType::KinematicPositionBased => {
// Remove from the active dynamic set if it was there. // Remove from the active dynamic set if it was there.
if islands.active_dynamic_set.get(ids.active_set_id) == Some(&handle) { if islands.active_dynamic_set.get(ids.active_set_id) == Some(&handle) {
islands.active_dynamic_set.swap_remove(ids.active_set_id); islands.active_dynamic_set.swap_remove(ids.active_set_id);