diff options
Diffstat (limited to 'src/pipeline')
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 7 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 24 | ||||
| -rw-r--r-- | src/pipeline/query_pipeline.rs | 116 | ||||
| -rw-r--r-- | src/pipeline/user_changes.rs | 2 |
4 files changed, 62 insertions, 87 deletions
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 631388b..d0beedd 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -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(); } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f05a3f0..7226063 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -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); } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 6682370..8a667de 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -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. diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index adfd023..00096de 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -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>, ) { |