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

5 files changed, 257 insertions, 20 deletions

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);
+        }
+    }
+}