From c32da78f2a6014c491aa3e975fb83ddb7c80610e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Apr 2021 17:59:25 +0200 Subject: Split rigid-bodies and colliders into multiple components --- src/dynamics/solver/parallel_island_solver.rs | 39 +++++++++++++++------------ 1 file changed, 22 insertions(+), 17 deletions(-) (limited to 'src/dynamics/solver/parallel_island_solver.rs') diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index add5f2c..115b099 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -1,9 +1,10 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; +use crate::data::ComponentSet; use crate::dynamics::solver::{ AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints, }; -use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Isometry, Real}; use crate::utils::WAngularInertia; @@ -150,13 +151,15 @@ impl ParallelIslandSolver { } } - pub fn solve_position_constraints<'s>( + pub fn solve_position_constraints<'s, Bodies>( &'s mut self, scope: &Scope<'s>, island_id: usize, params: &'s IntegrationParameters, - bodies: &'s mut RigidBodySet, - ) { + bodies: &'s mut Bodies, + ) where + Bodies: ComponentSet, + { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? @@ -179,7 +182,7 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let positions: &mut Vec> = unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; - let bodies: &mut RigidBodySet = + let bodies: &mut Bodies = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; let parallel_contact_constraints: &mut ParallelSolverConstraints = unsafe { std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed)) @@ -197,8 +200,8 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { + let rb = &mut bodies.index(handle.0); positions[rb.active_set_offset] = rb.next_position; } } @@ -216,8 +219,8 @@ impl ParallelIslandSolver { // Write results back to rigid bodies. concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.position_writeback_index] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.position_writeback_index) { + let rb = &mut bodies.index(handle.0); rb.set_next_position(positions[rb.active_set_offset]); } } @@ -225,17 +228,19 @@ impl ParallelIslandSolver { } } - pub fn init_constraints_and_solve_velocity_constraints<'s>( + pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>( &'s mut self, scope: &Scope<'s>, island_id: usize, params: &'s IntegrationParameters, - bodies: &'s mut RigidBodySet, + bodies: &'s mut Bodies, manifolds: &'s mut Vec<&'s mut ContactManifold>, manifold_indices: &'s [ContactManifoldIndex], joints: &'s mut Vec, joint_indices: &[JointIndex], - ) { + ) where + Bodies: ComponentSet, + { let num_threads = rayon::current_num_threads(); let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? @@ -280,7 +285,7 @@ impl ParallelIslandSolver { // Transmute *mut -> &mut let mj_lambdas: &mut Vec> = unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; - 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)) }; @@ -303,8 +308,8 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) { + let rb = &mut bodies.index(handle.0); let dvel = &mut mj_lambdas[rb.active_set_offset]; // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied @@ -348,8 +353,8 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; - for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { - let rb = &mut bodies[handle.0]; + for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) { + let rb = &mut bodies.index(handle.0); let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); -- cgit