From aa61fe65e3ff0289ecab57b4053a3410cf6d4a87 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 15:14:25 +0100 Subject: Add support of 64-bits reals. --- src/dynamics/solver/parallel_island_solver.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 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 3b7ab9f..543b9b7 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -2,7 +2,7 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; use crate::dynamics::solver::ParallelPositionSolver; use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; -use crate::math::Isometry; +use crate::math::{Isometry, Real}; use crate::utils::WAngularInertia; use rayon::Scope; use std::sync::atomic::{AtomicUsize, Ordering}; @@ -119,8 +119,8 @@ impl ThreadContext { } pub struct ParallelIslandSolver { - mj_lambdas: Vec>, - positions: Vec>, + mj_lambdas: Vec>, + positions: Vec>, parallel_groups: ParallelInteractionGroups, parallel_joint_groups: ParallelInteractionGroups, parallel_velocity_solver: ParallelVelocitySolver, @@ -199,9 +199,9 @@ impl ParallelIslandSolver { scope.spawn(move |_| { // Transmute *mut -> &mut - let mj_lambdas: &mut Vec> = + let mj_lambdas: &mut Vec> = unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; - let positions: &mut Vec> = + let positions: &mut Vec> = unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; let bodies: &mut RigidBodySet = unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; -- cgit From c28b14d31c43e1eb97a81df7673127d0c22d8deb Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 4 Jan 2021 17:59:51 +0100 Subject: Refactor the parallel solver code the same way we did with the non-parallel solver. --- src/dynamics/solver/parallel_island_solver.rs | 73 ++++++++++++++++++--------- 1 file changed, 48 insertions(+), 25 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 543b9b7..ea5ed23 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -1,5 +1,8 @@ use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; -use crate::dynamics::solver::ParallelPositionSolver; +use crate::dynamics::solver::{ + AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint, + AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints, +}; use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Isometry, Real}; @@ -123,8 +126,10 @@ pub struct ParallelIslandSolver { positions: Vec>, parallel_groups: ParallelInteractionGroups, parallel_joint_groups: ParallelInteractionGroups, - parallel_velocity_solver: ParallelVelocitySolver, - parallel_position_solver: ParallelPositionSolver, + parallel_contact_constraints: + ParallelSolverConstraints, + parallel_joint_constraints: + ParallelSolverConstraints, thread: ThreadContext, } @@ -135,8 +140,8 @@ impl ParallelIslandSolver { positions: Vec::new(), parallel_groups: ParallelInteractionGroups::new(), parallel_joint_groups: ParallelInteractionGroups::new(), - parallel_velocity_solver: ParallelVelocitySolver::new(), - parallel_position_solver: ParallelPositionSolver::new(), + parallel_contact_constraints: ParallelSolverConstraints::new(), + parallel_joint_constraints: ParallelSolverConstraints::new(), thread: ThreadContext::new(8), } } @@ -153,25 +158,21 @@ impl ParallelIslandSolver { joint_indices: &[JointIndex], ) { 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? + 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? self.parallel_groups .group_interactions(island_id, bodies, manifolds, manifold_indices); self.parallel_joint_groups .group_interactions(island_id, bodies, joints, joint_indices); - self.parallel_velocity_solver.init_constraint_groups( + self.parallel_contact_constraints.init_constraint_groups( island_id, bodies, manifolds, &self.parallel_groups, - joints, - &self.parallel_joint_groups, ); - self.parallel_position_solver.init_constraint_groups( + self.parallel_joint_constraints.init_constraint_groups( island_id, bodies, - manifolds, - &self.parallel_groups, joints, &self.parallel_joint_groups, ); @@ -192,10 +193,10 @@ impl ParallelIslandSolver { let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _); let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _); let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _); - let parallel_velocity_solver = - std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _); - let parallel_position_solver = - std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _); + let parallel_contact_constraints = + std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _); + let parallel_joint_constraints = + std::sync::atomic::AtomicPtr::new(&mut self.parallel_joint_constraints as *mut _); scope.spawn(move |_| { // Transmute *mut -> &mut @@ -209,18 +210,34 @@ impl ParallelIslandSolver { unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; let joints: &mut Vec = unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) }; - let parallel_velocity_solver: &mut ParallelVelocitySolver = unsafe { - std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed)) + let parallel_contact_constraints: &mut ParallelSolverConstraints = unsafe { + std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed)) }; - let parallel_position_solver: &mut ParallelPositionSolver = unsafe { - std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed)) + let parallel_joint_constraints: &mut ParallelSolverConstraints = unsafe { + std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed)) }; enable_flush_to_zero!(); // Ensure this is enabled on each thread. - parallel_velocity_solver.fill_constraints(&thread, params, bodies, manifolds, joints); - parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints); - parallel_velocity_solver - .solve_constraints(&thread, params, manifolds, joints, mj_lambdas); + parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds); + parallel_joint_constraints.fill_constraints(&thread, params, bodies, joints); + ThreadContext::lock_until_ge( + &thread.num_initialized_constraints, + parallel_contact_constraints.constraint_descs.len(), + ); + ThreadContext::lock_until_ge( + &thread.num_initialized_joint_constraints, + parallel_joint_constraints.constraint_descs.len(), + ); + + ParallelVelocitySolver::solve( + &thread, + params, + manifolds, + joints, + mj_lambdas, + parallel_contact_constraints, + parallel_joint_constraints + ); // Write results back to rigid bodies and integrate velocities. let island_range = bodies.active_island_range(island_id); @@ -243,7 +260,13 @@ impl ParallelIslandSolver { // initialized `positions` to the updated values. ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len()); - parallel_position_solver.solve_constraints(&thread, params, positions); + ParallelPositionSolver::solve( + &thread, + params, + positions, + parallel_contact_constraints, + parallel_joint_constraints + ); // Write results back to rigid bodies. concurrent_loop! { -- cgit From 0ade350b5f4b6e7c0c4116e1f4f2b30ab86b7e52 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Wed, 20 Jan 2021 16:33:42 +0100 Subject: Use newtypes for collider, rigid-body and joint handles. --- src/dynamics/solver/parallel_island_solver.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 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 ea5ed23..9a02330 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -247,7 +247,7 @@ 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]; + let rb = &mut bodies[handle.0]; let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); @@ -272,7 +272,7 @@ impl ParallelIslandSolver { concurrent_loop! { let batch_size = thread.batch_size; for handle in active_bodies[thread.position_writeback_index] { - let rb = &mut bodies[*handle]; + let rb = &mut bodies[handle.0]; rb.set_position_internal(positions[rb.active_set_offset]); } } -- cgit From 8f330b2a00610e5b68c1acd9208120e8f750c7aa Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 21 Jan 2021 14:58:40 +0100 Subject: Rotation locking: apply filter only to the world inertia properties to fix the multi-collider case. --- src/dynamics/solver/parallel_island_solver.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (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 9a02330..389b6e6 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -250,7 +250,7 @@ impl ParallelIslandSolver { let rb = &mut bodies[handle.0]; let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; - rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); + rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.integrate(params.dt()); positions[rb.active_set_offset] = rb.position; } -- cgit From b779eb261e7c3e816c8b586f1f496dcd17dbf700 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 22 Jan 2021 16:43:16 +0100 Subject: Fix compilation in 2D. --- src/dynamics/solver/parallel_island_solver.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (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 24435db..af8e9c0 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -251,7 +251,7 @@ impl ParallelIslandSolver { let dvel = mj_lambdas[rb.active_set_offset]; rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); - rb.integrate(params.dt)); + rb.integrate(params.dt); positions[rb.active_set_offset] = rb.position; } } -- cgit