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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

67
src/pipeline/collision_pipeline.rs
View File

@@ -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<Bodies, Colliders>(
&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<Bodies, Colliders>,
_events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyColliders>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyDominance>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetMut<ColliderShape>,
{
unimplemented!()
}
}

1
src/pipeline/mod.rs
View File

@@ -13,3 +13,4 @@ mod event_handler;
mod physics_hooks;
mod physics_pipeline;
mod query_pipeline;
mod user_changes;

83
src/pipeline/physics_hooks.rs
View File

@@ -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<RigidBodyHandle>,
/// The handle of the first body involved in the potential collision.
pub rigid_body2: Option<RigidBodyHandle>,
}
/// 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<RigidBodyHandle>,
/// The handle of the first body involved in the potential collision.
pub rigid_body2: Option<RigidBodyHandle>,
/// 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<Bodies, Colliders>: 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<SolverFlags> {
fn filter_contact_pair(
&self,
_context: &PairFilterContext<Bodies, Colliders>,
) -> Option<SolverFlags> {
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<Bodies, Colliders>) -> 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<Bodies, Colliders>) {
}
}
impl PhysicsHooks for () {
impl<Bodies, Colliders> PhysicsHooks<Bodies, Colliders> for () {
fn active_hooks(&self) -> PhysicsHooksFlags {
PhysicsHooksFlags::empty()
}
fn filter_contact_pair(&self, _: &PairFilterContext) -> Option<SolverFlags> {
fn filter_contact_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> Option<SolverFlags> {
None
}
fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool {
fn filter_intersection_pair(&self, _: &PairFilterContext<Bodies, Colliders>) -> bool {
false
}
fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {}
fn modify_solver_contacts(&self, _: &mut ContactModificationContext<Bodies, Colliders>) {}
}

366
src/pipeline/physics_pipeline.rs
View File

@@ -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<ColliderChanges>,
modified_colliders: &mut Vec<ColliderHandle>,
) {
for handle in modified_colliders.drain(..) {
colliders.set_internal(handle.0, ColliderChanges::empty())
}
}
fn detect_collisions<Bodies, Colliders>(
&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<Bodies, Colliders>,
events: &dyn EventHandler,
handle_user_changes: bool,
) {
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSet<RigidBodyDominance>,
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSet<ColliderChanges>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderGroups>
+ ComponentSet<ColliderMaterial>,
{
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<Bodies>(
&mut self,
integration_parameters: &IntegrationParameters,
bodies: &mut RigidBodySet,
) {
islands: &IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
{
#[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<Bodies, Colliders>(
&mut self,
gravity: &Vector<Real>,
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<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyForces>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetOption<ColliderParent>,
{
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<Bodies, Colliders>(
&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<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyMassProps>,
Colliders: ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSet<ColliderType>,
{
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<Bodies, Colliders>(
&mut self,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
islands: &IslandManager,
bodies: &mut Bodies,
colliders: &mut Colliders,
modified_colliders: &mut Vec<ColliderHandle>,
clear_forces: bool,
) {
) where
Bodies: ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyColliders>,
Colliders: ComponentSetMut<ColliderPosition>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetOption<ColliderParent>,
{
// 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<Bodies>(
&mut self,
integration_parameters: &IntegrationParameters,
bodies: &mut RigidBodySet,
) {
islands: &IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyPosition>,
{
// 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<Real>,
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<RigidBodySet, ColliderSet>,
events: &dyn EventHandler,
) {
self.counters.reset();
self.counters.step_started();
colliders.handle_user_changes(bodies);
bodies.handle_user_changes(colliders);
let mut modified_bodies = bodies.take_modified();
let mut modified_colliders = colliders.take_modified();
let mut removed_colliders = colliders.take_removed();
self.detect_collisions(
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<Bodies, Colliders>(
&mut self,
gravity: &Vector<Real>,
integration_parameters: &IntegrationParameters,
islands: &mut IslandManager,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut Bodies,
colliders: &mut Colliders,
modified_bodies: &mut Vec<RigidBodyHandle>,
modified_colliders: &mut Vec<ColliderHandle>,
removed_colliders: &mut Vec<ColliderHandle>,
joints: &mut JointSet,
ccd_solver: &mut CCDSolver,
hooks: &dyn PhysicsHooks<Bodies, Colliders>,
events: &dyn EventHandler,
) where
Bodies: ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyForces>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSetMut<RigidBodyChanges>
+ ComponentSetMut<RigidBodyCcd>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyDominance>
+ ComponentSet<RigidBodyType>,
Colliders: ComponentSetMut<ColliderBroadPhaseData>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetMut<ColliderPosition>
+ ComponentSet<ColliderShape>
+ ComponentSetOption<ColliderParent>
+ ComponentSet<ColliderType>
+ ComponentSet<ColliderGroups>
+ ComponentSet<ColliderMaterial>,
{
self.counters.reset();
self.counters.step_started();
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();

426
src/pipeline/query_pipeline.rs
View File

@@ -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<ColliderGroups> + ComponentSet<ColliderPosition> + ComponentSet<ColliderShape>,
{
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<Real>>, &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<Bodies, Colliders>(
&mut self,
islands: &IslandManager,
bodies: &Bodies,
colliders: &Colliders,
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>,
Colliders: ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
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<Bodies, Colliders>(
&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<RigidBodyPosition>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>,
Colliders: ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
struct DataGenerator<'a, Bs, Cs> {
bodies: &'a Bs,
colliders: &'a Cs,
mode: QueryPipelineMode,
}
impl<'a, Bs, Cs> SimdQuadtreeDataGenerator<ColliderHandle> for DataGenerator<'a, Bs, Cs>
where
Bs: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyForces>,
Cs: ComponentSet<ColliderShape>
+ ComponentSet<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
fn size_hint(&self) -> usize {
ComponentSet::<ColliderShape>::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<Colliders>(
&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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Colliders>(
&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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Colliders>(
&self,
colliders: &ColliderSet,
colliders: &Colliders,
shape_pos: &Isometry<Real>,
shape: &dyn Shape,
query_groups: InteractionGroups,
filter: Option<&dyn Fn(ColliderHandle, &Collider) -> bool>,
) -> Option<ColliderHandle> {
filter: Option<&dyn Fn(ColliderHandle) -> bool>,
) -> Option<ColliderHandle>
where
Colliders: ComponentSet<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Colliders>(
&self,
colliders: &ColliderSet,
colliders: &Colliders,
point: &Point<Real>,
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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Real>,
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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Colliders>(
&self,
colliders: &ColliderSet,
colliders: &Colliders,
point: &Point<Real>,
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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Real>,
shape_vel: &Vector<Real>,
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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Colliders>(
&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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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<Real>,
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<ColliderGroups>
+ ComponentSet<ColliderPosition>
+ ComponentSet<ColliderShape>,
{
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 pos12 = inv_shape_pos * coll.position();
let co_shape: Option<&ColliderShape> = colliders.get(handle.0);
if dispatcher.intersection_test(&pos12, shape, coll.shape()) == Ok(true) {
return callback(*handle, coll);
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 * co_pos.as_ref();
if dispatcher.intersection_test(&pos12, shape, &**co_shape) == Ok(true) {
return callback(*handle);
}
}
}

156
src/pipeline/user_changes.rs Normal file
View File

@@ -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<Colliders>(
bodies: &mut impl ComponentSet<RigidBodyPosition>,
colliders: &mut Colliders,
modified_colliders: &[ColliderHandle],
) where
Colliders: ComponentSetMut<ColliderChanges>
+ ComponentSetMut<ColliderPosition>
+ ComponentSetOption<ColliderParent>,
{
for handle in modified_colliders {
// NOTE: we use `get` because the collider may no longer
// exist if it has been removed.
let co_changes: Option<&ColliderChanges> = colliders.get(handle.0);
if let Some(co_changes) = co_changes {
if co_changes.contains(ColliderChanges::PARENT) {
let co_parent: Option<&ColliderParent> = colliders.get(handle.0);
if let Some(co_parent) = co_parent {
let parent_pos = bodies.index(co_parent.handle.0);
let new_pos = parent_pos.position * co_parent.pos_wrt_parent;
let new_changes = *co_changes | ColliderChanges::POSITION;
colliders.set_internal(handle.0, ColliderPosition(new_pos));
colliders.set_internal(handle.0, new_changes);
}
}
}
}
}
pub(crate) fn handle_user_changes_to_rigid_bodies<Bodies, Colliders>(
islands: &mut IslandManager,
bodies: &mut Bodies,
colliders: &mut Colliders,
modified_bodies: &[RigidBodyHandle],
modified_colliders: &mut Vec<ColliderHandle>,
) where
Bodies: ComponentSetMut<RigidBodyChanges>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>
+ ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyColliders>
+ ComponentSet<RigidBodyPosition>,
Colliders: ComponentSetMut<ColliderPosition>
+ ComponentSetMut<ColliderChanges>
+ ComponentSetOption<ColliderParent>,
{
enum FinalAction {
UpdateActiveKinematicSetId,
UpdateActiveDynamicSetId,
}
for handle in modified_bodies {
let mut final_action = None;
let mut changes: RigidBodyChanges = *bodies.index(handle.0);
let mut ids: RigidBodyIds = *bodies.index(handle.0);
let mut activation: RigidBodyActivation = *bodies.index(handle.0);
let (status, rb_colliders, poss): (
&RigidBodyType,
&RigidBodyColliders,
&RigidBodyPosition,
) = bodies.index_bundle(handle.0);
{
// The body's status changed. We need to make sure
// it is on the correct active set.
if changes.contains(RigidBodyChanges::TYPE) {
match status {
RigidBodyType::Dynamic => {
// Remove from the active kinematic set if it was there.
if islands.active_kinematic_set.get(ids.active_set_id) == Some(handle) {
islands.active_kinematic_set.swap_remove(ids.active_set_id);
final_action =
Some((FinalAction::UpdateActiveKinematicSetId, ids.active_set_id));
}
// Add to the active dynamic set.
activation.wake_up(true);
// Make sure the sleep change flag is set (even if for some
// reasons the rigid-body was already awake) to make
// sure the code handling sleeping change adds the body to
// the active_dynamic_set.
changes.set(RigidBodyChanges::SLEEP, true);
}
RigidBodyType::Kinematic => {
// Remove from the active dynamic set if it was there.
if islands.active_dynamic_set.get(ids.active_set_id) == Some(&handle) {
islands.active_dynamic_set.swap_remove(ids.active_set_id);
final_action =
Some((FinalAction::UpdateActiveDynamicSetId, ids.active_set_id));
}
// Add to the active kinematic set.
if islands.active_kinematic_set.get(ids.active_set_id) != Some(&handle) {
ids.active_set_id = islands.active_kinematic_set.len();
islands.active_kinematic_set.push(*handle);
}
}
RigidBodyType::Static => {}
}
}
// Update the positions of the colliders.
if changes.contains(RigidBodyChanges::POSITION)
|| changes.contains(RigidBodyChanges::COLLIDERS)
{
rb_colliders.update_positions(colliders, modified_colliders, &poss.position);
if status.is_kinematic()
&& islands.active_kinematic_set.get(ids.active_set_id) != Some(handle)
{
ids.active_set_id = islands.active_kinematic_set.len();
islands.active_kinematic_set.push(*handle);
}
}
// Push the body to the active set if it is not
// sleeping and if it is not already inside of the active set.
if changes.contains(RigidBodyChanges::SLEEP)
&& !activation.sleeping // May happen if the body was put to sleep manually.
&& status.is_dynamic() // Only dynamic bodies are in the active dynamic set.
&& islands.active_dynamic_set.get(ids.active_set_id) != Some(handle)
{
ids.active_set_id = islands.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates.
islands.active_dynamic_set.push(*handle);
}
bodies.set_internal(handle.0, RigidBodyChanges::empty());
bodies.set_internal(handle.0, ids);
bodies.set_internal(handle.0, activation);
}
// Adjust some ids, if needed.
if let Some((action, id)) = final_action {
let active_set = match action {
FinalAction::UpdateActiveKinematicSetId => &mut islands.active_kinematic_set,
FinalAction::UpdateActiveDynamicSetId => &mut islands.active_dynamic_set,
};
if id < active_set.len() {
bodies.map_mut_internal(active_set[id].0, |ids2: &mut RigidBodyIds| {
ids2.active_set_id = id;
});
}
}
}
}