Refactor the constraints solver code.

6 files changed, 435 insertions, 651 deletions

diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 1b40634..46d6b75 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -1,9 +1,15 @@
 use super::{PositionSolver, VelocitySolver};
 use crate::counters::Counters;
+use crate::dynamics::solver::{
+    AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
+    AnyVelocityConstraint, SolverConstraints,
+};
 use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 
 pub struct IslandSolver {
+    contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint>,
+    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint>,
     velocity_solver: VelocitySolver,
     position_solver: PositionSolver,
 }
@@ -11,6 +17,8 @@ pub struct IslandSolver {
 impl IslandSolver {
     pub fn new() -> Self {
         Self {
+            contact_constraints: SolverConstraints::new(),
+            joint_constraints: SolverConstraints::new(),
             velocity_solver: VelocitySolver::new(),
             position_solver: PositionSolver::new(),
         }
@@ -29,35 +37,23 @@ impl IslandSolver {
     ) {
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.velocity_assembly_time.resume();
-            self.position_solver.part.constraints.clear();
-            self.velocity_solver.init_constraints(
-                island_id,
-                params,
-                bodies,
-                manifolds,
-                &manifold_indices,
-                joints,
-                &joint_indices,
-                &mut self.position_solver.part.constraints,
-            );
+            self.contact_constraints
+                .init(island_id, params, bodies, manifolds, manifold_indices);
+            self.joint_constraints
+                .init(island_id, params, bodies, joints, joint_indices);
             counters.solver.velocity_assembly_time.pause();
 
             counters.solver.velocity_resolution_time.resume();
-            self.velocity_solver
-                .solve_constraints(island_id, params, bodies, manifolds, joints);
-            counters.solver.velocity_resolution_time.pause();
-
-            counters.solver.position_assembly_time.resume();
-            self.position_solver.init_constraints(
+            self.velocity_solver.solve(
                 island_id,
                 params,
                 bodies,
                 manifolds,
-                &manifold_indices,
                 joints,
-                &joint_indices,
+                &mut self.contact_constraints.velocity_constraints,
+                &mut self.joint_constraints.velocity_constraints,
             );
-            counters.solver.position_assembly_time.pause();
+            counters.solver.velocity_resolution_time.pause();
         }
 
         counters.solver.velocity_update_time.resume();
@@ -67,8 +63,13 @@ impl IslandSolver {
 
         if manifold_indices.len() != 0 || joint_indices.len() != 0 {
             counters.solver.position_resolution_time.resume();
-            self.position_solver
-                .solve_constraints(island_id, params, bodies);
+            self.position_solver.solve(
+                island_id,
+                params,
+                bodies,
+                &self.contact_constraints.position_constraints,
+                &self.joint_constraints.position_constraints,
+            );
             counters.solver.position_resolution_time.pause();
         }
     }
diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs
index 4d80f56..132b882 100644
--- a/src/dynamics/solver/mod.rs
+++ b/src/dynamics/solver/mod.rs
@@ -9,6 +9,8 @@ pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
 #[cfg(not(feature = "parallel"))]
 pub(self) use self::position_solver::PositionSolver;
 #[cfg(not(feature = "parallel"))]
+pub(self) use self::solver_constraints::SolverConstraints;
+#[cfg(not(feature = "parallel"))]
 pub(self) use self::velocity_solver::VelocitySolver;
 pub(self) use delta_vel::DeltaVel;
 pub(self) use interaction_groups::*;
@@ -46,6 +48,8 @@ mod position_ground_constraint;
 mod position_ground_constraint_wide;
 #[cfg(not(feature = "parallel"))]
 mod position_solver;
+#[cfg(not(feature = "parallel"))]
+mod solver_constraints;
 mod velocity_constraint;
 #[cfg(feature = "simd-is-enabled")]
 mod velocity_constraint_wide;
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index e467c62..6e8c681 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -20,7 +20,7 @@ pub(crate) enum VelocityConstraintDesc {
     GroundGrouped([usize; SIMD_WIDTH]),
 }
 
-pub(crate) struct ParallelVelocitySolverPart<Constraint> {
+pub(crate) struct ParallelSolverConstraints<Constraint> {
     pub not_ground_interactions: Vec<usize>,
     pub ground_interactions: Vec<usize>,
     pub interaction_groups: InteractionGroups,
@@ -30,7 +30,7 @@ pub(crate) struct ParallelVelocitySolverPart<Constraint> {
     pub parallel_desc_groups: Vec<usize>,
 }
 
-impl<Constraint> ParallelVelocitySolverPart<Constraint> {
+impl<Constraint> ParallelSolverConstraints<Constraint> {
     pub fn new() -> Self {
         Self {
             not_ground_interactions: Vec::new(),
@@ -46,7 +46,7 @@ impl<Constraint> ParallelVelocitySolverPart<Constraint> {
 
 macro_rules! impl_init_constraints_group {
     ($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => {
-        impl ParallelVelocitySolverPart<$Constraint> {
+        impl ParallelSolverConstraints<$Constraint> {
             pub fn init_constraints_groups(
                 &mut self,
                 island_id: usize,
@@ -181,7 +181,7 @@ impl_init_constraints_group!(
     weight
 );
 
-impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
+impl ParallelSolverConstraints<AnyVelocityConstraint> {
     fn fill_constraints(
         &mut self,
         thread: &ThreadContext,
@@ -219,7 +219,7 @@ impl ParallelVelocitySolverPart<AnyVelocityConstraint> {
     }
 }
 
-impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
+impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
     fn fill_constraints(
         &mut self,
         thread: &ThreadContext,
@@ -262,15 +262,15 @@ impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
 }
 
 pub(crate) struct ParallelVelocitySolver {
-    part: ParallelVelocitySolverPart<AnyVelocityConstraint>,
-    joint_part: ParallelVelocitySolverPart<AnyJointVelocityConstraint>,
+    part: ParallelSolverConstraints<AnyVelocityConstraint>,
+    joint_part: ParallelSolverConstraints<AnyJointVelocityConstraint>,
 }
 
 impl ParallelVelocitySolver {
     pub fn new() -> Self {
         Self {
-            part: ParallelVelocitySolverPart::new(),
-            joint_part: ParallelVelocitySolverPart::new(),
+            part: ParallelSolverConstraints::new(),
+            joint_part: ParallelSolverConstraints::new(),
         }
     }
 
diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs
index a0223a0..b5a953f 100644
--- a/src/dynamics/solver/position_solver.rs
+++ b/src/dynamics/solver/position_solver.rs
@@ -12,85 +12,24 @@ use crate::math::Isometry;
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::SIMD_WIDTH;
 
-pub(crate) struct PositionSolverJointPart {
-    pub nonground_joints: Vec<JointIndex>,
-    pub ground_joints: Vec<JointIndex>,
-    pub nonground_joint_groups: InteractionGroups,
-    pub ground_joint_groups: InteractionGroups,
-    pub constraints: Vec<AnyJointPositionConstraint>,
-}
-
-impl PositionSolverJointPart {
-    pub fn new() -> Self {
-        Self {
-            nonground_joints: Vec::new(),
-            ground_joints: Vec::new(),
-            nonground_joint_groups: InteractionGroups::new(),
-            ground_joint_groups: InteractionGroups::new(),
-            constraints: Vec::new(),
-        }
-    }
-}
-
-pub(crate) struct PositionSolverPart {
-    pub plane_point_manifolds: Vec<ContactManifoldIndex>,
-    pub plane_point_ground_manifolds: Vec<ContactManifoldIndex>,
-    pub plane_point_groups: InteractionGroups,
-    pub plane_point_ground_groups: InteractionGroups,
-    pub constraints: Vec<AnyPositionConstraint>,
-}
-
-impl PositionSolverPart {
-    pub fn new() -> Self {
-        Self {
-            plane_point_manifolds: Vec::new(),
-            plane_point_ground_manifolds: Vec::new(),
-            plane_point_groups: InteractionGroups::new(),
-            plane_point_ground_groups: InteractionGroups::new(),
-            constraints: Vec::new(),
-        }
-    }
-}
-
 pub(crate) struct PositionSolver {
     positions: Vec<Isometry<f32>>,
-    pub part: PositionSolverPart,
-    joint_part: PositionSolverJointPart,
 }
 
 impl PositionSolver {
     pub fn new() -> Self {
         Self {
             positions: Vec::new(),
-            part: PositionSolverPart::new(),
-            joint_part: PositionSolverJointPart::new(),
         }
     }
 
-    pub fn init_constraints(
-        &mut self,
-        island_id: usize,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        manifolds: &[&mut ContactManifold],
-        manifold_indices: &[ContactManifoldIndex],
-        joints: &[JointGraphEdge],
-        joint_constraint_indices: &[JointIndex],
-    ) {
-        self.joint_part.init_constraints(
-            island_id,
-            params,
-            bodies,
-            joints,
-            joint_constraint_indices,
-        );
-    }
-
-    pub fn solve_constraints(
+    pub fn solve(
         &mut self,
         island_id: usize,
         params: &IntegrationParameters,
         bodies: &mut RigidBodySet,
+        contact_constraints: &[AnyPositionConstraint],
+        joint_constraints: &[AnyJointPositionConstraint],
     ) {
         self.positions.clear();
         self.positions.extend(
@@ -100,11 +39,11 @@ impl PositionSolver {
         );
 
         for _ in 0..params.max_position_iterations {
-            for constraint in &self.joint_part.constraints {
+            for constraint in joint_constraints {
                 constraint.solve(params, &mut self.positions)
             }
 
-            for constraint in &self.part.constraints {
+            for constraint in contact_constraints {
                 constraint.solve(params, &mut self.positions)
             }
         }
@@ -114,201 +53,3 @@ impl PositionSolver {
         });
     }
 }
-
-impl PositionSolverJointPart {
-    pub fn init_constraints(
-        &mut self,
-        island_id: usize,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        joints: &[JointGraphEdge],
-        joint_constraint_indices: &[JointIndex],
-    ) {
-        self.ground_joints.clear();
-        self.nonground_joints.clear();
-        categorize_joints(
-            bodies,
-            joints,
-            joint_constraint_indices,
-            &mut self.ground_joints,
-            &mut self.nonground_joints,
-        );
-
-        self.nonground_joint_groups.clear_groups();
-        self.nonground_joint_groups
-            .group_joints(island_id, bodies, joints, &self.nonground_joints);
-
-        self.ground_joint_groups.clear_groups();
-        self.ground_joint_groups
-            .group_joints(island_id, bodies, joints, &self.ground_joints);
-
-        /*
-         * Init ground joint constraints.
-         */
-        self.constraints.clear();
-        compute_nongrouped_joint_ground_constraints(
-            params,
-            bodies,
-            joints,
-            &self.ground_joint_groups.nongrouped_interactions,
-            &mut self.constraints,
-        );
-
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            compute_grouped_joint_ground_constraints(
-                params,
-                bodies,
-                joints,
-                &self.ground_joint_groups.grouped_interactions,
-                &mut self.constraints,
-            );
-        }
-
-        /*
-         * Init non-ground joint constraints.
-         */
-        compute_nongrouped_joint_constraints(
-            params,
-            bodies,
-            joints,
-            &self.nonground_joint_groups.nongrouped_interactions,
-            &mut self.constraints,
-        );
-
-        #[cfg(feature = "simd-is-enabled")]
-        {
-            compute_grouped_joint_constraints(
-                params,
-                bodies,
-                joints,
-                &self.nonground_joint_groups.grouped_interactions,
-                &mut self.constraints,
-            );
-        }
-    }
-}
-
-fn compute_nongrouped_constraints(
-    params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    manifolds_all: &[&mut ContactManifold],
-    manifold_indices: &[ContactManifoldIndex],
-    output: &mut Vec<AnyPositionConstraint>,
-) {
-    for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
-        PositionConstraint::generate(params, manifold, bodies, output, true)
-    }
-}
-
-#[cfg(feature = "simd-is-enabled")]
-fn compute_grouped_constraints(
-    params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    manifolds_all: &[&mut ContactManifold],
-    manifold_indices: &[ContactManifoldIndex],
-    output: &mut Vec<AnyPositionConstraint>,
-) {
-    for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
-        let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
-        WPositionConstraint::generate(params, manifolds, bodies, output, true)
-    }
-}
-
-fn compute_nongrouped_ground_constraints(
-    params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    manifolds_all: &[&mut ContactManifold],
-    manifold_indices: &[ContactManifoldIndex],
-    output: &mut Vec<AnyPositionConstraint>,
-) {
-    for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) {
-        PositionGroundConstraint::generate(params, manifold, bodies, output, true)
-    }
-}
-
-#[cfg(feature = "simd-is-enabled")]
-fn compute_grouped_ground_constraints(
-    params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    manifolds_all: &[&mut ContactManifold],
-    manifold_indices: &[ContactManifoldIndex],
-    output: &mut Vec<AnyPositionConstraint>,
-) {
-    for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) {
-        let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
-        WPositionGroundConstraint::generate(params, manifolds, bodies, output, true);
-    }
-}
-
-fn compute_nongrouped_joint_ground_constraints(
-    _params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    joints_all: &[JointGraphEdge],
-    joint_indices: &[JointIndex],
-    output: &mut Vec<AnyJointPositionConstraint>,
-) {
-    for joint_i in joint_indices {
-        let joint = &joints_all[*joint_i].weight;
-        let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
-        output.push(pos_constraint);
-    }
-}
-
-#[cfg(feature = "simd-is-enabled")]
-fn compute_grouped_joint_ground_constraints(
-    _params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    joints_all: &[JointGraphEdge],
-    joint_indices: &[JointIndex],
-    output: &mut Vec<AnyJointPositionConstraint>,
-) {
-    for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
-        let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
-        if let Some(pos_constraint) =
-            AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
-        {
-            output.push(pos_constraint);
-        } else {
-            for joint in joints.iter() {
-                output.push(AnyJointPositionConstraint::from_joint_ground(
-                    *joint, bodies,
-                ))
-            }
-        }
-    }
-}
-
-fn compute_nongrouped_joint_constraints(
-    _params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    joints_all: &[JointGraphEdge],
-    joint_indices: &[JointIndex],
-    output: &mut Vec<AnyJointPositionConstraint>,
-) {
-    for joint_i in joint_indices {
-        let joint = &joints_all[*joint_i];
-        let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies);
-        output.push(pos_constraint);
-    }
-}
-
-#[cfg(feature = "simd-is-enabled")]
-fn compute_grouped_joint_constraints(
-    _params: &IntegrationParameters,
-    bodies: &RigidBodySet,
-    joints_all: &[JointGraphEdge],
-    joint_indices: &[JointIndex],
-    output: &mut Vec<AnyJointPositionConstraint>,
-) {
-    for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) {
-        let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH];
-        if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) {
-            output.push(pos_constraint);
-        } else {
-            for joint in joints.iter() {
-                output.push(AnyJointPositionConstraint::from_joint(*joint, bodies))
-            }
-        }
-    }
-}
diff --git a/src/dynamics/solver/solver_constraints.rs b/src/dynamics/solver/solver_constraints.rs
new file mode 100644
index 0000000..88371ba
--- /dev/null
+++ b/src/dynamics/solver/solver_constraints.rs
@@ -0,0 +1,383 @@
+use super::{
+    AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint,
+};
+#[cfg(feature = "simd-is-enabled")]
+use super::{WVelocityConstraint, WVelocityGroundConstraint};
+use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
+use crate::dynamics::solver::{
+    AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
+    PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
+};
+use crate::dynamics::{
+    solver::{AnyVelocityConstraint, DeltaVel},
+    IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
+};
+use crate::geometry::{ContactManifold, ContactManifoldIndex};
+#[cfg(feature = "simd-is-enabled")]
+use crate::math::SIMD_WIDTH;
+use crate::utils::WAngularInertia;
+
+pub(crate) struct SolverConstraints<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>,
+}
+
+impl<VelocityConstraint, PositionConstraint>
+    SolverConstraints<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(),
+        }
+    }
+}
+
+impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
+    pub fn init_constraint_groups(
+        &mut self,
+        island_id: usize,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.not_ground_interactions.clear();
+        self.ground_interactions.clear();
+        categorize_contacts(
+            bodies,
+            manifolds,
+            manifold_indices,
+            &mut self.ground_interactions,
+            &mut self.not_ground_interactions,
+        );
+
+        self.interaction_groups.clear_groups();
+        self.interaction_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds,
+            &self.not_ground_interactions,
+        );
+
+        self.ground_interaction_groups.clear_groups();
+        self.ground_interaction_groups.group_manifolds(
+            island_id,
+            bodies,
+            manifolds,
+            &self.ground_interactions,
+        );
+
+        // NOTE: uncomment this do disable SIMD contact resolution.
+        //        self.interaction_groups
+        //            .nongrouped_interactions
+        //            .append(&mut self.interaction_groups.grouped_interactions);
+        //        self.ground_interaction_groups
+        //            .nongrouped_interactions
+        //            .append(&mut self.ground_interaction_groups.grouped_interactions);
+    }
+
+    pub fn init(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds: &[&mut ContactManifold],
+        manifold_indices: &[ContactManifoldIndex],
+    ) {
+        self.velocity_constraints.clear();
+        self.position_constraints.clear();
+
+        self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
+
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_constraints(params, bodies, manifolds);
+        }
+        self.compute_nongrouped_constraints(params, bodies, manifolds);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_ground_constraints(params, bodies, manifolds);
+        }
+        self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifolds_i in self
+            .interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
+            let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+            WVelocityConstraint::generate(
+                params,
+                manifold_id,
+                manifolds,
+                bodies,
+                &mut self.velocity_constraints,
+                true,
+            );
+            WPositionConstraint::generate(
+                params,
+                manifolds,
+                bodies,
+                &mut self.position_constraints,
+                true,
+            );
+        }
+    }
+
+    fn compute_nongrouped_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifold_i in &self.interaction_groups.nongrouped_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            VelocityConstraint::generate(
+                params,
+                *manifold_i,
+                manifold,
+                bodies,
+                &mut self.velocity_constraints,
+                true,
+            );
+            PositionConstraint::generate(
+                params,
+                manifold,
+                bodies,
+                &mut self.position_constraints,
+                true,
+            );
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifolds_i in self
+            .ground_interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
+            let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
+            WVelocityGroundConstraint::generate(
+                params,
+                manifold_id,
+                manifolds,
+                bodies,
+                &mut self.velocity_constraints,
+                true,
+            );
+            WPositionGroundConstraint::generate(
+                params,
+                manifolds,
+                bodies,
+                &mut self.position_constraints,
+                true,
+            );
+        }
+    }
+
+    fn compute_nongrouped_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        manifolds_all: &[&mut ContactManifold],
+    ) {
+        for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
+            let manifold = &manifolds_all[*manifold_i];
+            VelocityGroundConstraint::generate(
+                params,
+                *manifold_i,
+                manifold,
+                bodies,
+                &mut self.velocity_constraints,
+                true,
+            );
+            PositionGroundConstraint::generate(
+                params,
+                manifold,
+                bodies,
+                &mut self.position_constraints,
+                true,
+            )
+        }
+    }
+}
+
+impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
+    pub fn init(
+        &mut self,
+        island_id: usize,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints: &[JointGraphEdge],
+        joint_constraint_indices: &[JointIndex],
+    ) {
+        // Generate constraints for joints.
+        self.not_ground_interactions.clear();
+        self.ground_interactions.clear();
+        categorize_joints(
+            bodies,
+            joints,
+            joint_constraint_indices,
+            &mut self.ground_interactions,
+            &mut self.not_ground_interactions,
+        );
+
+        self.velocity_constraints.clear();
+        self.position_constraints.clear();
+
+        self.interaction_groups.clear_groups();
+        self.interaction_groups.group_joints(
+            island_id,
+            bodies,
+            joints,
+            &self.not_ground_interactions,
+        );
+
+        self.ground_interaction_groups.clear_groups();
+        self.ground_interaction_groups.group_joints(
+            island_id,
+            bodies,
+            joints,
+            &self.ground_interactions,
+        );
+        // NOTE: uncomment this do disable SIMD joint resolution.
+        // self.interaction_groups
+        //     .nongrouped_interactions
+        //     .append(&mut self.interaction_groups.grouped_interactions);
+        // self.ground_interaction_groups
+        //     .nongrouped_interactions
+        //     .append(&mut self.ground_interaction_groups.grouped_interactions);
+
+        self.compute_nongrouped_joint_ground_constraints(params, bodies, joints);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_joint_ground_constraints(params, bodies, joints);
+        }
+        self.compute_nongrouped_joint_constraints(params, bodies, joints);
+        #[cfg(feature = "simd-is-enabled")]
+        {
+            self.compute_grouped_joint_constraints(params, bodies, joints);
+        }
+    }
+
+    fn compute_nongrouped_joint_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
+            let joint = &joints_all[*joint_i].weight;
+            let vel_constraint =
+                AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies);
+            self.velocity_constraints.push(vel_constraint);
+            let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies);
+            self.position_constraints.push(pos_constraint);
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_joint_ground_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joints_i in self
+            .ground_interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
+            let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
+            let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
+                params, joints_id, joints, bodies,
+            );
+            self.velocity_constraints.push(vel_constraint);
+
+            if let Some(pos_constraint) =
+                AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies)
+            {
+                self.position_constraints.push(pos_constraint);
+            } else {
+                for joint in joints.iter() {
+                    self.position_constraints
+                        .push(AnyJointPositionConstraint::from_joint_ground(
+                            *joint, bodies,
+                        ))
+                }
+            }
+        }
+    }
+
+    fn compute_nongrouped_joint_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joint_i in &self.interaction_groups.nongrouped_interactions {
+            let joint = &joints_all[*joint_i].weight;
+            let vel_constraint =
+                AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies);
+            self.velocity_constraints.push(vel_constraint);
+            let pos_constraint = AnyJointPositionConstraint::from_joint(joint, bodies);
+            self.position_constraints.push(pos_constraint);
+        }
+    }
+
+    #[cfg(feature = "simd-is-enabled")]
+    fn compute_grouped_joint_constraints(
+        &mut self,
+        params: &IntegrationParameters,
+        bodies: &RigidBodySet,
+        joints_all: &[JointGraphEdge],
+    ) {
+        for joints_i in self
+            .interaction_groups
+            .grouped_interactions
+            .chunks_exact(SIMD_WIDTH)
+        {
+            let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
+            let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
+            let vel_constraint =
+                AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
+            self.velocity_constraints.push(vel_constraint);
+
+            if let Some(pos_constraint) =
+                AnyJointPositionConstraint::from_wide_joint(joints, bodies)
+            {
+                self.position_constraints.push(pos_constraint);
+            } else {
+                for joint in joints.iter() {
+                    self.position_constraints
+                        .push(AnyJointPositionConstraint::from_joint(*joint, bodies))
+                }
+            }
+        }
+    }
+}
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index 5f68995..89f2809 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -5,8 +5,8 @@ use super::{
 use super::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
 use crate::dynamics::solver::{
-    AnyPositionConstraint, PositionConstraint, PositionGroundConstraint, WPositionConstraint,
-    WPositionGroundConstraint,
+    AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint,
+    PositionGroundConstraint, WPositionConstraint, WPositionGroundConstraint,
 };
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
@@ -19,54 +19,24 @@ use crate::utils::WAngularInertia;
 
 pub(crate) struct VelocitySolver {
     pub mj_lambdas: Vec<DeltaVel<f32>>,
-    pub contact_part: VelocitySolverPart<AnyVelocityConstraint>,
-    pub joint_part: VelocitySolverPart<AnyJointVelocityConstraint>,
 }
 
 impl VelocitySolver {
     pub fn new() -> Self {
         Self {
             mj_lambdas: Vec::new(),
-            contact_part: VelocitySolverPart::new(),
-            joint_part: VelocitySolverPart::new(),
         }
     }
 
-    pub fn init_constraints(
-        &mut self,
-        island_id: usize,
-        params: &IntegrationParameters,
-        bodies: &RigidBodySet,
-        manifolds: &[&mut ContactManifold],
-        manifold_indices: &[ContactManifoldIndex],
-        joints: &[JointGraphEdge],
-        joint_constraint_indices: &[JointIndex],
-        position_constraints: &mut Vec<AnyPositionConstraint>,
-