diff options
Diffstat (limited to 'src/pipeline')
| -rw-r--r-- | src/pipeline/collision_pipeline.rs | 26 | ||||
| -rw-r--r-- | src/pipeline/mod.rs | 2 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 347 | ||||
| -rw-r--r-- | src/pipeline/query_pipeline.rs | 125 |
4 files changed, 407 insertions, 93 deletions
diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index a74a6e5..da572f3 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -44,16 +44,15 @@ impl CollisionPipeline { hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { - bodies.maintain(colliders); + colliders.handle_user_changes(bodies); + bodies.handle_user_changes(colliders); self.broadphase_collider_pairs.clear(); - broad_phase.update_aabbs(prediction_distance, bodies, colliders); - self.broad_phase_events.clear(); - broad_phase.find_pairs(&mut self.broad_phase_events); + 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); @@ -61,26 +60,17 @@ impl CollisionPipeline { colliders, narrow_phase, self.empty_joints.joint_graph(), - 0, + 128, ); - // // Update kinematic bodies velocities. - // bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - // body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); - // }); - // Update colliders positions and kinematic bodies positions. bodies.foreach_active_body_mut_internal(|_, rb| { - if rb.is_kinematic() { - rb.position = rb.predicted_position; - } else { - rb.update_predicted_position(0.0); - } + rb.position = rb.next_position; + rb.update_colliders_positions(colliders); for handle in &rb.colliders { - let collider = &mut colliders[*handle]; + let collider = colliders.get_mut_internal(*handle).unwrap(); collider.position = rb.position * collider.delta; - collider.predicted_position = rb.predicted_position * collider.delta; } }); diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs index fd85cfa..5fad9bc 100644 --- a/src/pipeline/mod.rs +++ b/src/pipeline/mod.rs @@ -6,7 +6,7 @@ pub use physics_hooks::{ ContactModificationContext, PairFilterContext, PhysicsHooks, PhysicsHooksFlags, }; pub use physics_pipeline::PhysicsPipeline; -pub use query_pipeline::QueryPipeline; +pub use query_pipeline::{QueryPipeline, QueryPipelineMode}; mod collision_pipeline; mod event_handler; diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 198c4be..0f7b5f6 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -3,7 +3,7 @@ use crate::counters::Counters; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; -use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; +use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ @@ -58,57 +58,37 @@ impl PhysicsPipeline { } } - /// Executes one timestep of the physics simulation. - pub fn step( + fn detect_collisions( &mut self, - gravity: &Vector<Real>, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, + handle_user_changes: bool, ) { - self.counters.step_started(); - bodies.maintain(colliders); - broad_phase.maintain(colliders); - narrow_phase.maintain(colliders, bodies); - - // 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_predicted_position(integration_parameters.inv_dt()); - }); + self.counters.stages.collision_detection_time.resume(); + self.counters.cd.broad_phase_time.resume(); - self.counters.stages.collision_detection_time.start(); - self.counters.cd.broad_phase_time.start(); + // Update broad-phase. + self.broad_phase_events.clear(); self.broadphase_collider_pairs.clear(); - // let t = instant::now(); - broad_phase.update_aabbs( + broad_phase.update( integration_parameters.prediction_distance, - bodies, colliders, + &mut self.broad_phase_events, ); - // println!("Update AABBs time: {}", instant::now() - t); - // let t = instant::now(); - self.broad_phase_events.clear(); - broad_phase.find_pairs(&mut self.broad_phase_events); - // println!("Find pairs time: {}", instant::now() - t); - - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); self.counters.cd.broad_phase_time.pause(); + self.counters.cd.narrow_phase_time.resume(); - // println!("Num contact pairs: {}", pairs.len()); - - self.counters.cd.narrow_phase_time.start(); - - // let t = instant::now(); + // Update narrow-phase. + if handle_user_changes { + narrow_phase.handle_user_changes(colliders, bodies, events); + } + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); narrow_phase.compute_contacts( integration_parameters.prediction_distance, bodies, @@ -117,9 +97,73 @@ impl PhysicsPipeline { events, ); narrow_phase.compute_intersections(bodies, colliders, hooks, events); - // println!("Compute contact time: {}", instant::now() - t); - self.counters.stages.island_construction_time.start(); + // Clear colliders modification flags. + colliders.clear_modified_colliders(); + + self.counters.cd.narrow_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); + } + + fn solve_position_constraints( + &mut self, + integration_parameters: &IntegrationParameters, + bodies: &mut RigidBodySet, + ) { + #[cfg(not(feature = "parallel"))] + { + enable_flush_to_zero!(); + + for island_id in 0..bodies.num_islands() { + self.solvers[island_id].solve_position_constraints( + island_id, + &mut self.counters, + integration_parameters, + bodies, + ) + } + } + + #[cfg(feature = "parallel")] + { + use rayon::prelude::*; + use std::sync::atomic::Ordering; + + let num_islands = bodies.num_islands(); + let solvers = &mut self.solvers[..num_islands]; + let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); + + rayon::scope(|scope| { + enable_flush_to_zero!(); + + solvers + .par_iter_mut() + .enumerate() + .for_each(|(island_id, solver)| { + let bodies: &mut RigidBodySet = + unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; + + solver.solve_position_constraints( + scope, + island_id, + integration_parameters, + bodies, + ) + }); + }); + } + } + + fn build_islands_and_solve_velocity_constraints( + &mut self, + gravity: &Vector<Real>, + integration_parameters: &IntegrationParameters, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ) { + self.counters.stages.island_construction_time.resume(); bodies.update_active_set_with_contacts( colliders, narrow_phase, @@ -139,25 +183,17 @@ impl PhysicsPipeline { } let mut manifolds = Vec::new(); - narrow_phase.sort_and_select_active_contacts( - bodies, - &mut manifolds, - &mut self.manifold_indices, - ); + narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices); joints.select_active_interactions(bodies, &mut self.joint_constraint_indices); - self.counters.cd.narrow_phase_time.pause(); - self.counters.stages.collision_detection_time.pause(); - - self.counters.stages.update_time.start(); + self.counters.stages.update_time.resume(); bodies.foreach_active_dynamic_body_mut_internal(|_, b| { b.update_world_mass_properties(); b.add_gravity(*gravity) }); self.counters.stages.update_time.pause(); - self.counters.solver.reset(); - self.counters.stages.solver_time.start(); + self.counters.stages.solver_time.resume(); if self.solvers.len() < bodies.num_islands() { self.solvers .resize_with(bodies.num_islands(), IslandSolver::new); @@ -168,7 +204,7 @@ impl PhysicsPipeline { enable_flush_to_zero!(); for island_id in 0..bodies.num_islands() { - self.solvers[island_id].solve_island( + self.solvers[island_id].init_constraints_and_solve_velocity_constraints( island_id, &mut self.counters, integration_parameters, @@ -209,7 +245,7 @@ impl PhysicsPipeline { let joints: &mut Vec<JointGraphEdge> = unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) }; - solver.solve_island( + solver.init_constraints_and_solve_velocity_constraints( scope, island_id, integration_parameters, @@ -222,31 +258,224 @@ impl PhysicsPipeline { }); }); } + self.counters.stages.solver_time.pause(); + } + + fn run_ccd_motion_clamping( + &mut self, + integration_parameters: &IntegrationParameters, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + narrow_phase: &NarrowPhase, + ccd_solver: &mut CCDSolver, + events: &dyn EventHandler, + ) { + self.counters.ccd.toi_computation_time.start(); + // Handle CCD + let impacts = ccd_solver.predict_impacts_at_next_positions( + integration_parameters.dt, + bodies, + colliders, + narrow_phase, + events, + ); + ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts); + self.counters.ccd.toi_computation_time.pause(); + } - // Update colliders positions and kinematic bodies positions. - // FIXME: do this in the solver? + fn advance_to_final_positions( + &mut self, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + clear_forces: bool, + ) { + // Set the rigid-bodies and kinematic bodies to their final position. bodies.foreach_active_body_mut_internal(|_, rb| { if rb.is_kinematic() { - rb.position = rb.predicted_position; rb.linvel = na::zero(); rb.angvel = na::zero(); - } else { - rb.update_predicted_position(integration_parameters.dt); } + if clear_forces { + rb.force = na::zero(); + rb.torque = na::zero(); + } + + rb.position = rb.next_position; rb.update_colliders_positions(colliders); }); + } - self.counters.stages.solver_time.pause(); + fn interpolate_kinematic_velocities( + &mut self, + integration_parameters: &IntegrationParameters, + bodies: &mut RigidBodySet, + ) { + // 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()); + }); + } + + /// Executes one timestep of the physics simulation. + pub fn step( + &mut self, + gravity: &Vector<Real>, + integration_parameters: &IntegrationParameters, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ccd_solver: &mut CCDSolver, + hooks: &dyn PhysicsHooks, + events: &dyn EventHandler, + ) { + self.counters.reset(); + self.counters.step_started(); + colliders.handle_user_changes(bodies); + bodies.handle_user_changes(colliders); + + self.detect_collisions( + integration_parameters, + broad_phase, + narrow_phase, + bodies, + colliders, + hooks, + events, + true, + ); + + let mut remaining_time = integration_parameters.dt; + let mut integration_parameters = *integration_parameters; + + let (ccd_is_enabled, mut remaining_substeps) = + if integration_parameters.max_ccd_substeps == 0 { + (false, 1) + } else { + (true, integration_parameters.max_ccd_substeps) + }; + + while remaining_substeps > 0 { + // If there are more than one CCD substep, we need to split + // the timestep into multiple intervals. First, estimate the + // size of the time slice we will integrate for this substep. + // + // Note that we must do this now, before the constrains resolution + // because we need to use the correct timestep length for the + // integration of external forces. + // + // If there is only one or zero CCD substep, there is no need + // to split the timetsep interval. So we can just skip this part. + 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 first_impact = if ccd_active { + ccd_solver.find_first_impact(remaining_time, bodies, colliders, narrow_phase) + } else { + None + }; + + if let Some(toi) = first_impact { + let original_interval = remaining_time / (remaining_substeps as Real); + + if toi < original_interval { + integration_parameters.dt = original_interval; + } else { + integration_parameters.dt = + toi + (remaining_time - toi) / (remaining_substeps as Real); + } + + remaining_substeps -= 1; + } else { + // No impact, don't do any other substep after this one. + integration_parameters.dt = remaining_time; + remaining_substeps = 0; + } + + remaining_time -= integration_parameters.dt; + + // Avoid substep length that are too small. + if remaining_time <= integration_parameters.min_ccd_dt { + integration_parameters.dt += remaining_time; + remaining_substeps = 0; + } + } else { + integration_parameters.dt = remaining_time; + remaining_time = 0.0; + remaining_substeps = 0; + } + + self.counters.ccd.num_substeps += 1; + + self.interpolate_kinematic_velocities(&integration_parameters, bodies); + self.build_islands_and_solve_velocity_constraints( + gravity, + &integration_parameters, + narrow_phase, + bodies, + colliders, + joints, + ); + + // If CCD is enabled, execute the CCD motion clamping. + 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); + if ccd_active { + self.run_ccd_motion_clamping( + &integration_parameters, + bodies, + colliders, + narrow_phase, + ccd_solver, + events, + ); + } + } + + // NOTE: we need to run the position solver **after** the + // CCD motion clamping because otherwise the clamping + // would undo the depenetration done by the position + // solver. + // This happens because our CCD use the real rigid-body + // velocities instead of just interpolating between + // isometries. + self.solve_position_constraints(&integration_parameters, bodies); + + let clear_forces = remaining_substeps == 0; + self.advance_to_final_positions(bodies, colliders, clear_forces); + self.detect_collisions( + &integration_parameters, + broad_phase, + narrow_phase, + bodies, + colliders, + hooks, + events, + false, + ); + + bodies.modified_inactive_set.clear(); + } - bodies.modified_inactive_set.clear(); self.counters.step_completed(); } } #[cfg(test)] mod test { - use crate::dynamics::{IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet}; + use crate::dynamics::{ + CCDSolver, IntegrationParameters, JointSet, RigidBodyBuilder, RigidBodySet, + }; use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase}; use crate::math::Vector; use crate::pipeline::PhysicsPipeline; @@ -278,6 +507,7 @@ mod test { &mut bodies, &mut colliders, &mut joints, + &mut CCDSolver::new(), &(), &(), ); @@ -321,6 +551,7 @@ mod test { &mut bodies, &mut colliders, &mut joints, + &mut CCDSolver::new(), &(), &(), ); diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 8cc6a60..5451cfa 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,10 +1,9 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray, - RayIntersection, SimdQuadTree, + RayIntersection, SimdQuadTree, AABB, }; use crate::math::{Isometry, Point, Real, Vector}; -use crate::parry::motion::RigidMotion; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -15,7 +14,7 @@ use parry::query::details::{ use parry::query::visitors::{ BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor, }; -use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI}; +use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI}; use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape}; use std::sync::Arc; @@ -39,6 +38,22 @@ struct QueryPipelineAsCompositeShape<'a> { groups: InteractionGroups, } +/// Indicates how the colliders position should be taken into account when +/// updating the query pipeline. +pub enum QueryPipelineMode { + /// The `Collider::position` is taken into account. + CurrentPosition, + /// The `RigidBody::next_position * Collider::position_wrt_parent` is taken into account for + /// the colliders positions. + SweepTestWithNextPosition, + /// The `RigidBody::predict_position_using_velocity_and_forces * Collider::position_wrt_parent` + /// is taken into account for the colliders position. + SweepTestWithPredictedPosition { + /// The time used to integrate the rigid-body's velocity and acceleration. + dt: Real, + }, +} + impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { type PartShape = dyn Shape; type PartId = ColliderHandle; @@ -95,7 +110,7 @@ impl QueryPipeline { /// Initializes an empty query pipeline with a custom `QueryDispatcher`. /// /// Use this constructor in order to use a custom `QueryDispatcher` that is - /// awary of your own user-defined shapes. + /// aware of your own user-defined shapes. pub fn with_query_dispatcher<D>(d: D) -> Self where D: 'static + QueryDispatcher, @@ -108,11 +123,48 @@ impl QueryPipeline { } } + /// The query dispatcher used by this query pipeline for running scene queries. + pub fn query_dispatcher(&self) -> &dyn QueryDispatcher { + &*self.query_dispatcher + } + /// Update the acceleration structure on the query pipeline. pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { + self.update_with_mode(bodies, colliders, QueryPipelineMode::CurrentPosition) + } + + /// Update the acceleration structure on the query pipeline. + pub fn update_with_mode( + &mut self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + mode: QueryPipelineMode, + ) { if !self.tree_built { - let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); + 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); + } + } + // FIXME: uncomment this once we handle insertion/removals properly. // self.tree_built = true; return; @@ -127,10 +179,37 @@ impl QueryPipeline { } } - self.quadtree.update( - |handle| colliders[*handle].compute_aabb(), - self.dilation_factor, - ); + match mode { + QueryPipelineMode::CurrentPosition => { + self.quadtree.update( + |handle| colliders[*handle].compute_aabb(), + 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) + }, + self.dilation_factor, + ); + } + 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) + }, + self.dilation_factor, + ); + } + } } /// Find the closest intersection between a ray and a set of collider. @@ -336,6 +415,16 @@ impl QueryPipeline { .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1)) } + /// Finds all handles of all the colliders with an AABB intersecting the given AABB. + pub fn colliders_with_aabb_intersecting_aabb( + &self, + aabb: &AABB, + mut callback: impl FnMut(&ColliderHandle) -> bool, + ) { + let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback); + self.quadtree.traverse_depth_first(&mut visitor); + } + /// Casts a shape at a constant linear velocity and retrieve the first collider it hits. /// /// This is similar to ray-casting except that we are casting a whole shape instead of @@ -386,20 +475,24 @@ impl QueryPipeline { pub fn nonlinear_cast_shape( &self, colliders: &ColliderSet, - shape_motion: &dyn RigidMotion, + shape_motion: &NonlinearRigidMotion, shape: &dyn Shape, - max_toi: Real, - target_distance: Real, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, groups: InteractionGroups, ) -> Option<(ColliderHandle, TOI)> { let pipeline_shape = self.as_composite_shape(colliders, groups); + let pipeline_motion = NonlinearRigidMotion::identity(); let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, - shape_motion, + &pipeline_motion, &pipeline_shape, + shape_motion, shape, - max_toi, - target_distance, + start_time, + end_time, + stop_at_penetration, ); self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1) } |