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

14 files changed, 691 insertions, 873 deletions

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<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);
+        }
+    }
+}
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<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(
-                    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<Self> {
+    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<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);
+        }
+    }
+}
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<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);
+        }
+    }
+}
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<Isometry<Real>>,
     parallel_groups: ParallelInteractionGroups,
     parallel_joint_groups: ParallelInteractionGroups,
-    parallel_velocity_solver: ParallelVelocitySolver,
-    parallel_position_solver: ParallelPositionSolver,
+    parallel_contact_constraints:
+        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_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<JointGraphEdge> =
                     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<AnyVelocityConstraint, AnyPositionConstraint> = 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<AnyJointVelocityConstraint, AnyJointPositionConstraint> = 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<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);
-