From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- src/pipeline/collision_pipeline.rs | 67 +++--- src/pipeline/mod.rs | 1 + src/pipeline/physics_hooks.rs | 83 ++++---- src/pipeline/physics_pipeline.rs | 360 ++++++++++++++++++++++++------- src/pipeline/query_pipeline.rs | 424 ++++++++++++++++++++++++++----------- src/pipeline/user_changes.rs | 156 ++++++++++++++ 6 files changed, 820 insertions(+), 271 deletions(-) create mode 100644 src/pipeline/user_changes.rs (limited to 'src/pipeline') diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index da572f3..074ba75 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,7 +1,11 @@ //! Physics pipeline structures. -use crate::dynamics::{JointSet, RigidBodySet}; -use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; +use crate::data::{ComponentSet, ComponentSetMut}; +use crate::dynamics::{ + IslandManager, JointSet, RigidBodyActivation, RigidBodyColliders, RigidBodyDominance, + RigidBodyIds, RigidBodyType, RigidBodyVelocity, +}; +use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderShape, NarrowPhase}; use crate::math::Real; use crate::pipeline::{EventHandler, PhysicsHooks}; @@ -34,46 +38,25 @@ impl CollisionPipeline { } /// Executes one step of the collision detection. - pub fn step( + pub fn step( &mut self, - prediction_distance: Real, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - hooks: &dyn PhysicsHooks, - events: &dyn EventHandler, - ) { - colliders.handle_user_changes(bodies); - bodies.handle_user_changes(colliders); - self.broadphase_collider_pairs.clear(); - - self.broad_phase_events.clear(); - broad_phase.update(prediction_distance, colliders, &mut self.broad_phase_events); - - narrow_phase.handle_user_changes(colliders, bodies, events); - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events); - narrow_phase.compute_intersections(bodies, colliders, hooks, events); - - bodies.update_active_set_with_contacts( - colliders, - narrow_phase, - self.empty_joints.joint_graph(), - 128, - ); - - // Update colliders positions and kinematic bodies positions. - bodies.foreach_active_body_mut_internal(|_, rb| { - rb.position = rb.next_position; - rb.update_colliders_positions(colliders); - - for handle in &rb.colliders { - let collider = colliders.get_mut_internal(*handle).unwrap(); - collider.position = rb.position * collider.delta; - } - }); - - bodies.modified_inactive_set.clear(); + _prediction_distance: Real, + _broad_phase: &mut BroadPhase, + _narrow_phase: &mut NarrowPhase, + _islands: &mut IslandManager, + _bodies: &mut Bodies, + _colliders: &mut Colliders, + _hooks: &dyn PhysicsHooks, + _events: &dyn EventHandler, + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSetMut + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut, + { + unimplemented!() } } diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index 5fad9bc..ad288d6 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -13,3 +13,4 @@ mod event_handler; mod physics_hooks; mod physics_pipeline; mod query_pipeline; +mod user_changes; diff --git a/src/pipeline/physics_hooks.rs b/src/pipeline/physics_hooks.rs index 72b635f..c4ef245 100644 --- a/src/pipeline/physics_hooks.rs +++ b/src/pipeline/physics_hooks.rs @@ -1,38 +1,38 @@ -use crate::dynamics::RigidBody; -use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags}; +use crate::dynamics::RigidBodyHandle; +use crate::geometry::{ColliderHandle, ContactManifold, SolverContact, SolverFlags}; use crate::math::{Real, Vector}; use na::ComplexField; /// Context given to custom collision filters to filter-out collisions. -pub struct PairFilterContext<'a> { - /// The first collider involved in the potential collision. - pub rigid_body1: &'a RigidBody, - /// The first collider involved in the potential collision. - pub rigid_body2: &'a RigidBody, - /// The first collider involved in the potential collision. - pub collider_handle1: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider_handle2: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider1: &'a Collider, - /// The first collider involved in the potential collision. - pub collider2: &'a Collider, +pub struct PairFilterContext<'a, Bodies, Colliders> { + /// The set of rigid-bodies. + pub bodies: &'a Bodies, + /// The set of colliders. + pub colliders: &'a Colliders, + /// The handle of the first collider involved in the potential collision. + pub collider1: ColliderHandle, + /// The handle of the first collider involved in the potential collision. + pub collider2: ColliderHandle, + /// The handle of the first body involved in the potential collision. + pub rigid_body1: Option, + /// The handle of the first body involved in the potential collision. + pub rigid_body2: Option, } /// Context given to custom contact modifiers to modify the contacts seen by the constraints solver. -pub struct ContactModificationContext<'a> { - /// The first collider involved in the potential collision. - pub rigid_body1: &'a RigidBody, - /// The first collider involved in the potential collision. - pub rigid_body2: &'a RigidBody, - /// The first collider involved in the potential collision. - pub collider_handle1: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider_handle2: ColliderHandle, - /// The first collider involved in the potential collision. - pub collider1: &'a Collider, - /// The first collider involved in the potential collision. - pub collider2: &'a Collider, +pub struct ContactModificationContext<'a, Bodies, Colliders> { + /// The set of rigid-bodies. + pub bodies: &'a Bodies, + /// The set of colliders. + pub colliders: &'a Colliders, + /// The handle of the first collider involved in the potential collision. + pub collider1: ColliderHandle, + /// The handle of the first collider involved in the potential collision. + pub collider2: ColliderHandle, + /// The handle of the first body involved in the potential collision. + pub rigid_body1: Option, + /// The handle of the first body involved in the potential collision. + pub rigid_body2: Option, /// The contact manifold. pub manifold: &'a ContactManifold, /// The solver contacts that can be modified. @@ -45,7 +45,7 @@ pub struct ContactModificationContext<'a> { pub user_data: &'a mut u32, } -impl<'a> ContactModificationContext<'a> { +impl<'a, Bodies, Colliders> ContactModificationContext<'a, Bodies, Colliders> { /// Helper function to update `self` to emulate a oneway-platform. /// /// The "oneway" behavior will only allow contacts between two colliders @@ -127,9 +127,14 @@ bitflags::bitflags! { const MODIFY_SOLVER_CONTACTS = 0b0100; } } +impl Default for PhysicsHooksFlags { + fn default() -> Self { + PhysicsHooksFlags::empty() + } +} /// User-defined functions called by the physics engines during one timestep in order to customize its behavior. -pub trait PhysicsHooks: Send + Sync { +pub trait PhysicsHooks: Send + Sync { /// The sets of hooks that must be taken into account. fn active_hooks(&self) -> PhysicsHooksFlags; @@ -156,7 +161,10 @@ pub trait PhysicsHooks: Send + Sync { /// will be taken into account by the constraints solver. If this returns /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. - fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option { + fn filter_contact_pair( + &self, + _context: &PairFilterContext, + ) -> Option { None } @@ -179,7 +187,7 @@ pub trait PhysicsHooks: Send + Sync { /// not compute any intersection information for it. /// If this return `true` then the narrow-phase will compute intersection /// information for this pair. - fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool { false } @@ -207,21 +215,22 @@ pub trait PhysicsHooks: Send + Sync { /// as 0 and can be modified in `context.user_data`. /// /// The world-space contact normal can be modified in `context.normal`. - fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {} + fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) { + } } -impl PhysicsHooks for () { +impl PhysicsHooks for () { fn active_hooks(&self) -> PhysicsHooksFlags { PhysicsHooksFlags::empty() } - fn filter_contact_pair(&self, _: &PairFilterContext) -> Option { + fn filter_contact_pair(&self, _: &PairFilterContext) -> Option { None } - fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { + fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool { false } - fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} + fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {} } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f4ac531..5a7a827 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -1,17 +1,28 @@ //! Physics pipeline structures. use crate::counters::Counters; +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; -use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; +use crate::dynamics::{ + CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodyActivation, RigidBodyCcd, + RigidBodyChanges, RigidBodyColliders, RigidBodyDamping, RigidBodyDominance, RigidBodyForces, + RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, + RigidBodyVelocity, +}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderBroadPhaseData, ColliderChanges, ColliderGroups, + ColliderHandle, ColliderMaterial, ColliderPair, ColliderParent, ColliderPosition, + ColliderShape, ColliderType, ContactManifoldIndex, NarrowPhase, }; use crate::math::{Real, Vector}; use crate::pipeline::{EventHandler, PhysicsHooks}; +#[cfg(feature = "default-sets")] +use {crate::dynamics::RigidBodySet, crate::geometry::ColliderSet}; + /// The physics pipeline, responsible for stepping the whole physics simulation. /// /// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh @@ -58,17 +69,43 @@ impl PhysicsPipeline { } } - fn detect_collisions( + fn clear_modified_colliders( + &mut self, + colliders: &mut impl ComponentSetMut, + modified_colliders: &mut Vec, + ) { + for handle in modified_colliders.drain(..) { + colliders.set_internal(handle.0, ColliderChanges::empty()) + } + } + + fn detect_collisions( &mut self, integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - hooks: &dyn PhysicsHooks, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], + removed_colliders: &[ColliderHandle], + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, handle_user_changes: bool, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSetMut + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { self.counters.stages.collision_detection_time.resume(); self.counters.cd.broad_phase_time.resume(); @@ -78,6 +115,8 @@ impl PhysicsPipeline { broad_phase.update( integration_parameters.prediction_distance, colliders, + modified_colliders, + removed_colliders, &mut self.broad_phase_events, ); @@ -86,37 +125,46 @@ impl PhysicsPipeline { // Update narrow-phase. if handle_user_changes { - narrow_phase.handle_user_changes(colliders, bodies, events); + narrow_phase.handle_user_changes( + islands, + modified_colliders, + removed_colliders, + colliders, + bodies, + events, + ); } - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); + narrow_phase.register_pairs(islands, colliders, bodies, &self.broad_phase_events, events); narrow_phase.compute_contacts( integration_parameters.prediction_distance, bodies, colliders, + modified_colliders, hooks, events, ); - narrow_phase.compute_intersections(bodies, colliders, hooks, events); - - // Clear colliders modification flags. - colliders.clear_modified_colliders(); + narrow_phase.compute_intersections(bodies, colliders, modified_colliders, hooks, events); self.counters.cd.narrow_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); } - fn solve_position_constraints( + fn solve_position_constraints( &mut self, integration_parameters: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + islands: &IslandManager, + bodies: &mut Bodies, + ) where + Bodies: ComponentSet + ComponentSetMut, + { #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..bodies.num_islands() { + for island_id in 0..islands.num_islands() { self.solvers[island_id].solve_position_constraints( island_id, + islands, &mut self.counters, integration_parameters, bodies, @@ -129,7 +177,7 @@ impl PhysicsPipeline { use rayon::prelude::*; use std::sync::atomic::Ordering; - let num_islands = bodies.num_islands(); + let num_islands = ilands.num_islands(); let solvers = &mut self.solvers[..num_islands]; let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); @@ -140,7 +188,7 @@ impl PhysicsPipeline { .par_iter_mut() .enumerate() .for_each(|(island_id, solver)| { - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; solver.solve_position_constraints( @@ -154,17 +202,30 @@ impl PhysicsPipeline { } } - fn build_islands_and_solve_velocity_constraints( + fn build_islands_and_solve_velocity_constraints( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, narrow_phase: &mut NarrowPhase, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, + bodies: &mut Bodies, + colliders: &mut Colliders, joints: &mut JointSet, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption, + { self.counters.stages.island_construction_time.resume(); - bodies.update_active_set_with_contacts( + islands.update_active_set_with_contacts( + bodies, colliders, narrow_phase, joints.joint_graph(), @@ -172,42 +233,58 @@ impl PhysicsPipeline { ); self.counters.stages.island_construction_time.pause(); - if self.manifold_indices.len() < bodies.num_islands() { + if self.manifold_indices.len() < islands.num_islands() { self.manifold_indices - .resize(bodies.num_islands(), Vec::new()); + .resize(islands.num_islands(), Vec::new()); } - if self.joint_constraint_indices.len() < bodies.num_islands() { + if self.joint_constraint_indices.len() < islands.num_islands() { self.joint_constraint_indices - .resize(bodies.num_islands(), Vec::new()); + .resize(islands.num_islands(), Vec::new()); } let mut manifolds = Vec::new(); - narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices); - joints.select_active_interactions(bodies, &mut self.joint_constraint_indices); + narrow_phase.select_active_contacts( + islands, + bodies, + &mut manifolds, + &mut self.manifold_indices, + ); + joints.select_active_interactions(islands, bodies, &mut self.joint_constraint_indices); self.counters.stages.update_time.resume(); - bodies.foreach_active_dynamic_body_mut_internal(|_, b| { - b.update_world_mass_properties(); - b.add_gravity(*gravity) - }); + for handle in islands.active_dynamic_bodies() { + let poss: &RigidBodyPosition = bodies.index(handle.0); + let position = poss.position; + + let effective_inv_mass = bodies + .map_mut_internal(handle.0, |mprops: &mut RigidBodyMassProps| { + mprops.update_world_mass_properties(&position); + mprops.effective_mass() + }) + .unwrap(); + bodies.map_mut_internal(handle.0, |forces: &mut RigidBodyForces| { + forces.add_linear_acceleration(&gravity, effective_inv_mass) + }); + } self.counters.stages.update_time.pause(); self.counters.stages.solver_time.resume(); - if self.solvers.len() < bodies.num_islands() { + if self.solvers.len() < islands.num_islands() { self.solvers - .resize_with(bodies.num_islands(), IslandSolver::new); + .resize_with(islands.num_islands(), IslandSolver::new); } #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..bodies.num_islands() { + for island_id in 0..islands.num_islands() { self.solvers[island_id].init_constraints_and_solve_velocity_constraints( island_id, &mut self.counters, integration_parameters, + islands, bodies, &mut manifolds[..], &self.manifold_indices[island_id], @@ -238,7 +315,7 @@ impl PhysicsPipeline { .par_iter_mut() .enumerate() .for_each(|(island_id, solver)| { - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let manifolds: &mut Vec<&mut ContactManifold> = unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; @@ -261,19 +338,32 @@ impl PhysicsPipeline { self.counters.stages.solver_time.pause(); } - fn run_ccd_motion_clamping( + fn run_ccd_motion_clamping( &mut self, integration_parameters: &IntegrationParameters, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, + islands: &IslandManager, + bodies: &mut Bodies, + colliders: &mut Colliders, narrow_phase: &NarrowPhase, ccd_solver: &mut CCDSolver, events: &dyn EventHandler, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { self.counters.ccd.toi_computation_time.start(); // Handle CCD let impacts = ccd_solver.predict_impacts_at_next_positions( integration_parameters.dt, + islands, bodies, colliders, narrow_phase, @@ -283,74 +373,176 @@ impl PhysicsPipeline { self.counters.ccd.toi_computation_time.pause(); } - fn advance_to_final_positions( + fn advance_to_final_positions( &mut self, - bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, + islands: &IslandManager, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_colliders: &mut Vec, clear_forces: bool, - ) { + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetOption, + { // Set the rigid-bodies and kinematic bodies to their final position. - bodies.foreach_active_body_mut_internal(|_, rb| { - if rb.is_kinematic() { - rb.linvel = na::zero(); - rb.angvel = na::zero(); + for handle in islands.iter_active_bodies() { + let status: &RigidBodyType = bodies.index(handle.0); + if status.is_kinematic() { + bodies.set_internal(handle.0, RigidBodyVelocity::zero()); } if clear_forces { - rb.force = na::zero(); - rb.torque = na::zero(); + bodies.map_mut_internal(handle.0, |f: &mut RigidBodyForces| { + f.torque = na::zero(); + f.force = na::zero(); + }); } - rb.position = rb.next_position; - rb.update_colliders_positions(colliders); - }); + bodies.map_mut_internal(handle.0, |poss: &mut RigidBodyPosition| { + poss.position = poss.next_position + }); + + let (rb_poss, rb_colls): (&RigidBodyPosition, &RigidBodyColliders) = + bodies.index_bundle(handle.0); + rb_colls.update_positions(colliders, modified_colliders, &rb_poss.position); + } } - fn interpolate_kinematic_velocities( + fn interpolate_kinematic_velocities( &mut self, integration_parameters: &IntegrationParameters, - bodies: &mut RigidBodySet, - ) { + islands: &IslandManager, + bodies: &mut Bodies, + ) where + Bodies: ComponentSetMut + ComponentSet, + { // Update kinematic bodies velocities. // TODO: what is the best place for this? It should at least be // located before the island computation because we test the velocity // there to determine if this kinematic body should wake-up dynamic // bodies it is touching. - bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - body.compute_velocity_from_next_position(integration_parameters.inv_dt()); - }); + for handle in islands.active_kinematic_bodies() { + let ppos: &RigidBodyPosition = bodies.index(handle.0); + let new_vel = ppos.interpolate_velocity(integration_parameters.inv_dt()); + bodies.set_internal(handle.0, new_vel); + } } - /// Executes one timestep of the physics simulation. + #[cfg(feature = "default-sets")] pub fn step( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, ccd_solver: &mut CCDSolver, - hooks: &dyn PhysicsHooks, + hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { + let mut modified_bodies = bodies.take_modified(); + let mut modified_colliders = colliders.take_modified(); + let mut removed_colliders = colliders.take_removed(); + + self.step_generic( + gravity, + integration_parameters, + islands, + broad_phase, + narrow_phase, + bodies, + colliders, + &mut modified_bodies, + &mut modified_colliders, + &mut removed_colliders, + joints, + ccd_solver, + hooks, + events, + ); + } + + /// Executes one timestep of the physics simulation. + pub fn step_generic( + &mut self, + gravity: &Vector, + integration_parameters: &IntegrationParameters, + islands: &mut IslandManager, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut Bodies, + colliders: &mut Colliders, + modified_bodies: &mut Vec, + modified_colliders: &mut Vec, + removed_colliders: &mut Vec, + joints: &mut JointSet, + ccd_solver: &mut CCDSolver, + hooks: &dyn PhysicsHooks, + events: &dyn EventHandler, + ) where + Bodies: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetMut + + ComponentSet + + ComponentSetOption + + ComponentSet + + ComponentSet + + ComponentSet, + { self.counters.reset(); self.counters.step_started(); - colliders.handle_user_changes(bodies); - bodies.handle_user_changes(colliders); + + super::user_changes::handle_user_changes_to_colliders( + bodies, + colliders, + &modified_colliders[..], + ); + super::user_changes::handle_user_changes_to_rigid_bodies( + islands, + bodies, + colliders, + &modified_bodies, + modified_colliders, + ); self.detect_collisions( integration_parameters, + islands, broad_phase, narrow_phase, bodies, colliders, + &modified_colliders[..], + removed_colliders, hooks, events, true, ); + self.clear_modified_colliders(colliders, modified_colliders); + removed_colliders.clear(); + let mut remaining_time = integration_parameters.dt; let mut integration_parameters = *integration_parameters; @@ -375,9 +567,16 @@ impl PhysicsPipeline { if ccd_is_enabled && remaining_substeps > 1 { // NOTE: Take forces into account when updating the bodies CCD activation flags // these forces have not been integrated to the body's velocity yet. - let ccd_active = ccd_solver.update_ccd_active_flags(bodies, remaining_time, true); + let ccd_active = + ccd_solver.update_ccd_active_flags(islands, bodies, remaining_time, true); let first_impact = if ccd_active { - ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase) + ccd_solver.find_first_impact( + remaining_time, + islands, + bodies, + colliders, + narrow_phase, + ) } else { None }; @@ -414,10 +613,11 @@ impl PhysicsPipeline { self.counters.ccd.num_substeps += 1; - self.interpolate_kinematic_velocities(&integration_parameters, bodies); + self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies); self.build_islands_and_solve_velocity_constraints( gravity, &integration_parameters, + islands, narrow_phase, bodies, colliders, @@ -428,11 +628,16 @@ impl PhysicsPipeline { if ccd_is_enabled { // NOTE: don't the forces into account when updating the CCD active flags because // they have already been integrated into the velocities by the solver. - let ccd_active = - ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt, false); + let ccd_active = ccd_solver.update_ccd_active_flags( + islands, + bodies, + integration_parameters.dt, + false, + ); if ccd_active { self.run_ccd_motion_clamping( &integration_parameters, + islands, bodies, colliders, narrow_phase, @@ -449,22 +654,31 @@ impl PhysicsPipeline { // This happens because our CCD use the real rigid-body // velocities instead of just interpolating between // isometries. - self.solve_position_constraints(&integration_parameters, bodies); + self.solve_position_constraints(&integration_parameters, islands, bodies); let clear_forces = remaining_substeps == 0; - self.advance_to_final_positions(bodies, colliders, clear_forces); + self.advance_to_final_positions( + islands, + bodies, + colliders, + modified_colliders, + clear_forces, + ); self.detect_collisions( &integration_parameters, + islands, broad_phase, narrow_phase, bodies, colliders, + modified_colliders, + removed_colliders, hooks, events, false, ); - bodies.modified_inactive_set.clear(); + self.clear_modified_colliders(colliders, modified_colliders); } self.counters.step_completed(); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index cb56141..3139a88 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,9 +1,14 @@ -use crate::dynamics::RigidBodySet; +use crate::data::{BundleSet, ComponentSet, ComponentSetOption}; +use crate::dynamics::{ + IslandManager, RigidBodyColliders, RigidBodyForces, RigidBodyMassProps, RigidBodyPosition, + RigidBodyVelocity, +}; use crate::geometry::{ - Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray, - RayIntersection, SimdQuadTree, AABB, + ColliderGroups, ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, + InteractionGroups, PointProjection, Ray, RayIntersection, SimdQuadTree, AABB, }; use crate::math::{Isometry, Point, Real, Vector}; +use parry::partitioning::SimdQuadtreeDataGenerator; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -32,11 +37,11 @@ pub struct QueryPipeline { dilation_factor: Real, } -struct QueryPipelineAsCompositeShape<'a> { +struct QueryPipelineAsCompositeShape<'a, Colliders> { query_pipeline: &'a QueryPipeline, - colliders: &'a ColliderSet, + colliders: &'a Colliders, query_groups: InteractionGroups, - filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>, + filter: Option<&'a dyn Fn(ColliderHandle) -> bool>, } /// Indicates how the colliders position should be taken into account when @@ -55,7 +60,12 @@ pub enum QueryPipelineMode { }, } -impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { +impl<'a, Colliders> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a, Colliders> +where + // TODO ECS: make everything optional but the shape? + Colliders: + ComponentSet + ComponentSet + ComponentSet, +{ type PartShape = dyn Shape; type PartId = ColliderHandle; @@ -64,11 +74,15 @@ impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { shape_id: Self::PartId, mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), ) { - if let Some(collider) = self.colliders.get(shape_id) { - if collider.collision_groups.test(self.query_groups) - && self.filter.map(|f| f(shape_id, collider)).unwrap_or(true) + let co_groups: Option<&ColliderGroups> = self.colliders.get(shape_id.0); + + if let Some(co_groups) = co_groups { + if co_groups.collision_groups.test(self.query_groups) + && self.filter.map(|f| f(shape_id)).unwrap_or(true) { - f(Some(collider.position()), collider.shape()) + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + self.colliders.index_bundle(shape_id.0); + f(Some(co_pos), &**co_shape) } } } @@ -98,12 +112,12 @@ impl QueryPipeline { Self::with_query_dispatcher(DefaultQueryDispatcher) } - fn as_composite_shape<'a>( + fn as_composite_shape<'a, Colliders>( &'a self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, query_groups: InteractionGroups, - filter: Option<&'a dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> QueryPipelineAsCompositeShape<'a> { + filter: Option<&'a dyn Fn(ColliderHandle) -> bool>, + ) -> QueryPipelineAsCompositeShape<'a, Colliders> { QueryPipelineAsCompositeShape { query_pipeline: self, colliders, @@ -134,52 +148,140 @@ impl QueryPipeline { } /// 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) + pub fn update( + &mut self, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSet + + ComponentSet + + ComponentSetOption, + { + self.update_with_mode( + islands, + bodies, + colliders, + QueryPipelineMode::CurrentPosition, + ) } /// Update the acceleration structure on the query pipeline. - pub fn update_with_mode( + pub fn update_with_mode( &mut self, - bodies: &RigidBodySet, - colliders: &ColliderSet, + islands: &IslandManager, + bodies: &Bodies, + colliders: &Colliders, mode: QueryPipelineMode, - ) { - if !self.tree_built { - match mode { - QueryPipelineMode::CurrentPosition => { - let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); - } - QueryPipelineMode::SweepTestWithNextPosition => { - let data = colliders.iter().map(|(h, c)| { - let next_position = - bodies[c.parent()].next_position * c.position_wrt_parent(); - (h, c.compute_swept_aabb(&next_position)) - }); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); - } - QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { - let data = colliders.iter().map(|(h, c)| { - let next_position = bodies[c.parent()] - .predict_position_using_velocity_and_forces(dt) - * c.position_wrt_parent(); - (h, c.compute_swept_aabb(&next_position)) - }); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); + ) where + Bodies: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Colliders: ComponentSet + + ComponentSet + + ComponentSetOption, + { + struct DataGenerator<'a, Bs, Cs> { + bodies: &'a Bs, + colliders: &'a Cs, + mode: QueryPipelineMode, + } + + impl<'a, Bs, Cs> SimdQuadtreeDataGenerator for DataGenerator<'a, Bs, Cs> + where + Bs: ComponentSet + + ComponentSet + + ComponentSet + + ComponentSet, + Cs: ComponentSet + + ComponentSet + + ComponentSetOption, + { + fn size_hint(&self) -> usize { + ComponentSet::::size_hint(self.colliders) + } + + #[inline(always)] + fn for_each(&mut self, mut f: impl FnMut(ColliderHandle, AABB)) { + match self.mode { + QueryPipelineMode::CurrentPosition => { + self.colliders.for_each(|h, co_shape: &ColliderShape| { + let co_pos: &ColliderPosition = self.colliders.index(h); + f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) + }) + } + QueryPipelineMode::SweepTestWithNextPosition => { + self.colliders.for_each(|h, co_shape: &ColliderShape| { + let co_parent: Option<&ColliderParent> = self.colliders.get(h); + let co_pos: &ColliderPosition = self.colliders.index(h); + + if let Some(co_parent) = co_parent { + let rb_pos: &RigidBodyPosition = + self.bodies.index(co_parent.handle.0); + let next_position = rb_pos.next_position * co_parent.pos_wrt_parent; + + f( + ColliderHandle(h), + co_shape.compute_swept_aabb(&co_pos, &next_position), + ) + } else { + f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) + } + }) + } + QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { + self.colliders.for_each(|h, co_shape: &ColliderShape| { + let co_parent: Option<&ColliderParent> = self.colliders.get(h); + let co_pos: &ColliderPosition = self.colliders.index(h); + + if let Some(co_parent) = co_parent { + let (rb_pos, vels, forces, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + ) = self.bodies.index_bundle(co_parent.handle.0); + let predicted_pos = + rb_pos.integrate_force_and_velocity(dt, forces, vels, mprops); + + let next_position = predicted_pos * co_parent.pos_wrt_parent; + f( + ColliderHandle(h), + co_shape.compute_swept_aabb(&co_pos, &next_position), + ) + } else { + f(ColliderHandle(h), co_shape.compute_aabb(&co_pos)) + } + }) + } } } + } + + if !self.tree_built { + let generator = DataGenerator { + bodies, + colliders, + mode, + }; + self.quadtree + .clear_and_rebuild(generator, self.dilation_factor); // FIXME: uncomment this once we handle insertion/removals properly. // self.tree_built = true; return; } - for (_, body) in bodies - .iter_active_dynamic() - .chain(bodies.iter_active_kinematic()) - { - for handle in &body.colliders { + for handle in islands.iter_active_bodies() { + let body_colliders: &RigidBodyColliders = bodies.index(handle.0); + for handle in &body_colliders.0 { self.quadtree.pre_update(*handle) } } @@ -187,17 +289,28 @@ impl QueryPipeline { match mode { QueryPipelineMode::CurrentPosition => { self.quadtree.update( - |handle| colliders[*handle].compute_aabb(), + |handle| { + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(handle.0); + co_shape.compute_aabb(&co_pos) + }, self.dilation_factor, ); } QueryPipelineMode::SweepTestWithNextPosition => { self.quadtree.update( |handle| { - let co = &colliders[*handle]; - let next_position = - bodies[co.parent()].next_position * co.position_wrt_parent(); - co.compute_swept_aabb(&next_position) + let co_parent: Option<&ColliderParent> = colliders.get(handle.0); + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(handle.0); + + if let Some(co_parent) = co_parent { + let rb_pos: &RigidBodyPosition = bodies.index(co_parent.handle.0); + let next_position = rb_pos.next_position * co_parent.pos_wrt_parent; + co_shape.compute_swept_aabb(&co_pos, &next_position) + } else { + co_shape.compute_aabb(&co_pos) + } }, self.dilation_factor, ); @@ -205,11 +318,26 @@ impl QueryPipeline { QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { self.quadtree.update( |handle| { - let co = &colliders[*handle]; - let next_position = bodies[co.parent()] - .predict_position_using_velocity_and_forces(dt) - * co.position_wrt_parent(); - co.compute_swept_aabb(&next_position) + let co_parent: Option<&ColliderParent> = colliders.get(handle.0); + let (co_pos, co_shape): (&ColliderPosition, &ColliderShape) = + colliders.index_bundle(handle.0); + + if let Some(co_parent) = co_parent { + let (rb_pos, vels, forces, mprops): ( + &RigidBodyPosition, + &RigidBodyVelocity, + &RigidBodyForces, + &RigidBodyMassProps, + ) = bodies.index_bundle(co_parent.handle.0); + + let predicted_pos = + rb_pos.integrate_force_and_velocity(dt, forces, vels, mprops); + + let next_position = predicted_pos * co_parent.pos_wrt_parent; + co_shape.compute_swept_aabb(&co_pos, &next_position) + } else { + co_shape.compute_aabb(&co_pos) + } }, self.dilation_factor, ); @@ -232,15 +360,20 @@ impl QueryPipeline { /// - `filter`: a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_ray( + pub fn cast_ray( &self, - colliders: &ColliderSet, + colliders: &Colliders, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, Real)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, Real)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = RayCompositeShapeToiBestFirstVisitor::new(&pipeline_shape, ray, max_toi, solid); @@ -263,15 +396,20 @@ impl QueryPipeline { /// - `filter`: a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_ray_and_get_normal( + pub fn cast_ray_and_get_normal( &self, - colliders: &ColliderSet, + colliders: &Colliders, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, RayIntersection)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, RayIntersection)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = RayCompositeShapeToiAndNormalBestFirstVisitor::new( &pipeline_shape, @@ -301,26 +439,31 @@ impl QueryPipeline { /// - `callback`: function executed on each collider for which a ray intersection has been found. /// There is no guarantees on the order the results will be yielded. If this callback returns `false`, /// this method will exit early, ignore any further raycast. - pub fn intersections_with_ray<'a>( + pub fn intersections_with_ray<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, ray: &Ray, max_toi: Real, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, - ) { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + mut callback: impl FnMut(ColliderHandle, RayIntersection) -> bool, + ) where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(coll) = colliders.get(*handle) { - if coll.collision_groups.test(query_groups) - && filter.map(|f| f(*handle, coll)).unwrap_or(true) + let co_shape: Option<&ColliderShape> = colliders.get(handle.0); + if let Some(co_shape) = co_shape { + let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) = + colliders.index_bundle(handle.0); + if co_groups.collision_groups.test(query_groups) + && filter.map(|f| f(*handle)).unwrap_or(true) { - if let Some(hit) = - coll.shape() - .cast_ray_and_get_normal(coll.position(), ray, max_toi, solid) + if let Some(hit) = co_shape.cast_ray_and_get_normal(co_pos, ray, max_toi, solid) { - return callback(*handle, coll, hit); + return callback(*handle, hit); } } } @@ -343,14 +486,19 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn intersection_with_shape( + pub fn intersection_with_shape( &self, - colliders: &ColliderSet, + colliders: &Colliders, shape_pos: &Isometry, shape: &dyn Shape, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = IntersectionCompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, @@ -379,14 +527,19 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn project_point( + pub fn project_point( &self, - colliders: &ColliderSet, + colliders: &Colliders, point: &Point, solid: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, PointProjection)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, PointProjection)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = PointCompositeShapeProjBestFirstVisitor::new(&pipeline_shape, point, solid); @@ -408,21 +561,30 @@ impl QueryPipeline { /// is either `None` or returns `true`. /// * `callback` - A function called with each collider with a shape /// containing the `point`. - pub fn intersections_with_point<'a>( + pub fn intersections_with_point<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, point: &Point, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool, - ) { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + mut callback: impl FnMut(ColliderHandle) -> bool, + ) where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(coll) = colliders.get(*handle) { - if coll.collision_groups.test(query_groups) - && filter.map(|f| f(*handle, coll)).unwrap_or(true) - && coll.shape().contains_point(coll.position(), point) + let co_shape: Option<&ColliderShape> = colliders.get(handle.0); + + if let Some(co_shape) = co_shape { + let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) = + colliders.index_bundle(handle.0); + + if co_groups.collision_groups.test(query_groups) + && filter.map(|f| f(*handle)).unwrap_or(true) + && co_shape.contains_point(co_pos, point) { - return callback(*handle, coll); + return callback(*handle); } } @@ -451,13 +613,18 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn project_point_and_get_feature( + pub fn project_point_and_get_feature( &self, - colliders: &ColliderSet, + colliders: &Colliders, point: &Point, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, PointProjection, FeatureId)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, PointProjection, FeatureId)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = PointCompositeShapeProjWithFeatureBestFirstVisitor::new(&pipeline_shape, point, false); @@ -493,16 +660,21 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn cast_shape<'a>( + pub fn cast_shape<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, shape_pos: &Isometry, shape_vel: &Vector, shape: &dyn Shape, max_toi: Real, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, TOI)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, TOI)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, @@ -535,17 +707,22 @@ impl QueryPipeline { /// * `filter` - a more fine-grained filter. A collider is taken into account by this query if /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. - pub fn nonlinear_cast_shape( + pub fn nonlinear_cast_shape( &self, - colliders: &ColliderSet, + colliders: &Colliders, shape_motion: &NonlinearRigidMotion, shape: &dyn Shape, start_time: Real, end_time: Real, stop_at_penetration: bool, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - ) -> Option<(ColliderHandle, TOI)> { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + ) -> Option<(ColliderHandle, TOI)> + where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let pipeline_shape = self.as_composite_shape(colliders, query_groups, filter); let pipeline_motion = NonlinearRigidMotion::identity(); let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( @@ -574,27 +751,36 @@ impl QueryPipeline { /// its `contact_group` is compatible with the `query_groups`, and if this `filter` /// is either `None` or returns `true`. /// * `callback` - A function called with the handles of each collider intersecting the `shape`. - pub fn intersections_with_shape<'a>( + pub fn intersections_with_shape<'a, Colliders>( &self, - colliders: &'a ColliderSet, + colliders: &'a Colliders, shape_pos: &Isometry, shape: &dyn Shape, query_groups: InteractionGroups, - filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>, - mut callback: impl FnMut(ColliderHandle, &'a Collider) -> bool, - ) { + filter: Option<&dyn Fn(ColliderHandle) -> bool>, + mut callback: impl FnMut(ColliderHandle) -> bool, + ) where + Colliders: ComponentSet + + ComponentSet + + ComponentSet, + { let dispatcher = &*self.query_dispatcher; let inv_shape_pos = shape_pos.inverse(); let mut leaf_callback = &mut |handle: &ColliderHandle| { - if let Some(coll) = colliders.get(*handle) { - if coll.collision_groups.test(query_groups) - && filter.map(|f| f(*handle, coll)).unwrap_or(true) + let co_shape: Option<&ColliderShape> = colliders.get(handle.0); + + if let Some(co_shape) = co_shape { + let (co_groups, co_pos): (&ColliderGroups, &ColliderPosition) = + colliders.index_bundle(handle.0); + + if co_groups.collision_groups.test(query_groups) + && filter.map(|f| f(*handle)).unwrap_or(true) { - let pos12 = inv_shape_pos * coll.position(); + let pos12 = inv_shape_pos * co_pos.as_ref(); - if dispatcher.intersection_test(&pos12, shape, coll.shape()) == Ok(true) { - return callback(*handle, coll); + if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) { + return callback(*handle); } } } diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs new file mode 100644 index 0000000..354ff3d --- /dev/null +++ b/src/pipeline/user_changes.rs @@ -0,0 +1,156 @@ +use crate::data::{BundleSet, ComponentSet, ComponentSetMut, ComponentSetOption}; +use crate::dynamics::{ + IslandManager, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, RigidBodyHandle, + RigidBodyIds, RigidBodyPosition, RigidBodyType, +}; +use crate::geometry::{ColliderChanges, ColliderHandle, ColliderParent, ColliderPosition}; + +pub(crate) fn handle_user_changes_to_colliders( + bodies: &mut impl ComponentSet, + colliders: &mut Colliders, + modified_colliders: &[ColliderHandle], +) where + Colliders: ComponentSetMut + + ComponentSetMut + + ComponentSetOption, +{ + for handle in modified_colliders { + // NOT