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

Allow the PhysicsPipeline and CollisionPipeline to update the QueryPipeline incrementally

Browse Source
This commit is contained in:
Sébastien Crozet
2022-11-26 17:45:14 +01:00
parent 46d976d97b
commit 683baf6bf7
12 changed files with 212 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -123,7 +123,6 @@ impl CCDSolver {
) -> Option<Real> {
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::SweepTestWithPredictedPosition { dt },
@@ -245,7 +244,6 @@ impl CCDSolver {
// Update the query pipeline.
self.query_pipeline.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::SweepTestWithNextPosition,

1
src/dynamics/joint/generic_joint.rs
View File

@@ -5,7 +5,6 @@ use crate::utils::{WBasis, WReal};
#[cfg(feature = "dim3")]
use crate::dynamics::SphericalJoint;
use crate::geometry::ColliderEnabled;
#[cfg(feature = "dim3")]
bitflags::bitflags! {

88
src/geometry/broad_phase_qbvh.rs Normal file
View File

@@ -0,0 +1,88 @@
use crate::geometry::{BroadPhasePairEvent, ColliderHandle, ColliderPair, ColliderSet};
use parry::bounding_volume::BoundingVolume;
use parry::math::Real;
use parry::partitioning::Qbvh;
use parry::partitioning::QbvhUpdateWorkspace;
use parry::query::visitors::BoundingVolumeIntersectionsSimultaneousVisitor;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct BroadPhase {
qbvh: Qbvh<ColliderHandle>,
stack: Vec<(u32, u32)>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
workspace: QbvhUpdateWorkspace,
}
impl Default for BroadPhase {
fn default() -> Self {
Self::new()
}
}
impl BroadPhase {
pub fn new() -> Self {
Self {
qbvh: Qbvh::new(),
stack: vec![],
workspace: QbvhUpdateWorkspace::default(),
}
}
#[allow(dead_code)] // This broad-phase is just experimental right now.
pub fn update(
&mut self,
prediction_distance: Real,
colliders: &ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
events: &mut Vec<BroadPhasePairEvent>,
) {
let margin = 0.01;
if modified_colliders.is_empty() {
return;
}
// Visitor to find collision pairs.
let mut visitor = BoundingVolumeIntersectionsSimultaneousVisitor::new(
|co1: &ColliderHandle, co2: &ColliderHandle| {
events.push(BroadPhasePairEvent::AddPair(ColliderPair::new(*co1, *co2)));
true
},
);
let full_rebuild = self.qbvh.raw_nodes().is_empty();
if full_rebuild {
self.qbvh.clear_and_rebuild(
colliders.iter().map(|(handle, collider)| {
(
handle,
collider.compute_aabb().loosened(prediction_distance / 2.0),
)
}),
margin,
);
self.qbvh
.traverse_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
} else {
for modified in modified_colliders {
self.qbvh.pre_update_or_insert(*modified);
}
for removed in removed_colliders {
self.qbvh.remove(*removed);
}
let _ = self.qbvh.refit(margin, &mut self.workspace, |handle| {
colliders[*handle]
.compute_aabb()
.loosened(prediction_distance / 2.0)
});
self.qbvh
.traverse_modified_bvtt_with_stack(&self.qbvh, &mut visitor, &mut self.stack);
self.qbvh.rebalance(margin, &mut self.workspace);
}
}
}

1
src/geometry/collider_set.rs
View File

@@ -285,6 +285,7 @@ impl ColliderSet {
// Just a very long name instead of `.get_mut` to make sure
// this is really the method we wanted to use instead of `get_mut_internal`.
#[allow(dead_code)]
pub(crate) fn get_mut_internal_with_modification_tracking(
&mut self,
handle: ColliderHandle,

6
src/geometry/mod.rs
View File

@@ -1,6 +1,9 @@
//! Structures related to geometry: colliders, shapes, etc.
pub use self::broad_phase_multi_sap::{BroadPhase, BroadPhasePairEvent, ColliderPair};
pub use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
pub use self::broad_phase_multi_sap::BroadPhase;
// pub use self::broad_phase_qbvh::BroadPhase;
pub use self::collider_components::*;
pub use self::contact_pair::{
ContactData, ContactManifoldData, ContactPair, IntersectionPair, SolverContact, SolverFlags,
@@ -199,5 +202,6 @@ mod interaction_graph;
mod interaction_groups;
mod narrow_phase;
mod broad_phase_qbvh;
mod collider;
mod collider_set;

7
src/pipeline/collision_pipeline.rs
View File

@@ -5,7 +5,7 @@ use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, NarrowPhase,
};
use crate::math::Real;
use crate::pipeline::{EventHandler, PhysicsHooks};
use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
/// The collision pipeline, responsible for performing collision detection between colliders.
@@ -111,6 +111,7 @@ impl CollisionPipeline {
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
query_pipeline: Option<&mut QueryPipeline>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
@@ -154,6 +155,10 @@ impl CollisionPipeline {
true,
);
if let Some(queries) = query_pipeline {
queries.update_incremental(colliders, &modified_colliders, &removed_colliders, true);
}
self.clear_modified_colliders(colliders, &mut modified_colliders);
removed_colliders.clear();
}

24
src/pipeline/physics_pipeline.rs
View File

@@ -14,7 +14,7 @@ use crate::geometry::{
ContactManifoldIndex, NarrowPhase, TemporaryInteractionIndex,
};
use crate::math::{Real, Vector};
use crate::pipeline::{EventHandler, PhysicsHooks};
use crate::pipeline::{EventHandler, PhysicsHooks, QueryPipeline};
use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet};
/// The physics pipeline, responsible for stepping the whole physics simulation.
@@ -404,6 +404,7 @@ impl PhysicsPipeline {
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
ccd_solver: &mut CCDSolver,
mut query_pipeline: Option<&mut QueryPipeline>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
@@ -468,13 +469,17 @@ impl PhysicsPipeline {
colliders,
impulse_joints,
multibody_joints,
&modified_colliders[..],
&mut removed_colliders,
&modified_colliders,
&removed_colliders,
hooks,
events,
true,
);
if let Some(queries) = query_pipeline.as_deref_mut() {
queries.update_incremental(colliders, &modified_colliders, &removed_colliders, false);
}
self.clear_modified_colliders(colliders, &mut modified_colliders);
removed_colliders.clear();
@@ -595,13 +600,22 @@ impl PhysicsPipeline {
colliders,
impulse_joints,
multibody_joints,
&mut modified_colliders,
&mut removed_colliders,
&modified_colliders,
&[],
hooks,
events,
false,
);
if let Some(queries) = query_pipeline.as_deref_mut() {
queries.update_incremental(
colliders,
&modified_colliders,
&[],
remaining_substeps == 0,
);
}
self.clear_modified_colliders(colliders, &mut modified_colliders);
}

116
src/pipeline/query_pipeline.rs
View File

@@ -1,10 +1,10 @@
use crate::dynamics::{IslandManager, RigidBodyHandle};
use crate::dynamics::RigidBodyHandle;
use crate::geometry::{
Aabb, Collider, ColliderHandle, InteractionGroups, PointProjection, Qbvh, Ray, RayIntersection,
};
use crate::math::{Isometry, Point, Real, Vector};
use crate::{dynamics::RigidBodySet, geometry::ColliderSet};
use parry::partitioning::QbvhDataGenerator;
use parry::partitioning::{QbvhDataGenerator, QbvhUpdateWorkspace};
use parry::query::details::{
IntersectionCompositeShapeShapeBestFirstVisitor,
NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -30,8 +30,9 @@ pub struct QueryPipeline {
)]
query_dispatcher: Arc<dyn QueryDispatcher>,
qbvh: Qbvh<ColliderHandle>,
tree_built: bool,
dilation_factor: Real,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
workspace: QbvhUpdateWorkspace,
}
struct QueryPipelineAsCompositeShape<'a> {
@@ -310,8 +311,8 @@ impl QueryPipeline {
Self {
query_dispatcher: Arc::new(d),
qbvh: Qbvh::new(),
tree_built: false,
dilation_factor: 0.01,
workspace: QbvhUpdateWorkspace::default(),
}
}
@@ -320,25 +321,39 @@ impl QueryPipeline {
&*self.query_dispatcher
}
/// Update the acceleration structure on the query pipeline.
pub fn update(
/// Update the query pipeline incrementally, avoiding a complete rebuild of its
/// internal data-structure.
pub fn update_incremental(
&mut self,
islands: &IslandManager,
bodies: &RigidBodySet,
colliders: &ColliderSet,
modified_colliders: &[ColliderHandle],
removed_colliders: &[ColliderHandle],
refit_and_rebalance: bool,
) {
self.update_with_mode(
islands,
bodies,
colliders,
QueryPipelineMode::CurrentPosition,
)
for modified in modified_colliders {
self.qbvh.pre_update_or_insert(*modified);
}
for removed in removed_colliders {
self.qbvh.remove(*removed);
}
if refit_and_rebalance {
let _ = self.qbvh.refit(0.0, &mut self.workspace, |handle| {
colliders[*handle].compute_aabb()
});
self.qbvh.rebalance(0.0, &mut self.workspace);
}
}
/// Update the acceleration structure on the query pipeline.
pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition)
}
/// Update the acceleration structure on the query pipeline.
pub fn update_with_mode(
&mut self,
islands: &IslandManager,
bodies: &RigidBodySet,
colliders: &ColliderSet,
mode: QueryPipelineMode,
@@ -392,71 +407,12 @@ impl QueryPipeline {
}
}
if !self.tree_built {
let generator = DataGenerator {
bodies,
colliders,
mode,
};
self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
// FIXME: uncomment this once we handle insertion/removals properly.
// self.tree_built = true;
return;
}
for handle in islands.iter_active_bodies() {
let rb = &bodies[handle];
for handle in &rb.colliders.0 {
self.qbvh.pre_update(*handle)
}
}
match mode {
QueryPipelineMode::CurrentPosition => {
self.qbvh.update(
|handle| {
let co = &colliders[*handle];
co.shape.compute_aabb(&co.pos)
},
self.dilation_factor,
);
}
QueryPipelineMode::SweepTestWithNextPosition => {
self.qbvh.update(
|handle| {
let co = &colliders[*handle];
if let Some(parent) = &co.parent {
let rb_next_pos = &bodies[parent.handle].pos.next_position;
let next_position = rb_next_pos * parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position)
} else {
co.shape.compute_aabb(&co.pos)
}
},
self.dilation_factor,
);
}
QueryPipelineMode::SweepTestWithPredictedPosition { dt } => {
self.qbvh.update(
|handle| {
let co = &colliders[*handle];
if let Some(parent) = co.parent {
let rb = &bodies[parent.handle];
let predicted_pos = rb.pos.integrate_forces_and_velocities(
dt, &rb.forces, &rb.vels, &rb.mprops,
);
let next_position = predicted_pos * parent.pos_wrt_parent;
co.shape.compute_swept_aabb(&co.pos, &next_position)
} else {
co.shape.compute_aabb(&co.pos)
}
},
self.dilation_factor,
);
}
}
let generator = DataGenerator {
bodies,
colliders,
mode,
};
self.qbvh.clear_and_rebuild(generator, self.dilation_factor);
}
/// Find the closest intersection between a ray and a set of collider.

2
src/pipeline/user_changes.rs
View File

@@ -46,7 +46,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
impulse_joints: &mut ImpulseJointSet,
multibody_joints: &mut MultibodyJointSet,
_multibody_joints: &mut MultibodyJointSet, // FIXME: propagate disabled state to multibodies
modified_bodies: &[RigidBodyHandle],
modified_colliders: &mut Vec<ColliderHandle>,
) {