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/interaction_groups.rs | 2 +- .../fixed_position_constraint_wide.rs | 53 +++ .../joint_constraint/joint_position_constraint.rs | 94 +++- src/dynamics/solver/joint_constraint/mod.rs | 24 +- .../prismatic_position_constraint_wide.rs | 53 +++ .../revolute_position_constraint_wide.rs | 53 +++ src/dynamics/solver/mod.rs | 4 + src/dynamics/solver/parallel_island_solver.rs | 73 ++- src/dynamics/solver/parallel_position_solver.rs | 513 ++------------------- src/dynamics/solver/parallel_solver_constraints.rs | 309 +++++++++++++ src/dynamics/solver/parallel_velocity_solver.rs | 352 ++------------ src/dynamics/solver/position_constraint.rs | 4 +- src/dynamics/solver/solver_constraints.rs | 26 +- src/dynamics/solver/velocity_constraint.rs | 4 +- 14 files changed, 691 insertions(+), 873 deletions(-) create mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs create mode 100644 src/dynamics/solver/parallel_solver_constraints.rs (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index b409f98..2550a95 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -77,7 +77,7 @@ impl ParallelInteractionGroups { .iter() .zip(self.interaction_colors.iter_mut()) { - let body_pair = interactions[*interaction_id].data.body_pair(); + let body_pair = interactions[*interaction_id].body_pair(); let rb1 = &bodies[body_pair.body1]; let rb2 = &bodies[body_pair.body2]; diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs new file mode 100644 index 0000000..c37164a --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs @@ -0,0 +1,53 @@ +use super::{FixedPositionConstraint, FixedPositionGroundConstraint}; +use crate::dynamics::{FixedJoint, IntegrationParameters, JointIndex, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH}; +use crate::utils::WAngularInertia; +use na::Unit; + +// TODO: this does not uses SIMD optimizations yet. +#[derive(Debug)] +pub(crate) struct WFixedPositionConstraint { + constraints: [FixedPositionConstraint; SIMD_WIDTH], +} + +impl WFixedPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&FixedJoint; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} + +#[derive(Debug)] +pub(crate) struct WFixedPositionGroundConstraint { + constraints: [FixedPositionGroundConstraint; SIMD_WIDTH], +} + +impl WFixedPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&FixedJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs index 0ebe88e..a50897b 100644 --- a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -4,8 +4,15 @@ use super::{ }; #[cfg(feature = "dim3")] use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; +#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] +use super::{WRevolutePositionConstraint, WRevolutePositionGroundConstraint}; + #[cfg(feature = "simd-is-enabled")] -use super::{WBallPositionConstraint, WBallPositionGroundConstraint}; +use super::{ + WBallPositionConstraint, WBallPositionGroundConstraint, WFixedPositionConstraint, + WFixedPositionGroundConstraint, WPrismaticPositionConstraint, + WPrismaticPositionGroundConstraint, +}; use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; #[cfg(feature = "simd-is-enabled")] use crate::math::SIMD_WIDTH; @@ -20,12 +27,24 @@ pub(crate) enum AnyJointPositionConstraint { WBallGroundConstraint(WBallPositionGroundConstraint), FixedJoint(FixedPositionConstraint), FixedGroundConstraint(FixedPositionGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WFixedJoint(WFixedPositionConstraint), + #[cfg(feature = "simd-is-enabled")] + WFixedGroundConstraint(WFixedPositionGroundConstraint), PrismaticJoint(PrismaticPositionConstraint), PrismaticGroundConstraint(PrismaticPositionGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WPrismaticJoint(WPrismaticPositionConstraint), + #[cfg(feature = "simd-is-enabled")] + WPrismaticGroundConstraint(WPrismaticPositionGroundConstraint), #[cfg(feature = "dim3")] RevoluteJoint(RevolutePositionConstraint), #[cfg(feature = "dim3")] RevoluteGroundConstraint(RevolutePositionGroundConstraint), + #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] + WRevoluteJoint(WRevolutePositionConstraint), + #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] + WRevoluteGroundConstraint(WRevolutePositionGroundConstraint), #[allow(dead_code)] // The Empty variant is only used with parallel code. Empty, } @@ -71,21 +90,38 @@ impl AnyJointPositionConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option { + pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self { let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; match &joints[0].params { JointParams::BallJoint(_) => { let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; - Some(AnyJointPositionConstraint::WBallJoint( - WBallPositionConstraint::from_params(rbs1, rbs2, joints), + AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params( + rbs1, rbs2, joints, )) } - JointParams::FixedJoint(_) => None, - JointParams::PrismaticJoint(_) => None, + JointParams::FixedJoint(_) => { + let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params( + rbs1, rbs2, joints, + )) + } + JointParams::PrismaticJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WPrismaticJoint( + WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints), + ) + } #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(_) => None, + JointParams::RevoluteJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WRevoluteJoint( + WRevolutePositionConstraint::from_params(rbs1, rbs2, joints), + ) + } } } @@ -118,10 +154,7 @@ impl AnyJointPositionConstraint { } #[cfg(feature = "simd-is-enabled")] - pub fn from_wide_joint_ground( - joints: [&Joint; SIMD_WIDTH], - bodies: &RigidBodySet, - ) -> Option { + pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self { let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; @@ -136,14 +169,31 @@ impl AnyJointPositionConstraint { match &joints[0].params { JointParams::BallJoint(_) => { let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; - Some(AnyJointPositionConstraint::WBallGroundConstraint( + AnyJointPositionConstraint::WBallGroundConstraint( WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), - )) + ) + } + JointParams::FixedJoint(_) => { + let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WFixedGroundConstraint( + WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + ) + } + JointParams::PrismaticJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WPrismaticGroundConstraint( + WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + ) } - JointParams::FixedJoint(_) => None, - JointParams::PrismaticJoint(_) => None, #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(_) => None, + JointParams::RevoluteJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + AnyJointPositionConstraint::WRevoluteGroundConstraint( + WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + ) + } } } @@ -157,12 +207,24 @@ impl AnyJointPositionConstraint { AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions), AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions), AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WFixedJoint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WFixedGroundConstraint(c) => c.solve(params, positions), AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions), AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WPrismaticJoint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WPrismaticGroundConstraint(c) => c.solve(params, positions), #[cfg(feature = "dim3")] AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions), #[cfg(feature = "dim3")] AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions), + #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] + AnyJointPositionConstraint::WRevoluteJoint(c) => c.solve(params, positions), + #[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] + AnyJointPositionConstraint::WRevoluteGroundConstraint(c) => c.solve(params, positions), AnyJointPositionConstraint::Empty => unreachable!(), } } diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs index dc604a5..154ff83 100644 --- a/src/dynamics/solver/joint_constraint/mod.rs +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -9,6 +9,10 @@ pub(self) use ball_velocity_constraint_wide::{ WBallVelocityConstraint, WBallVelocityGroundConstraint, }; pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use fixed_position_constraint_wide::{ + WFixedPositionConstraint, WFixedPositionGroundConstraint, +}; pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint}; #[cfg(feature = "simd-is-enabled")] pub(self) use fixed_velocity_constraint_wide::{ @@ -19,6 +23,10 @@ pub(crate) use joint_position_constraint::AnyJointPositionConstraint; pub(self) use prismatic_position_constraint::{ PrismaticPositionConstraint, PrismaticPositionGroundConstraint, }; +#[cfg(feature = "simd-is-enabled")] +pub(self) use prismatic_position_constraint_wide::{ + WPrismaticPositionConstraint, WPrismaticPositionGroundConstraint, +}; pub(self) use prismatic_velocity_constraint::{ PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint, }; @@ -30,12 +38,15 @@ pub(self) use prismatic_velocity_constraint_wide::{ pub(self) use revolute_position_constraint::{ RevolutePositionConstraint, RevolutePositionGroundConstraint, }; +#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] +pub(self) use revolute_position_constraint_wide::{ + WRevolutePositionConstraint, WRevolutePositionGroundConstraint, +}; #[cfg(feature = "dim3")] pub(self) use revolute_velocity_constraint::{ RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint, }; -#[cfg(feature = "dim3")] -#[cfg(feature = "simd-is-enabled")] +#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] pub(self) use revolute_velocity_constraint_wide::{ WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint, }; @@ -47,19 +58,24 @@ mod ball_velocity_constraint; #[cfg(feature = "simd-is-enabled")] mod ball_velocity_constraint_wide; mod fixed_position_constraint; +#[cfg(feature = "simd-is-enabled")] +mod fixed_position_constraint_wide; mod fixed_velocity_constraint; #[cfg(feature = "simd-is-enabled")] mod fixed_velocity_constraint_wide; mod joint_constraint; mod joint_position_constraint; mod prismatic_position_constraint; +#[cfg(feature = "simd-is-enabled")] +mod prismatic_position_constraint_wide; mod prismatic_velocity_constraint; #[cfg(feature = "simd-is-enabled")] mod prismatic_velocity_constraint_wide; #[cfg(feature = "dim3")] mod revolute_position_constraint; +#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] +mod revolute_position_constraint_wide; #[cfg(feature = "dim3")] mod revolute_velocity_constraint; -#[cfg(feature = "dim3")] -#[cfg(feature = "simd-is-enabled")] +#[cfg(all(feature = "dim3", feature = "simd-is-enabled"))] mod revolute_velocity_constraint_wide; diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs new file mode 100644 index 0000000..3d87f42 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs @@ -0,0 +1,53 @@ +use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint}; +use crate::dynamics::{IntegrationParameters, JointIndex, PrismaticJoint, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH}; +use crate::utils::WAngularInertia; +use na::Unit; + +// TODO: this does not uses SIMD optimizations yet. +#[derive(Debug)] +pub(crate) struct WPrismaticPositionConstraint { + constraints: [PrismaticPositionConstraint; SIMD_WIDTH], +} + +impl WPrismaticPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&PrismaticJoint; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} + +#[derive(Debug)] +pub(crate) struct WPrismaticPositionGroundConstraint { + constraints: [PrismaticPositionGroundConstraint; SIMD_WIDTH], +} + +impl WPrismaticPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&PrismaticJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs new file mode 100644 index 0000000..44da104 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs @@ -0,0 +1,53 @@ +use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; +use crate::dynamics::{IntegrationParameters, JointIndex, RevoluteJoint, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector, SIMD_WIDTH}; +use crate::utils::WAngularInertia; +use na::Unit; + +// TODO: this does not uses SIMD optimizations yet. +#[derive(Debug)] +pub(crate) struct WRevolutePositionConstraint { + constraints: [RevolutePositionConstraint; SIMD_WIDTH], +} + +impl WRevolutePositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&RevoluteJoint; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} + +#[derive(Debug)] +pub(crate) struct WRevolutePositionGroundConstraint { + constraints: [RevolutePositionGroundConstraint; SIMD_WIDTH], +} + +impl WRevolutePositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&RevoluteJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + Self { + constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH], + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + for constraint in &self.constraints { + constraint.solve(params, positions); + } + } +} diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs index 132b882..090d0f3 100644 --- a/src/dynamics/solver/mod.rs +++ b/src/dynamics/solver/mod.rs @@ -5,6 +5,8 @@ pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContex #[cfg(feature = "parallel")] pub(self) use self::parallel_position_solver::ParallelPositionSolver; #[cfg(feature = "parallel")] +pub(self) use self::parallel_solver_constraints::ParallelSolverConstraints; +#[cfg(feature = "parallel")] pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver; #[cfg(not(feature = "parallel"))] pub(self) use self::position_solver::PositionSolver; @@ -39,6 +41,8 @@ mod parallel_island_solver; #[cfg(feature = "parallel")] mod parallel_position_solver; #[cfg(feature = "parallel")] +mod parallel_solver_constraints; +#[cfg(feature = "parallel")] mod parallel_velocity_solver; mod position_constraint; #[cfg(feature = "simd-is-enabled")] 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! { diff --git a/src/dynamics/solver/parallel_position_solver.rs b/src/dynamics/solver/parallel_position_solver.rs index dfcd0d4..67af3ea 100644 --- a/src/dynamics/solver/parallel_position_solver.rs +++ b/src/dynamics/solver/parallel_position_solver.rs @@ -1,14 +1,17 @@ use super::ParallelInteractionGroups; use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext}; -use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts}; -use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint}; +use crate::dynamics::solver::categorization::categorize_joints; +use crate::dynamics::solver::{ + AnyJointVelocityConstraint, AnyVelocityConstraint, InteractionGroups, + ParallelSolverConstraints, PositionConstraint, PositionGroundConstraint, +}; use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; use crate::geometry::ContactManifold; use crate::math::{Isometry, Real}; #[cfg(feature = "simd-is-enabled")] use crate::{ dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}, - simd::SIMD_WIDTH, + math::SIMD_WIDTH, }; use std::sync::atomic::Ordering; @@ -21,488 +24,25 @@ pub(crate) enum PositionConstraintDesc { GroundGrouped([usize; SIMD_WIDTH]), } -pub(crate) struct ParallelPositionSolverContactPart { - pub point_point: Vec, - pub plane_point: Vec, - pub ground_point_point: Vec, - pub ground_plane_point: Vec, - pub interaction_groups: InteractionGroups, - pub ground_interaction_groups: InteractionGroups, - pub constraints: Vec, - pub constraint_descs: Vec<(usize, PositionConstraintDesc)>, - pub parallel_desc_groups: Vec, -} - -pub(crate) struct ParallelPositionSolverJointPart { - pub not_ground_interactions: Vec, - pub ground_interactions: Vec, - pub interaction_groups: InteractionGroups, - pub ground_interaction_groups: InteractionGroups, - pub constraints: Vec, - pub constraint_descs: Vec<(usize, PositionConstraintDesc)>, - pub parallel_desc_groups: Vec, -} - -impl ParallelPositionSolverContactPart { - pub fn new() -> Self { - Self { - point_point: Vec::new(), - plane_point: Vec::new(), - ground_point_point: Vec::new(), - ground_plane_point: Vec::new(), - interaction_groups: InteractionGroups::new(), - ground_interaction_groups: InteractionGroups::new(), - constraints: Vec::new(), - constraint_descs: Vec::new(), - parallel_desc_groups: Vec::new(), - } - } -} -impl ParallelPositionSolverJointPart { - pub fn new() -> Self { - Self { - not_ground_interactions: Vec::new(), - ground_interactions: Vec::new(), - interaction_groups: InteractionGroups::new(), - ground_interaction_groups: InteractionGroups::new(), - constraints: Vec::new(), - constraint_descs: Vec::new(), - parallel_desc_groups: Vec::new(), - } - } -} - -impl ParallelPositionSolverJointPart { - pub fn init_constraints_groups( - &mut self, - island_id: usize, - bodies: &RigidBodySet, - joints: &mut [JointGraphEdge], - joint_groups: &ParallelInteractionGroups, - ) { - let mut total_num_constraints = 0; - let num_groups = joint_groups.num_groups(); - - self.interaction_groups.clear_groups(); - self.ground_interaction_groups.clear_groups(); - self.parallel_desc_groups.clear(); - self.constraint_descs.clear(); - self.parallel_desc_groups.push(0); - - for i in 0..num_groups { - let group = joint_groups.group(i); - - self.not_ground_interactions.clear(); - self.ground_interactions.clear(); - categorize_joints( - bodies, - joints, - group, - &mut self.ground_interactions, - &mut self.not_ground_interactions, - ); - - #[cfg(feature = "simd-is-enabled")] - let start_grouped = self.interaction_groups.grouped_interactions.len(); - let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); - #[cfg(feature = "simd-is-enabled")] - let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); - let start_nongrouped_ground = - self.ground_interaction_groups.nongrouped_interactions.len(); - - self.interaction_groups.group_joints( - island_id, - bodies, - joints, - &self.not_ground_interactions, - ); - self.ground_interaction_groups.group_joints( - island_id, - bodies, - joints, - &self.ground_interactions, - ); - - // Compute constraint indices. - for interaction_i in - &self.interaction_groups.nongrouped_interactions[start_nongrouped..] - { - let joint = &mut joints[*interaction_i].weight; - joint.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::NongroundNongrouped(*interaction_i), - )); - total_num_constraints += - AnyJointPositionConstraint::num_active_constraints(joint, false); - } - - #[cfg(feature = "simd-is-enabled")] - for interaction_i in - self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) - { - let joint = &mut joints[interaction_i[0]].weight; - joint.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::NongroundGrouped( - array![|ii| interaction_i[ii]; SIMD_WIDTH], - ), - )); - total_num_constraints += - AnyJointPositionConstraint::num_active_constraints(joint, true); - } - - for interaction_i in - &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] - { - let joint = &mut joints[*interaction_i].weight; - joint.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::GroundNongrouped(*interaction_i), - )); - total_num_constraints += - AnyJointPositionConstraint::num_active_constraints(joint, false); - } - - #[cfg(feature = "simd-is-enabled")] - for interaction_i in self.ground_interaction_groups.grouped_interactions - [start_grouped_ground..] - .chunks(SIMD_WIDTH) - { - let joint = &mut joints[interaction_i[0]].weight; - joint.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::GroundGrouped( - array![|ii| interaction_i[ii]; SIMD_WIDTH], - ), - )); - total_num_constraints += - AnyJointPositionConstraint::num_active_constraints(joint, true); - } - - self.parallel_desc_groups.push(self.constraint_descs.len()); - } - - // Resize the constraints set. - self.constraints.clear(); - self.constraints - .resize_with(total_num_constraints, || AnyJointPositionConstraint::Empty) - } - - fn fill_constraints( - &mut self, - thread: &ThreadContext, - bodies: &RigidBodySet, - joints_all: &[JointGraphEdge], - ) { - let descs = &self.constraint_descs; - - crate::concurrent_loop! { - let batch_size = thread.batch_size; - for desc in descs[thread.position_joint_constraint_initialization_index, thread.num_initialized_position_joint_constraints] { - match &desc.1 { - PositionConstraintDesc::NongroundNongrouped(joint_id) => { - let joint = &joints_all[*joint_id].weight; - let constraint = AnyJointPositionConstraint::from_joint( - joint, - bodies, - ); - self.constraints[joint.position_constraint_index] = constraint; - } - PositionConstraintDesc::GroundNongrouped(joint_id) => { - let joint = &joints_all[*joint_id].weight; - let constraint = AnyJointPositionConstraint::from_joint_ground( - joint, - bodies, - ); - self.constraints[joint.position_constraint_index] = constraint; - } - #[cfg(feature = "simd-is-enabled")] - PositionConstraintDesc::NongroundGrouped(joint_id) => { - let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; - if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint( - joints, bodies, - ) { - self.constraints[joints[0].position_constraint_index] = constraint - } else { - for ii in 0..SIMD_WIDTH { - let constraint = AnyJointPositionConstraint::from_joint(joints[ii], bodies); - self.constraints[joints[0].position_constraint_index + ii] = constraint; - } - } - } - #[cfg(feature = "simd-is-enabled")] - PositionConstraintDesc::GroundGrouped(joint_id) => { - let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; - if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint_ground( - joints, bodies, - ) { - self.constraints[joints[0].position_constraint_index] = constraint - } else { - for ii in 0..SIMD_WIDTH { - let constraint = AnyJointPositionConstraint::from_joint_ground(joints[ii], bodies); - self.constraints[joints[0].position_constraint_index + ii] = constraint; - } - } - } - } - } - } - } -} - -impl ParallelPositionSolverContactPart { - pub fn init_constraints_groups( - &mut self, - island_id: usize, - bodies: &RigidBodySet, - manifolds: &mut [&mut ContactManifold], - manifold_groups: &ParallelInteractionGroups, - ) { - let mut total_num_constraints = 0; - let num_groups = manifold_groups.num_groups(); - - self.interaction_groups.clear_groups(); - self.ground_interaction_groups.clear_groups(); - self.parallel_desc_groups.clear(); - self.constraint_descs.clear(); - self.parallel_desc_groups.push(0); - - for i in 0..num_groups { - let group = manifold_groups.group(i); - - self.ground_point_point.clear(); - self.ground_plane_point.clear(); - self.point_point.clear(); - self.plane_point.clear(); - categorize_position_contacts( - bodies, - manifolds, - group, - &mut self.ground_point_point, - &mut self.ground_plane_point, - &mut self.point_point, - &mut self.plane_point, - ); - - #[cfg(feature = "simd-is-enabled")] - let start_grouped = self.interaction_groups.grouped_interactions.len(); - let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); - #[cfg(feature = "simd-is-enabled")] - let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); - let start_nongrouped_ground = - self.ground_interaction_groups.nongrouped_interactions.len(); - - self.interaction_groups.group_manifolds( - island_id, - bodies, - manifolds, - &self.point_point, - ); - self.interaction_groups.group_manifolds( - island_id, - bodies, - manifolds, - &self.plane_point, - ); - self.ground_interaction_groups.group_manifolds( - island_id, - bodies, - manifolds, - &self.ground_point_point, - ); - self.ground_interaction_groups.group_manifolds( - island_id, - bodies, - manifolds, - &self.ground_plane_point, - ); - - // Compute constraint indices. - for interaction_i in - &self.interaction_groups.nongrouped_interactions[start_nongrouped..] - { - let manifold = &mut *manifolds[*interaction_i]; - manifold.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::NongroundNongrouped(*interaction_i), - )); - total_num_constraints += PositionConstraint::num_active_constraints(manifold); - } - - #[cfg(feature = "simd-is-enabled")] - for interaction_i in - self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) - { - let manifold = &mut *manifolds[interaction_i[0]]; - manifold.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::NongroundGrouped( - array![|ii| interaction_i[ii]; SIMD_WIDTH], - ), - )); - total_num_constraints += PositionConstraint::num_active_constraints(manifold); - } - - for interaction_i in - &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] - { - let manifold = &mut *manifolds[*interaction_i]; - manifold.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::GroundNongrouped(*interaction_i), - )); - total_num_constraints += PositionConstraint::num_active_constraints(manifold); - } - - #[cfg(feature = "simd-is-enabled")] - for interaction_i in self.ground_interaction_groups.grouped_interactions - [start_grouped_ground..] - .chunks(SIMD_WIDTH) - { - let manifold = &mut *manifolds[interaction_i[0]]; - manifold.position_constraint_index = total_num_constraints; - self.constraint_descs.push(( - total_num_constraints, - PositionConstraintDesc::GroundGrouped( - array![|ii| interaction_i[ii]; SIMD_WIDTH], - ), - )); - total_num_constraints += PositionConstraint::num_active_constraints(manifold); - } - - self.parallel_desc_groups.push(self.constraint_descs.len()); - } - - // Resize the constraints set. - self.constraints.clear(); - self.constraints - .resize_with(total_num_constraints, || AnyPositionConstraint::Empty) - } - - fn fill_constraints( - &mut self, - thread: &ThreadContext, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds_all: &[&mut ContactManifold], - ) { - let descs = &self.constraint_descs; - - crate::concurrent_loop! { - let batch_size = thread.batch_size; - for desc in descs[thread.position_constraint_initialization_index, thread.num_initialized_position_constraints] { - match &desc.1 { - PositionConstraintDesc::NongroundNongrouped(manifold_id) => { - let manifold = &*manifolds_all[*manifold_id]; - PositionConstraint::generate( - params, - manifold, - bodies, - &mut self.constraints, - false, - ); - } - PositionConstraintDesc::GroundNongrouped(manifold_id) => { - let manifold = &*manifolds_all[*manifold_id]; - PositionGroundConstraint::generate( - params, - manifold, - bodies, - &mut self.constraints, - false, - ); - } - #[cfg(feature = "simd-is-enabled")] - PositionConstraintDesc::NongroundGrouped(manifold_id) => { - let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; - WPositionConstraint::generate( - params, - manifolds, - bodies, - &mut self.constraints, - false, - ); - } - #[cfg(feature = "simd-is-enabled")] - PositionConstraintDesc::GroundGrouped(manifold_id) => { - let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; - WPositionGroundConstraint::generate( - params, - manifolds, - bodies, - &mut self.constraints, - false, - ); - } - } - } - } - } -} - -pub(crate) struct ParallelPositionSolver { - part: ParallelPositionSolverContactPart, - joint_part: ParallelPositionSolverJointPart, -} +pub(crate) struct ParallelPositionSolver; impl ParallelPositionSolver { - pub fn new() -> Self { - Self { - part: ParallelPositionSolverContactPart::new(), - joint_part: ParallelPositionSolverJointPart::new(), - } - } - - pub fn init_constraint_groups( - &mut self, - island_id: usize, - bodies: &RigidBodySet, - manifolds: &mut [&mut ContactManifold], - manifold_groups: &ParallelInteractionGroups, - joints: &mut [JointGraphEdge], - joint_groups: &ParallelInteractionGroups, - ) { - self.part - .init_constraints_groups(island_id, bodies, manifolds, manifold_groups); - self.joint_part - .init_constraints_groups(island_id, bodies, joints, joint_groups); - } - - pub fn fill_constraints( - &mut self, - thread: &ThreadContext, - params: &IntegrationParameters, - bodies: &RigidBodySet, - manifolds: &[&mut ContactManifold], - joints: &[JointGraphEdge], - ) { - self.part - .fill_constraints(thread, params, bodies, manifolds); - self.joint_part.fill_constraints(thread, bodies, joints); - ThreadContext::lock_until_ge( - &thread.num_initialized_position_constraints, - self.part.constraint_descs.len(), - ); - ThreadContext::lock_until_ge( - &thread.num_initialized_position_joint_constraints, - self.joint_part.constraint_descs.len(), - ); - } - - pub fn solve_constraints( - &mut self, + pub fn solve( thread: &ThreadContext, params: &IntegrationParameters, positions: &mut [Isometry], + contact_constraints: &mut ParallelSolverConstraints< + AnyVelocityConstraint, + AnyPositionConstraint, + >, + joint_constraints: &mut ParallelSolverConstraints< + AnyJointVelocityConstraint, + AnyJointPositionConstraint, + >, ) { - if self.part.constraint_descs.len() == 0 { + if contact_constraints.constraint_descs.is_empty() + && joint_constraints.constraint_descs.is_empty() + { return; } @@ -518,8 +58,8 @@ impl ParallelPositionSolver { .solve_position_interaction_index .fetch_add(thread.batch_size, Ordering::SeqCst); let mut batch_size = thread.batch_size; - let contact_descs = &self.part.constraint_descs[..]; - let joint_descs = &self.joint_part.constraint_descs[..]; + let contact_descs = &contact_constraints.constraint_descs[..]; + let joint_descs = &joint_constraints.constraint_descs[..]; let mut target_num_desc = 0; let mut shift = 0; @@ -535,9 +75,12 @@ impl ParallelPositionSolver { let end_index = (start_index + batch_size).min(group[1]); let constraints = if end_index == $part.constraint_descs.len() { - &mut $part.constraints[$part.constraint_descs[start_index].0..] + &mut $part.position_constraints + [$part.constraint_descs[start_index].0..] } else { - &mut $part.constraints[$part.constraint_descs[start_index].0 + &mut $part.position_constraints[$part.constraint_descs + [start_index] + .0 ..$part.constraint_descs[end_index].0] }; @@ -570,10 +113,10 @@ impl ParallelPositionSolver { }; } - solve!(self.joint_part); + solve!(joint_constraints); shift += joint_descs.len(); start_index -= joint_descs.len(); - solve!(self.part); + solve!(contact_constraints); shift += contact_descs.len(); start_index -= contact_descs.len(); } diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs new file mode 100644 index 0000000..fe144bb --- /dev/null +++ b/src/dynamics/solver/parallel_solver_constraints.rs @@ -0,0 +1,309 @@ +use super::ParallelInteractionGroups; +use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext}; +use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints}; +use crate::dynamics::solver::{ + AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint, + PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint, WPositionConstraint, + WPositionGroundConstraint, +}; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; +use crate::geometry::ContactManifold; +#[cfg(feature = "simd-is-enabled")] +use crate::{ + dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}, + math::{Real, SIMD_WIDTH}, +}; +use std::sync::atomic::Ordering; + +// pub fn init_constraint_groups( +// &mut self, +// island_id: usize, +// bodies: &RigidBodySet, +// manifolds: &mut [&mut ContactManifold], +// manifold_groups: &ParallelInteractionGroups, +// joints: &mut [JointGraphEdge], +// joint_groups: &ParallelInteractionGroups, +// ) { +// self.part +// .init_constraints_groups(island_id, bodies, manifolds, manifold_groups); +// self.joint_part +// .init_constraints_groups(island_id, bodies, joints, joint_groups); +// } + +pub(crate) enum ConstraintDesc { + NongroundNongrouped(usize), + GroundNongrouped(usize), + #[cfg(feature = "simd-is-enabled")] + NongroundGrouped([usize; SIMD_WIDTH]), + #[cfg(feature = "simd-is-enabled")] + GroundGrouped([usize; SIMD_WIDTH]), +} + +pub(crate) struct ParallelSolverConstraints { + pub not_ground_interactions: Vec, + pub ground_interactions: Vec, + pub interaction_groups: InteractionGroups, + pub ground_interaction_groups: InteractionGroups, + pub velocity_constraints: Vec, + pub position_constraints: Vec, + pub constraint_descs: Vec<(usize, ConstraintDesc)>, + pub parallel_desc_groups: Vec, +} + +impl + ParallelSolverConstraints +{ + pub fn new() -> Self { + Self { + not_ground_interactions: Vec::new(), + ground_interactions: Vec::new(), + interaction_groups: InteractionGroups::new(), + ground_interaction_groups: InteractionGroups::new(), + velocity_constraints: Vec::new(), + position_constraints: Vec::new(), + constraint_descs: Vec::new(), + parallel_desc_groups: Vec::new(), + } + } +} + +macro_rules! impl_init_constraints_group { + ($VelocityConstraint: ty, $PositionConstraint: ty, $Interaction: ty, + $categorize: ident, $group: ident, + $data: ident$(.$constraint_index: ident)*, + $num_active_constraints: path, $empty_velocity_constraint: expr, $empty_position_constraint: expr $(, $weight: ident)*) => { + impl ParallelSolverConstraints<$VelocityConstraint, $PositionConstraint> { + pub fn init_constraint_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &mut [$Interaction], + interaction_groups: &ParallelInteractionGroups, + ) { + let mut total_num_constraints = 0; + let num_groups = interaction_groups.num_groups(); + + self.interaction_groups.clear_groups(); + self.ground_interaction_groups.clear_groups(); + self.parallel_desc_groups.clear(); + self.constraint_descs.clear(); + self.parallel_desc_groups.push(0); + + for i in 0..num_groups { + let group = interaction_groups.group(i); + + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + $categorize( + bodies, + interactions, + group, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + #[cfg(feature = "simd-is-enabled")] + let start_grouped = self.interaction_groups.grouped_interactions.len(); + let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); + #[cfg(feature = "simd-is-enabled")] + let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); + let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len(); + + self.interaction_groups.$group( + island_id, + bodies, + interactions, + &self.not_ground_interactions, + ); + self.ground_interaction_groups.$group( + island_id, + bodies, + interactions, + &self.ground_interactions, + ); + + // Compute constraint indices. + for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] { + let interaction = &mut interactions[*interaction_i]$(.$weight)*; + interaction.$data$(.$constraint_index)* = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + ConstraintDesc::NongroundNongrouped(*interaction_i), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in + self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) + { + let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; + interaction.$data$(.$constraint_index)* = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + ConstraintDesc::NongroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + for interaction_i in + &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] + { + let interaction = &mut interactions[*interaction_i]$(.$weight)*; + interaction.$data$(.$constraint_index)* = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + ConstraintDesc::GroundNongrouped(*interaction_i), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in self.ground_interaction_groups.grouped_interactions + [start_grouped_ground..] + .chunks(SIMD_WIDTH) + { + let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; + interaction.$data$(.$constraint_index)* = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + ConstraintDesc::GroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + self.parallel_desc_groups.push(self.constraint_descs.len()); + } + + // Resize the constraint sets. + self.velocity_constraints.clear(); + self.velocity_constraints + .resize_with(total_num_constraints, || $empty_velocity_constraint); + self.position_constraints.clear(); + self.position_constraints + .resize_with(total_num_constraints, || $empty_position_constraint); + } + } + } +} + +impl_init_constraints_group!( + AnyVelocityConstraint, + AnyPositionConstraint, + &mut ContactManifold, + categorize_contacts, + group_manifolds, + data.constraint_index, + VelocityConstraint::num_active_constraints, + AnyVelocityConstraint::Empty, + AnyPositionConstraint::Empty +); + +impl_init_constraints_group!( + AnyJointVelocityConstraint, + AnyJointPositionConstraint, + JointGraphEdge, + categorize_joints, + group_joints, + constraint_index, + AnyJointVelocityConstraint::num_active_constraints, + AnyJointVelocityConstraint::Empty, + AnyJointPositionConstraint::Empty, + weight +); + +impl ParallelSolverConstraints { + pub fn fill_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let descs = &self.constraint_descs; + + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] { + match &desc.1 { + ConstraintDesc::NongroundNongrouped(manifold_id) => { + let manifold = &*manifolds_all[*manifold_id]; + VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false); + PositionConstraint::generate(params, manifold, bodies, &mut self.position_constraints, false); + } + ConstraintDesc::GroundNongrouped(manifold_id) => { + let manifold = &*manifolds_all[*manifold_id]; + VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false); + PositionGroundConstraint::generate(params, manifold, bodies, &mut self.position_constraints, false); + } + #[cfg(feature = "simd-is-enabled")] + ConstraintDesc::NongroundGrouped(manifold_id) => { + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false); + WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false); + } + #[cfg(feature = "simd-is-enabled")] + ConstraintDesc::GroundGrouped(manifold_id) => { + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false); + WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false); + } + } + } + } + } +} + +impl ParallelSolverConstraints { + pub fn fill_constrai