From 3a1502be74901f3df96a05a7d479f15bd4f8b507 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sat, 13 Mar 2021 18:00:58 +0100 Subject: First complete implementation of the hierarchical SAP. --- src/pipeline/collision_pipeline.rs | 9 ++++++--- src/pipeline/physics_pipeline.rs | 13 ++++--------- 2 files changed, 10 insertions(+), 12 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index a74a6e5..6255d60 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -47,10 +47,13 @@ impl CollisionPipeline { bodies.maintain(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, + bodies, + colliders, + &mut self.broad_phase_events, + ); narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 198c4be..1908fad 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -73,7 +73,6 @@ impl PhysicsPipeline { ) { self.counters.step_started(); bodies.maintain(colliders); - broad_phase.maintain(colliders); narrow_phase.maintain(colliders, bodies); // Update kinematic bodies velocities. @@ -87,19 +86,15 @@ impl PhysicsPipeline { self.counters.stages.collision_detection_time.start(); self.counters.cd.broad_phase_time.start(); + 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(); -- cgit From 97157c9423f3360c5e941b4065377689221014ae Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 26 Mar 2021 18:16:27 +0100 Subject: First working version of non-linear CCD based on single-substep motion-clamping. --- src/pipeline/collision_pipeline.rs | 11 +++--- src/pipeline/physics_pipeline.rs | 25 +++++++++---- src/pipeline/query_pipeline.rs | 74 +++++++++++++++++++++++++++++--------- 3 files changed, 79 insertions(+), 31 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 6255d60..5df3f6a 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -69,21 +69,18 @@ impl CollisionPipeline { // // Update kinematic bodies velocities. // bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - // body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + // body.compute_velocity_from_next_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]; + collider.prev_position = collider.position; collider.position = rb.position * collider.delta; - collider.predicted_position = rb.predicted_position * collider.delta; } }); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 1908fad..7abe634 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::{ @@ -68,6 +68,7 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, + ccd_solver: Option<&mut CCDSolver>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -81,7 +82,7 @@ impl PhysicsPipeline { // 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()); + body.compute_velocity_from_next_position(integration_parameters.inv_dt()); }); self.counters.stages.collision_detection_time.start(); @@ -218,23 +219,33 @@ impl PhysicsPipeline { }); } - // Update colliders positions and kinematic bodies positions. - // FIXME: do this in the solver? + // Handle CCD + if let Some(ccd_solver) = ccd_solver { + let impacts = ccd_solver.predict_next_impacts( + integration_parameters, + bodies, + colliders, + integration_parameters.dt, + events, + ); + ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts); + } + + // 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); } + rb.position = rb.next_position; rb.update_colliders_positions(colliders); }); self.counters.stages.solver_time.pause(); bodies.modified_inactive_set.clear(); + self.counters.step_completed(); } } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 8cc6a60..e89a03e 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; @@ -95,7 +94,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) -> Self where D: 'static + QueryDispatcher, @@ -108,11 +107,26 @@ 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) { + pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) { if !self.tree_built { - let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); + if !use_swept_aabb { + let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + } else { + let data = colliders.iter().map(|(h, co)| { + let next_position = + bodies[co.parent()].next_position * co.position_wrt_parent(); + (h, co.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 +141,22 @@ impl QueryPipeline { } } - self.quadtree.update( - |handle| colliders[*handle].compute_aabb(), - self.dilation_factor, - ); + if !use_swept_aabb { + self.quadtree.update( + |handle| colliders[*handle].compute_aabb(), + self.dilation_factor, + ); + } else { + 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, + ); + } } /// Find the closest intersection between a ray and a set of collider. @@ -336,6 +362,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 +422,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) } -- cgit From dec3e4197f3f8b47baedb28ddec976a846e7d099 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Sun, 28 Mar 2021 11:54:33 +0200 Subject: Small refactoring of the PhysicsPipeline. --- src/pipeline/physics_pipeline.rs | 123 +++++++++++++++++++++++++++++---------- 1 file changed, 91 insertions(+), 32 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 7abe634..fa01def 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -58,33 +58,16 @@ impl PhysicsPipeline { } } - /// Executes one timestep of the physics simulation. - pub fn step( + fn detect_collisions( &mut self, - gravity: &Vector, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhase, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - joints: &mut JointSet, - ccd_solver: Option<&mut CCDSolver>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { - self.counters.step_started(); - bodies.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_next_position(integration_parameters.inv_dt()); - }); - self.counters.stages.collision_detection_time.start(); self.counters.cd.broad_phase_time.start(); self.broad_phase_events.clear(); @@ -96,15 +79,14 @@ impl PhysicsPipeline { colliders, &mut self.broad_phase_events, ); - - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); self.counters.cd.broad_phase_time.pause(); // println!("Num contact pairs: {}", pairs.len()); self.counters.cd.narrow_phase_time.start(); + narrow_phase.maintain(colliders, bodies); + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - // let t = instant::now(); narrow_phase.compute_contacts( integration_parameters.prediction_distance, bodies, @@ -113,8 +95,19 @@ impl PhysicsPipeline { events, ); narrow_phase.compute_intersections(bodies, colliders, hooks, events); - // println!("Compute contact time: {}", instant::now() - t); + self.counters.cd.narrow_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); + } + fn build_islands_and_solve_constraints( + &mut self, + gravity: &Vector, + integration_parameters: &IntegrationParameters, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ) { self.counters.stages.island_construction_time.start(); bodies.update_active_set_with_contacts( colliders, @@ -135,16 +128,9 @@ 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(); bodies.foreach_active_dynamic_body_mut_internal(|_, b| { b.update_world_mass_properties(); @@ -218,7 +204,17 @@ impl PhysicsPipeline { }); }); } + self.counters.stages.solver_time.pause(); + } + fn run_ccd_motion_clamping( + &mut self, + integration_parameters: &IntegrationParameters, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ccd_solver: Option<&mut CCDSolver>, + events: &dyn EventHandler, + ) { // Handle CCD if let Some(ccd_solver) = ccd_solver { let impacts = ccd_solver.predict_next_impacts( @@ -230,7 +226,13 @@ impl PhysicsPipeline { ); ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts); } + } + fn advance_to_final_positions( + &mut self, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) { // Set the rigid-bodies and kinematic bodies to their final position. bodies.foreach_active_body_mut_internal(|_, rb| { if rb.is_kinematic() { @@ -241,11 +243,68 @@ impl PhysicsPipeline { 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()); + }); + } - bodies.modified_inactive_set.clear(); + /// Executes one timestep of the physics simulation. + pub fn step( + &mut self, + gravity: &Vector, + integration_parameters: &IntegrationParameters, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ccd_solver: Option<&mut CCDSolver>, + hooks: &dyn PhysicsHooks, + events: &dyn EventHandler, + ) { + self.counters.step_started(); + bodies.maintain(colliders); + self.interpolate_kinematic_velocities(integration_parameters, bodies); + self.detect_collisions( + integration_parameters, + broad_phase, + narrow_phase, + bodies, + colliders, + hooks, + events, + ); + self.build_islands_and_solve_constraints( + gravity, + integration_parameters, + narrow_phase, + bodies, + colliders, + joints, + ); + self.run_ccd_motion_clamping( + integration_parameters, + bodies, + colliders, + ccd_solver, + events, + ); + self.advance_to_final_positions(bodies, colliders); + + bodies.modified_inactive_set.clear(); self.counters.step_completed(); } } -- cgit From 8173e7ada2e3f5c99de53b532adc085a26c1cefd Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 14:54:54 +0200 Subject: Allow collider modification after its insersion to the ColliderSet. --- src/pipeline/collision_pipeline.rs | 22 +++-------- src/pipeline/physics_pipeline.rs | 75 +++++++++++++++++++++++++++++++++----- 2 files changed, 72 insertions(+), 25 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 5df3f6a..da572f3 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -44,19 +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(); self.broad_phase_events.clear(); - broad_phase.update( - prediction_distance, - bodies, - colliders, - &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); @@ -64,22 +60,16 @@ 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_next_position(integration_parameters.inv_dt()); - // }); - // 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 = &mut colliders[*handle]; - collider.prev_position = collider.position; + let collider = colliders.get_mut_internal(*handle).unwrap(); collider.position = rb.position * collider.delta; } }); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index fa01def..296a400 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -58,7 +58,7 @@ impl PhysicsPipeline { } } - fn detect_collisions( + fn detect_collisions_after_user_modifications( &mut self, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhase, @@ -70,23 +70,67 @@ impl PhysicsPipeline { ) { 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(); - broad_phase.update( integration_parameters.prediction_distance, - bodies, colliders, &mut self.broad_phase_events, ); - self.counters.cd.broad_phase_time.pause(); - - // println!("Num contact pairs: {}", pairs.len()); + self.counters.cd.broad_phase_time.pause(); self.counters.cd.narrow_phase_time.start(); - narrow_phase.maintain(colliders, bodies); + + // Update narrow-phase. + 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, + colliders, + hooks, + events, + ); + narrow_phase.compute_intersections(bodies, colliders, hooks, events); + + // Clear colliders modification flags. + colliders.clear_modified_colliders(); + + self.counters.cd.narrow_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); + } + + fn detect_collisions_after_integration( + &mut self, + integration_parameters: &IntegrationParameters, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + hooks: &dyn PhysicsHooks, + events: &dyn EventHandler, + ) { + self.counters.stages.collision_detection_time.resume(); + self.counters.cd.broad_phase_time.resume(); + + // Update broad-phase. + self.broad_phase_events.clear(); + self.broadphase_collider_pairs.clear(); + broad_phase.update( + integration_parameters.prediction_distance, + colliders, + &mut self.broad_phase_events, + ); + self.counters.cd.broad_phase_time.pause(); + self.counters.cd.narrow_phase_time.resume(); + + // Update narrow-phase. + // NOTE: we don't need to call `narrow_phase.handle_user_changes` because this + // has already been done at the beginning of the timestep. + narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); narrow_phase.compute_contacts( integration_parameters.prediction_distance, bodies, @@ -95,6 +139,9 @@ impl PhysicsPipeline { events, ); narrow_phase.compute_intersections(bodies, colliders, hooks, events); + // Clear colliders modification flags. + colliders.clear_modified_colliders(); + self.counters.cd.narrow_phase_time.pause(); self.counters.stages.collision_detection_time.pause(); } @@ -275,10 +322,11 @@ impl PhysicsPipeline { events: &dyn EventHandler, ) { self.counters.step_started(); - bodies.maintain(colliders); + colliders.handle_user_changes(bodies); + bodies.handle_user_changes(colliders); self.interpolate_kinematic_velocities(integration_parameters, bodies); - self.detect_collisions( + self.detect_collisions_after_user_modifications( integration_parameters, broad_phase, narrow_phase, @@ -303,6 +351,15 @@ impl PhysicsPipeline { events, ); self.advance_to_final_positions(bodies, colliders); + self.detect_collisions_after_integration( + integration_parameters, + broad_phase, + narrow_phase, + bodies, + colliders, + hooks, + events, + ); bodies.modified_inactive_set.clear(); self.counters.step_completed(); -- cgit From a733f97028f5cd532212572f9561ab64e09f002b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 29 Mar 2021 17:21:49 +0200 Subject: Implement the ability to run multiple CCD substeps. --- src/pipeline/mod.rs | 2 +- src/pipeline/physics_pipeline.rs | 145 ++++++++++++++++++++++++++------------- src/pipeline/query_pipeline.rs | 95 ++++++++++++++++++------- 3 files changed, 169 insertions(+), 73 deletions(-) (limited to 'src/pipeline') 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 296a400..99e1bd9 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -68,8 +68,8 @@ impl PhysicsPipeline { hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { - self.counters.stages.collision_detection_time.start(); - self.counters.cd.broad_phase_time.start(); + self.counters.stages.collision_detection_time.resume(); + self.counters.cd.broad_phase_time.resume(); // Update broad-phase. self.broad_phase_events.clear(); @@ -81,7 +81,7 @@ impl PhysicsPipeline { ); self.counters.cd.broad_phase_time.pause(); - self.counters.cd.narrow_phase_time.start(); + self.counters.cd.narrow_phase_time.resume(); // Update narrow-phase. narrow_phase.handle_user_changes(colliders, bodies, events); @@ -155,7 +155,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, joints: &mut JointSet, ) { - self.counters.stages.island_construction_time.start(); + self.counters.stages.island_construction_time.resume(); bodies.update_active_set_with_contacts( colliders, narrow_phase, @@ -178,15 +178,14 @@ impl PhysicsPipeline { narrow_phase.select_active_contacts(bodies, &mut manifolds, &mut self.manifold_indices); joints.select_active_interactions(bodies, &mut self.joint_constraint_indices); - 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); @@ -259,20 +258,17 @@ impl PhysicsPipeline { integration_parameters: &IntegrationParameters, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, - ccd_solver: Option<&mut CCDSolver>, + ccd_solver: &mut CCDSolver, events: &dyn EventHandler, ) { // Handle CCD - if let Some(ccd_solver) = ccd_solver { - let impacts = ccd_solver.predict_next_impacts( - integration_parameters, - bodies, - colliders, - integration_parameters.dt, - events, - ); - ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts); - } + let impacts = ccd_solver.predict_impacts_at_next_positions( + integration_parameters.dt, + bodies, + colliders, + events, + ); + ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts); } fn advance_to_final_positions( @@ -317,15 +313,15 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, - ccd_solver: Option<&mut CCDSolver>, + 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.interpolate_kinematic_velocities(integration_parameters, bodies); self.detect_collisions_after_user_modifications( integration_parameters, broad_phase, @@ -335,33 +331,90 @@ impl PhysicsPipeline { hooks, events, ); - self.build_islands_and_solve_constraints( - gravity, - integration_parameters, - narrow_phase, - bodies, - colliders, - joints, - ); - self.run_ccd_motion_clamping( - integration_parameters, - bodies, - colliders, - ccd_solver, - events, - ); - self.advance_to_final_positions(bodies, colliders); - self.detect_collisions_after_integration( - integration_parameters, - broad_phase, - narrow_phase, - bodies, - colliders, - hooks, - events, - ); - bodies.modified_inactive_set.clear(); + let mut remaining_time = integration_parameters.dt; + let mut remaining_substeps = integration_parameters.max_ccd_substeps; + let mut integration_parameters = *integration_parameters; + + let ccd_active = ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt); + + loop { + if ccd_active && remaining_substeps > 1 { + // 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. + // + // 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 let Some(toi) = ccd_solver.find_first_impact(remaining_time, bodies, colliders) { + 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 = 1; + } + + 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 = 1; + } + } else { + integration_parameters.dt = remaining_time; + remaining_time = 0.0; + remaining_substeps = 1; + } + + self.interpolate_kinematic_velocities(&integration_parameters, bodies); + self.build_islands_and_solve_constraints( + gravity, + &integration_parameters, + narrow_phase, + bodies, + colliders, + joints, + ); + + // If CCD is enabled, execute the CCD motion clamping. + if ccd_active && remaining_substeps > 0 { + self.run_ccd_motion_clamping( + &integration_parameters, + bodies, + colliders, + ccd_solver, + events, + ); + } + + self.advance_to_final_positions(bodies, colliders); + self.detect_collisions_after_integration( + &integration_parameters, + broad_phase, + narrow_phase, + bodies, + colliders, + hooks, + events, + ); + + bodies.modified_inactive_set.clear(); + + if !ccd_active || remaining_substeps <= 1 { + // We executed all the substeps. + break; + } + } self.counters.step_completed(); } } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index e89a03e..5b2cc88 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -38,6 +38,12 @@ struct QueryPipelineAsCompositeShape<'a> { groups: InteractionGroups, } +pub enum QueryPipelineMode { + CurrentPosition, + SweepTestWithNextPosition, + SweepTestWithPredictedPosition { dt: Real }, +} + impl<'a> TypedSimdCompositeShape for QueryPipelineAsCompositeShape<'a> { type PartShape = dyn Shape; type PartId = ColliderHandle; @@ -113,18 +119,40 @@ impl QueryPipeline { } /// Update the acceleration structure on the query pipeline. - pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) { + 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 { - if !use_swept_aabb { - let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); - } else { - let data = colliders.iter().map(|(h, co)| { - let next_position = - bodies[co.parent()].next_position * co.position_wrt_parent(); - (h, co.compute_swept_aabb(&next_position)) - }); - 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, co)| { + let next_position = + bodies[co.parent()].next_position * co.position_wrt_parent(); + (h, co.compute_swept_aabb(&next_position)) + }); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + } + QueryPipelineMode::SweepTestWithPredictedPosition { dt } => { + let data = colliders.iter().map(|(h, co)| { + let next_position = bodies[co.parent()] + .predict_position_using_velocity_and_forces(dt) + * co.position_wrt_parent(); + (h, co.compute_swept_aabb(&next_position)) + }); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + } } // FIXME: uncomment this once we handle insertion/removals properly. @@ -141,21 +169,36 @@ impl QueryPipeline { } } - if !use_swept_aabb { - self.quadtree.update( - |handle| colliders[*handle].compute_aabb(), - self.dilation_factor, - ); - } else { - 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, - ); + 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, + ); + } } } -- cgit From d2ee6420538d7ee524f2096995d4f44fcfef4551 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 30 Mar 2021 17:08:51 +0200 Subject: CCD: take angular motion and penetration depth into account in various thresholds. --- src/pipeline/query_pipeline.rs | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 5b2cc88..5451cfa 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -38,10 +38,20 @@ 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, - SweepTestWithPredictedPosition { dt: Real }, + /// 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> { @@ -137,19 +147,19 @@ impl QueryPipeline { self.quadtree.clear_and_rebuild(data, self.dilation_factor); } QueryPipelineMode::SweepTestWithNextPosition => { - let data = colliders.iter().map(|(h, co)| { + let data = colliders.iter().map(|(h, c)| { let next_position = - bodies[co.parent()].next_position * co.position_wrt_parent(); - (h, co.compute_swept_aabb(&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, co)| { - let next_position = bodies[co.parent()] + let data = colliders.iter().map(|(h, c)| { + let next_position = bodies[c.parent()] .predict_position_using_velocity_and_forces(dt) - * co.position_wrt_parent(); - (h, co.compute_swept_aabb(&next_position)) + * c.position_wrt_parent(); + (h, c.compute_swept_aabb(&next_position)) }); self.quadtree.clear_and_rebuild(data, self.dilation_factor); } -- cgit From 88933bd4317c6ae522a4af906919dffd2becc6f9 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 30 Mar 2021 17:11:52 +0200 Subject: Run the position solver after the CCD motion clamping. --- src/pipeline/physics_pipeline.rs | 170 ++++++++++++++++++++++----------------- 1 file changed, 97 insertions(+), 73 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 99e1bd9..40ae5d1 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -58,7 +58,7 @@ impl PhysicsPipeline { } } - fn detect_collisions_after_user_modifications( + fn detect_collisions( &mut self, integration_parameters: &IntegrationParameters, broad_phase: &mut BroadPhase, @@ -67,6 +67,7 @@ impl PhysicsPipeline { colliders: &mut ColliderSet, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, + handle_user_changes: bool, ) { self.counters.stages.collision_detection_time.resume(); self.counters.cd.broad_phase_time.resume(); @@ -84,7 +85,9 @@ impl PhysicsPipeline { self.counters.cd.narrow_phase_time.resume(); // Update narrow-phase. - narrow_phase.handle_user_changes(colliders, bodies, events); + 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, @@ -102,51 +105,27 @@ impl PhysicsPipeline { self.counters.stages.collision_detection_time.pause(); } - fn detect_collisions_after_integration( + fn solve_position_constraints( &mut self, integration_parameters: &IntegrationParameters, - broad_phase: &mut BroadPhase, - narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, - colliders: &mut ColliderSet, - hooks: &dyn PhysicsHooks, - events: &dyn EventHandler, ) { - self.counters.stages.collision_detection_time.resume(); - self.counters.cd.broad_phase_time.resume(); - - // Update broad-phase. - self.broad_phase_events.clear(); - self.broadphase_collider_pairs.clear(); - broad_phase.update( - integration_parameters.prediction_distance, - colliders, - &mut self.broad_phase_events, - ); - - self.counters.cd.broad_phase_time.pause(); - self.counters.cd.narrow_phase_time.resume(); - - // Update narrow-phase. - // NOTE: we don't need to call `narrow_phase.handle_user_changes` because this - // has already been done at the beginning of the timestep. - narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - narrow_phase.compute_contacts( - integration_parameters.prediction_distance, - bodies, - colliders, - hooks, - events, - ); - narrow_phase.compute_intersections(bodies, colliders, hooks, events); - // Clear colliders modification flags. - colliders.clear_modified_colliders(); + #[cfg(not(feature = "parallel"))] + { + enable_flush_to_zero!(); - self.counters.cd.narrow_phase_time.pause(); - self.counters.stages.collision_detection_time.pause(); + for island_id in 0..bodies.num_islands() { + self.solvers[island_id].solve_position_constraints( + island_id, + &mut self.counters, + integration_parameters, + bodies, + ) + } + } } - fn build_islands_and_solve_constraints( + fn build_islands_and_solve_velocity_constraints( &mut self, gravity: &Vector, integration_parameters: &IntegrationParameters, @@ -196,7 +175,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, @@ -246,6 +225,7 @@ impl PhysicsPipeline { &manifold_indices[island_id], joints, &joint_constraint_indices[island_id], + is_last_substep, ) }); }); @@ -258,23 +238,28 @@ impl PhysicsPipeline { 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(); } 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| { @@ -283,6 +268,11 @@ impl PhysicsPipeline { rb.angvel = na::zero(); } + if clear_forces { + rb.force = na::zero(); + rb.torque = na::zero(); + } + rb.position = rb.next_position; rb.update_colliders_positions(colliders); }); @@ -322,7 +312,7 @@ impl PhysicsPipeline { colliders.handle_user_changes(bodies); bodies.handle_user_changes(colliders); - self.detect_collisions_after_user_modifications( + self.detect_collisions( integration_parameters, broad_phase, narrow_phase, @@ -330,23 +320,41 @@ impl PhysicsPipeline { colliders, hooks, events, + true, ); let mut remaining_time = integration_parameters.dt; - let mut remaining_substeps = integration_parameters.max_ccd_substeps; let mut integration_parameters = *integration_parameters; - let ccd_active = ccd_solver.update_ccd_active_flags(bodies, integration_parameters.dt); - - loop { - if ccd_active && remaining_substeps > 1 { - // 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. - // - // 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 let Some(toi) = ccd_solver.find_first_impact(remaining_time, bodies, colliders) { + 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 { @@ -360,7 +368,7 @@ impl PhysicsPipeline { } else { // No impact, don't do any other substep after this one. integration_parameters.dt = remaining_time; - remaining_substeps = 1; + remaining_substeps = 0; } remaining_time -= integration_parameters.dt; @@ -368,16 +376,18 @@ impl PhysicsPipeline { // Avoid substep length that are too small. if remaining_time <= integration_parameters.min_ccd_dt { integration_parameters.dt += remaining_time; - remaining_substeps = 1; + remaining_substeps = 0; } } else { integration_parameters.dt = remaining_time; remaining_time = 0.0; - remaining_substeps = 1; + remaining_substeps = 0; } + self.counters.ccd.num_substeps += 1; + self.interpolate_kinematic_velocities(&integration_parameters, bodies); - self.build_islands_and_solve_constraints( + self.build_islands_and_solve_velocity_constraints( gravity, &integration_parameters, narrow_phase, @@ -387,18 +397,35 @@ impl PhysicsPipeline { ); // If CCD is enabled, execute the CCD motion clamping. - if ccd_active && remaining_substeps > 0 { - self.run_ccd_motion_clamping( - &integration_parameters, - bodies, - colliders, - ccd_solver, - events, - ); + 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, + ); + } } - self.advance_to_final_positions(bodies, colliders); - self.detect_collisions_after_integration( + // 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, @@ -406,15 +433,12 @@ impl PhysicsPipeline { colliders, hooks, events, + false, ); bodies.modified_inactive_set.clear(); - - if !ccd_active || remaining_substeps <= 1 { - // We executed all the substeps. - break; - } } + self.counters.step_completed(); } } -- cgit From e9f6384081e7f3722976b9fefda6926f5206e0a2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 31 Mar 2021 10:53:44 +0200 Subject: Fix the parallel solver to work properly with CCD. --- src/pipeline/physics_pipeline.rs | 33 +++++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) (limited to 'src/pipeline') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 40ae5d1..bab10b7 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -123,6 +123,36 @@ impl PhysicsPipeline { ) } } + + #[cfg(feature = "parallel")] + { + use crate::geometry::ContactManifold; + 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( @@ -216,7 +246,7 @@ impl PhysicsPipeline { let joints: &mut Vec = unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) }; - solver.solve_island( + solver.init_constraints_and_solve_velocity_constraints( scope, island_id, integration_parameters, @@ -225,7 +255,6 @@ impl PhysicsPipeline { &manifold_indices[island_id], joints, &joint_constraint_indices[island_id], - is_last_substep, ) }); }); -- cgit From 4e84c122df9838e530c7828f8b7b23477e04dc68 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 31 Mar 2021 16:55:18 +0200 Subject: Fix more warnings. --- src/pipeline/physics_pipeline.rs | 1 - 1 file changed, 1 deletion(-) (limited to 'src/pipeline') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index bab10b7..20e8395 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -126,7 +126,6 @@ impl PhysicsPipeline { #[cfg(feature = "parallel")] { - use crate::geometry::ContactManifold; use rayon::prelude::*; use std::sync::atomic::Ordering; -- cgit From a6b8b4b638553c79f3b476b99fa68aca4e0a910b Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 31 Mar 2021 17:46:59 +0200 Subject: Fix tests. --- src/pipeline/physics_pipeline.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) (limited to 'src/pipeline') diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 20e8395..0f7b5f6 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -473,7 +473,9 @@ impl PhysicsPipeline { #[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; @@ -505,6 +507,7 @@ mod test { &mut bodies, &mut colliders, &mut joints, + &mut CCDSolver::new(), &(), &(), ); @@ -548,6 +551,7 @@ mod test { &mut bodies, &mut colliders, &mut joints, + &mut CCDSolver::new(), &(), &(), ); -- cgit