Refactor the parallel solver code the same way we did with the non-parallel solver.

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];
 

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<Real>]) {
+        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<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

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<Self> {
+    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(
+                AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
-                    WBallPositionConstraint::from_params(rbs1, rbs2, joints),
+                    rbs1, rbs2, joints,
                 ))
             }
-            JointParams::FixedJoint(_) => None,
+            JointParams::FixedJoint(_) => {
-            JointParams::PrismaticJoint(_) => None,
+                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(
+    pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
-        joints: [&Joint; SIMD_WIDTH],
-        bodies: &RigidBodySet,
-    ) -> Option<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!(),
         }
     }

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(all(feature = "dim3", feature = "simd-is-enabled"))]
-#[cfg(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(all(feature = "dim3", feature = "simd-is-enabled"))]
-#[cfg(feature = "simd-is-enabled")]
 mod revolute_velocity_constraint_wide;

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<Real>]) {
+        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<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

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<Real>]) {
+        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<Real>]) {
+        for constraint in &self.constraints {
+            constraint.solve(params, positions);
+        }
+    }
+}

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")]

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<Isometry<Real>>,
     parallel_groups: ParallelInteractionGroups,
     parallel_joint_groups: ParallelInteractionGroups,
-    parallel_velocity_solver: ParallelVelocitySolver,
+    parallel_contact_constraints:
-    parallel_position_solver: ParallelPositionSolver,
+        ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
+    parallel_joint_constraints:
+        ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
     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_contact_constraints: ParallelSolverConstraints::new(),
-            parallel_position_solver: ParallelPositionSolver::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 =
+            let parallel_contact_constraints =
-                std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _);
+                std::sync::atomic::AtomicPtr::new(&mut self.parallel_contact_constraints as *mut _);
-            let parallel_position_solver =
+            let parallel_joint_constraints =
-                std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _);
+                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<JointGraphEdge> =
                     unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) };
-                let parallel_velocity_solver: &mut ParallelVelocitySolver = unsafe {
+                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
-                    std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed))
+                    std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
                 };
-                let parallel_position_solver: &mut ParallelPositionSolver = unsafe {
+                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> = unsafe {
-                    std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed))
+                    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_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
-                parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints);
+                parallel_joint_constraints.fill_constraints(&thread, params, bodies, joints);
-                parallel_velocity_solver
+                ThreadContext::lock_until_ge(
-                    .solve_constraints(&thread, params, manifolds, joints, mj_lambdas);
+                    &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! {

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::categorization::categorize_joints;
-use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint};
+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(crate) struct ParallelPositionSolver;
-    pub point_point: Vec<usize>,
-    pub plane_point: Vec<usize>,
-    pub ground_point_point: Vec<usize>,
-    pub ground_plane_point: Vec<usize>,
-    pub interaction_groups: InteractionGroups,
-    pub ground_interaction_groups: InteractionGroups,
-    pub constraints: Vec<AnyPositionConstraint>,
-    pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
-    pub parallel_desc_groups: Vec<usize>,
-}
-
-pub(crate) struct ParallelPositionSolverJointPart {
-    pub not_ground_interactions: Vec<usize>,
-    pub ground_interactions: Vec<usize>,
-    pub interaction_groups: InteractionGroups,
-    pub ground_interaction_groups: InteractionGroups,
-    pub constraints: Vec<AnyJointPositionConstraint>,
-    pub constraint_descs: Vec<(usize, PositionConstraintDesc)>,
-    pub parallel_desc_groups: Vec<usize>,
-}
-
-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,
-}
 
 impl ParallelPositionSolver {
-    pub fn new() -> Self {
+    pub fn solve(
-        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,
         thread: &ThreadContext,
         params: &IntegrationParameters,
         positions: &mut [Isometry<Real>],
+        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 contact_descs = &contact_constraints.constraint_descs[..];
-            let joint_descs = &self.joint_part.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();
             }

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<VelocityConstraint, PositionConstraint> {
+    pub not_ground_interactions: Vec<usize>,
+    pub ground_interactions: Vec<usize>,
+    pub interaction_groups: InteractionGroups,
+    pub ground_interaction_groups: InteractionGroups,
+    pub velocity_constraints: Vec<VelocityConstraint>,
+    pub position_constraints: Vec<PositionConstraint>,
+    pub constraint_descs: Vec<(usize, ConstraintDesc)>,
+    pub parallel_desc_groups: Vec<usize>,
+}
+
+impl<VelocityConstraint, PositionConstraint>
+    ParallelSolverConstraints<VelocityConstraint, PositionConstraint>
+{
+    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<AnyVelocityConstraint, AnyPositionConstraint> {
+    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<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
+    pub fn fill_constraints(
+        &mut self,
+        thread: &ThreadContext,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        let descs = &self.constraint_descs;
+
+        crate::concurrent_loop! {
+            let batch_size = thread.batch_size;
+            for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
+                match &desc.1 {
+                    ConstraintDesc::NongroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let velocity_constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
+                        let position_constraint = AnyJointPositionConstraint::from_joint(joint, bodies);
+                        self.velocity_constraints[joint.constraint_index] = velocity_constraint;
+                        self.position_constraints[joint.constraint_index] = position_constraint;
+                    }
+                    ConstraintDesc::GroundNongrouped(joint_id) => {
+                        let joint = &joints_all[*joint_id].weight;
+                        let velocity_constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
+                        let position_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
+                        self.velocity_constraints[joint.constraint_index] = velocity_constraint;
+                        self.position_constraints[joint.constraint_index] = position_constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    ConstraintDesc::NongroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
+                        let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
+                        self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
+                        self.position_constraints[joints[0].constraint_index] = position_constraint;
+                    }
+                    #[cfg(feature = "simd-is-enabled")]
+                    ConstraintDesc::GroundGrouped(joint_id) => {
+                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
+                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
+                        let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
+                        self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
+                        self.position_constraints[joints[0].constraint_index] = position_constraint;
+                    }
+                }
+            }
+        }
+    }
+}

src/dynamics/solver/parallel_velocity_solver.rs

@@ -1,325 +1,41 @@
 use super::ParallelInteractionGroups;
 use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
-use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
+use crate::dynamics::solver::parallel_solver_constraints::ConstraintDesc;
+use crate::dynamics::solver::{
+    AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups,
+    ParallelSolverConstraints, VelocityConstraint, VelocityGroundConstraint,
+};
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
 use crate::geometry::ContactManifold;
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
     dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
-    simd::SIMD_WIDTH,
+    math::{Real, SIMD_WIDTH},
 };
 use std::sync::atomic::Ordering;
 
-pub(crate) enum VelocityConstraintDesc {
+pub(crate) struct ParallelVelocitySolver {}
-    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<Constraint> {
-    pub not_ground_interactions: Vec<usize>,
-    pub ground_interactions: Vec<usize>,
-    pub interaction_groups: InteractionGroups,
-    pub ground_interaction_groups: InteractionGroups,
-    pub constraints: Vec<Constraint>,
-    pub constraint_descs: Vec<(usize, VelocityConstraintDesc)>,
-    pub parallel_desc_groups: Vec<usize>,
-}
-
-impl<Constraint> ParallelSolverConstraints<Constraint> {
-    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(),
-        }
-    }
-}
-
-macro_rules! impl_init_constraints_group {
-    ($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
-        impl ParallelSolverConstraints<$Constraint> {
-            pub fn init_constraints_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.constraint_index = total_num_constraints;
-                        self.constraint_descs.push((
-                            total_num_constraints,
-                            VelocityConstraintDesc::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.constraint_index = total_num_constraints;
-                        self.constraint_descs.push((
-                            total_num_constraints,
-                            VelocityConstraintDesc::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.constraint_index = total_num_constraints;
-                        self.constraint_descs.push((
-                            total_num_constraints,
-                            VelocityConstraintDesc::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.constraint_index = total_num_constraints;
-                        self.constraint_descs.push((
-                            total_num_constraints,
-                            VelocityConstraintDesc::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 constraints set.
-                self.constraints.clear();
-                self.constraints
-                    .resize_with(total_num_constraints, || $empty_constraint)
-            }
-        }
-    }
-}
-
-impl_init_constraints_group!(
-    AnyVelocityConstraint,
-    &mut ContactManifold,
-    categorize_contacts,
-    group_manifolds,
-    VelocityConstraint::num_active_constraints,
-    AnyVelocityConstraint::Empty
-);
-
-impl_init_constraints_group!(
-    AnyJointVelocityConstraint,
-    JointGraphEdge,
-    categorize_joints,
-    group_joints,
-    AnyJointVelocityConstraint::num_active_constraints,
-    AnyJointVelocityConstraint::Empty,
-    weight
-);
-
-impl ParallelSolverConstraints<AnyVelocityConstraint> {
-    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 {
-                    VelocityConstraintDesc::NongroundNongrouped(manifold_id) => {
-                        let manifold = &*manifolds_all[*manifold_id];
-                        VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
-                    }
-                    VelocityConstraintDesc::GroundNongrouped(manifold_id) => {
-                        let manifold = &*manifolds_all[*manifold_id];
-                        VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false);
-                    }
-                    #[cfg(feature = "simd-is-enabled")]
-                    VelocityConstraintDesc::NongroundGrouped(manifold_id) => {
-                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
-                        WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
-                    }
-                    #[cfg(feature = "simd-is-enabled")]
-                    VelocityConstraintDesc::GroundGrouped(manifold_id) => {
-                        let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
-                        WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false);
-                    }
-                }
-            }
-        }
-    }
-}
-
-impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
-    fn fill_constraints(
-        &mut self,
-        thread: &ThreadContext,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        joints_all: &[JointGraphEdge],
-    ) {
-        let descs = &self.constraint_descs;
-
-        crate::concurrent_loop! {
-            let batch_size = thread.batch_size;
-            for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] {
-                match &desc.1 {
-                    VelocityConstraintDesc::NongroundNongrouped(joint_id) => {
-                        let joint = &joints_all[*joint_id].weight;
-                        let constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies);
-                        self.constraints[joint.constraint_index] = constraint;
-                    }
-                    VelocityConstraintDesc::GroundNongrouped(joint_id) => {
-                        let joint = &joints_all[*joint_id].weight;
-                        let constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
-                        self.constraints[joint.constraint_index] = constraint;
-                    }
-                    #[cfg(feature = "simd-is-enabled")]
-                    VelocityConstraintDesc::NongroundGrouped(joint_id) => {
-                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
-                        let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
-                        self.constraints[joints[0].data.constraint_index] = constraint;
-                    }
-                    #[cfg(feature = "simd-is-enabled")]
-                    VelocityConstraintDesc::GroundGrouped(joint_id) => {
-                        let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
-                        let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
-                        self.constraints[joints[0].data.constraint_index] = constraint;
-                    }
-                }
-            }
-        }
-    }
-}
-
-pub(crate) struct ParallelVelocitySolver {
-    part: ParallelSolverConstraints<AnyVelocityConstraint>,
-    joint_part: ParallelSolverConstraints<AnyJointVelocityConstraint>,
-}
 
 impl ParallelVelocitySolver {
-    pub fn new() -> Self {
+    pub fn solve(
-        Self {
-            part: ParallelSolverConstraints::new(),
-            joint_part: ParallelSolverConstraints::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, params, bodies, joints);
-        ThreadContext::lock_until_ge(
-            &thread.num_initialized_constraints,
-            self.part.constraint_descs.len(),
-        );
-        ThreadContext::lock_until_ge(
-            &thread.num_initialized_joint_constraints,
-            self.joint_part.constraint_descs.len(),
-        );
-    }
-
-    pub fn solve_constraints(
-        &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
         mj_lambdas: &mut [DeltaVel<Real>],
+        contact_constraints: &mut ParallelSolverConstraints<
+            AnyVelocityConstraint,
+            AnyPositionConstraint,
+        >,
+        joint_constraints: &mut ParallelSolverConstraints<
+            AnyJointVelocityConstraint,
+            AnyJointPositionConstraint,
+        >,
     ) {
-        if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 {
+        if contact_constraints.constraint_descs.is_empty()
+            && joint_constraints.constraint_descs.is_empty()
+        {
             return;
         }
 
@@ -348,9 +64,9 @@ impl ParallelVelocitySolver {
                             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.velocity_constraints[$part.constraint_descs[start_index].0..]
                             } else {
-                                &mut $part.constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
+                                &mut $part.velocity_constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0]
                             };
 
                             for constraint in constraints {
@@ -380,10 +96,10 @@ impl ParallelVelocitySolver {
                 }
             );
 
-            warmstart!(self.joint_part);
+            warmstart!(joint_constraints);
-            shift = self.joint_part.constraint_descs.len();
+            shift = joint_constraints.constraint_descs.len();
             start_index -= shift;
-            warmstart!(self.part);
+            warmstart!(contact_constraints);
         }
 
         /*
@@ -398,8 +114,8 @@ impl ParallelVelocitySolver {
                 .solve_interaction_index
                 .fetch_add(thread.batch_size, Ordering::SeqCst);
             let mut batch_size = thread.batch_size;
-            let contact_descs = &self.part.constraint_descs[..];
+            let contact_descs = &contact_constraints.constraint_descs[..];
-            let joint_descs = &self.joint_part.constraint_descs[..];
+            let joint_descs = &joint_constraints.constraint_descs[..];
             let mut target_num_desc = 0;
             let mut shift = 0;
 
@@ -416,9 +132,12 @@ impl ParallelVelocitySolver {
                                 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.velocity_constraints
+                                        [$part.constraint_descs[start_index].0..]
                                 } else {
-                                    &mut $part.constraints[$part.constraint_descs[start_index].0
+                                    &mut $part.velocity_constraints[$part.constraint_descs
+                                        [start_index]
+                                        .0
                                         ..$part.constraint_descs[end_index].0]
                                 };
 
@@ -455,10 +174,10 @@ impl ParallelVelocitySolver {
                     };
                 }
 
-                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();
             }
@@ -467,8 +186,9 @@ impl ParallelVelocitySolver {
         /*
          * Writeback impulses.
          */
-        let joint_constraints = &self.joint_part.constraints;
+        let joint_constraints = &joint_constraints.velocity_constraints;
-        let contact_constraints = &self.part.constraints;
+        let contact_constraints = &contact_constraints.velocity_constraints;
+
         crate::concurrent_loop! {
              let batch_size = thread.batch_size;
              for constraint in joint_constraints[thread.joint_writeback_index] {

src/dynamics/solver/position_constraint.rs

@@ -53,8 +53,8 @@ pub(crate) struct PositionConstraint {
 impl PositionConstraint {
     #[cfg(feature = "parallel")]
     pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
-        let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
+        let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
-        manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
+        manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
     }
 
     pub fn generate(

src/dynamics/solver/solver_constraints.rs

@@ -318,18 +318,8 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
             );
             self.velocity_constraints.push(vel_constraint);
 
-            if let Some(pos_constraint) =
+            let pos_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
-                AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
+            self.position_constraints.push(pos_constraint);
-            {
-                self.position_constraints.push(pos_constraint);
-            } else {
-                for joint in joints.iter() {
-                    self.position_constraints
-                        .push(AnyJointPositionConstraint::from_joint_ground(
-                            *joint, bodies,
-                        ))
-                }
-            }
         }
     }
 
@@ -367,16 +357,8 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
                 AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
             self.velocity_constraints.push(vel_constraint);
 
-            if let Some(pos_constraint) =
+            let pos_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
-                AnyJointPositionConstraint::from_wide_joint(joints, bodies)
+            self.position_constraints.push(pos_constraint);
-            {
-                self.position_constraints.push(pos_constraint);
-            } else {
-                for joint in joints.iter() {
-                    self.position_constraints
-                        .push(AnyJointPositionConstraint::from_joint(*joint, bodies))
-                }
-            }
         }
     }
 }

src/dynamics/solver/velocity_constraint.rs

@@ -132,8 +132,8 @@ pub(crate) struct VelocityConstraint {
 impl VelocityConstraint {
     #[cfg(feature = "parallel")]
     pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
-        let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0;
+        let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
-        manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize
+        manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
     }
 
     pub fn generate(