Start fixing the parallel version.

6 files changed, 396 insertions, 164 deletions

diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs
index 162cb7f..c006417 100644
--- a/src/dynamics/solver/joint_constraint/joint_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs
@@ -7,12 +7,12 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
 };
 use crate::dynamics::solver::DeltaVel;
 use crate::dynamics::{
-    ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
+    ImpulseJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, RigidBodyIds,
     RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
 };
 #[cfg(feature = "simd-is-enabled")]
 use crate::math::{Isometry, SimdReal, SIMD_WIDTH};
-use crate::math::{Real, SPATIAL_DIM};
+use crate::math::{Real, DIM, SPATIAL_DIM};
 use crate::prelude::MultibodyJointSet;
 use na::DVector;
 
@@ -30,8 +30,20 @@ pub enum AnyJointVelocityConstraint {
 
 impl AnyJointVelocityConstraint {
     #[cfg(feature = "parallel")]
-    pub fn num_active_constraints(_: &ImpulseJoint) -> usize {
-        1
+    pub fn num_active_constraints(joint: &ImpulseJoint) -> usize {
+        let joint = &joint.data;
+        let locked_axes = joint.locked_axes.bits();
+        let motor_axes = joint.motor_axes.bits() & !locked_axes;
+        let limit_axes = joint.limit_axes.bits() & !locked_axes;
+        let coupled_axes = joint.coupled_axes.bits();
+
+        (motor_axes & !coupled_axes).count_ones() as usize
+            + ((motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+            + ((motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
+            + locked_axes.count_ones() as usize
+            + (limit_axes & !coupled_axes).count_ones() as usize
+            + ((limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0) as usize
+            + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize
     }
 
     pub fn from_joint<Bodies>(
@@ -43,6 +55,7 @@ impl AnyJointVelocityConstraint {
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
         out: &mut Vec<Self>,
+        push: bool,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -130,8 +143,15 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
+            if push {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
+                    out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
+                }
+            } else {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                    out[joint.constraint_index + i] =
+                        AnyJointVelocityConstraint::JointGenericConstraint(c);
+                }
             }
         } else {
             // TODO: find a way to avoid the temporary buffer.
@@ -147,8 +167,15 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointConstraint(c));
+            if push {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
+                    out.push(AnyJointVelocityConstraint::JointConstraint(c));
+                }
+            } else {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                    out[joint.constraint_index + i] =
+                        AnyJointVelocityConstraint::JointConstraint(c);
+                }
             }
         }
     }
@@ -160,6 +187,7 @@ impl AnyJointVelocityConstraint {
         impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
         bodies: &Bodies,
         out: &mut Vec<Self>,
+        push: bool,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -232,8 +260,15 @@ impl AnyJointVelocityConstraint {
             &mut out_tmp,
         );
 
-        for c in out_tmp.into_iter().take(out_tmp_len) {
-            out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
+        if push {
+            for c in out_tmp.into_iter().take(out_tmp_len) {
+                out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
+            }
+        } else {
+            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                out[impulse_joints[0].constraint_index + i] =
+                    AnyJointVelocityConstraint::JointConstraintSimd(c);
+            }
         }
     }
 
@@ -246,6 +281,7 @@ impl AnyJointVelocityConstraint {
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
         out: &mut Vec<Self>,
+        push: bool,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyType>
@@ -334,8 +370,15 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
+            if push {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
+                    out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
+                }
+            } else {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                    out[joint.constraint_index + i] =
+                        AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
+                }
             }
         } else {
             // TODO: find a way to avoid the temporary buffer.
@@ -351,8 +394,15 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
+            if push {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
+                    out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
+                }
+            } else {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                    out[joint.constraint_index + i] =
+                        AnyJointVelocityConstraint::JointGroundConstraint(c);
+                }
             }
         }
     }
@@ -364,6 +414,7 @@ impl AnyJointVelocityConstraint {
         impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
         bodies: &Bodies,
         out: &mut Vec<Self>,
+        push: bool,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyType>
@@ -455,8 +506,15 @@ impl AnyJointVelocityConstraint {
             &mut out_tmp,
         );
 
-        for c in out_tmp.into_iter().take(out_tmp_len) {
-            out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
+        if push {
+            for c in out_tmp.into_iter().take(out_tmp_len) {
+                out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
+            }
+        } else {
+            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                out[impulse_joints[0].constraint_index + i] =
+                    AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
+            }
         }
     }
 
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index 6d5debe..96dc403 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -14,7 +14,7 @@ use crate::dynamics::{
 };
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Isometry, Real};
-use crate::utils::WAngularInertia;
+use na::DVector;
 
 use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
 
@@ -60,6 +60,25 @@ macro_rules! concurrent_loop {
             }
         }
     };
+
+    (let batch_size = $batch_size: expr;
+        for $elt: ident in &mut $array: ident[$index_stream:expr] $f: expr) => {
+        let max_index = $array.len();
+
+        if max_index > 0 {
+            loop {
+                let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst);
+                if start_index > max_index {
+                    break;
+                }
+
+                let end_index = (start_index + $batch_size).min(max_index);
+                for $elt in &mut $array[start_index..end_index] {
+                    $f
+                }
+            }
+        }
+    };
 }
 
 pub(crate) struct ThreadContext {
@@ -73,10 +92,14 @@ pub(crate) struct ThreadContext {
     pub num_solved_interactions: AtomicUsize,
     pub impulse_writeback_index: AtomicUsize,
     pub joint_writeback_index: AtomicUsize,
-    pub body_integration_index: AtomicUsize,
+    pub impulse_rm_bias_index: AtomicUsize,
+    pub joint_rm_bias_index: AtomicUsize,
+    pub body_integration_pos_index: AtomicUsize,
+    pub body_integration_vel_index: AtomicUsize,
     pub body_force_integration_index: AtomicUsize,
     pub num_force_integrated_bodies: AtomicUsize,
-    pub num_integrated_bodies: AtomicUsize,
+    pub num_integrated_pos_bodies: AtomicUsize,
+    pub num_integrated_vel_bodies: AtomicUsize,
 }
 
 impl ThreadContext {
@@ -91,10 +114,14 @@ impl ThreadContext {
             num_solved_interactions: AtomicUsize::new(0),
             impulse_writeback_index: AtomicUsize::new(0),
             joint_writeback_index: AtomicUsize::new(0),
+            impulse_rm_bias_index: AtomicUsize::new(0),
+            joint_rm_bias_index: AtomicUsize::new(0),
             body_force_integration_index: AtomicUsize::new(0),
             num_force_integrated_bodies: AtomicUsize::new(0),
-            body_integration_index: AtomicUsize::new(0),
-            num_integrated_bodies: AtomicUsize::new(0),
+            body_integration_pos_index: AtomicUsize::new(0),
+            body_integration_vel_index: AtomicUsize::new(0),
+            num_integrated_pos_bodies: AtomicUsize::new(0),
+            num_integrated_vel_bodies: AtomicUsize::new(0),
         }
     }
 
@@ -151,12 +178,12 @@ impl ParallelIslandSolver {
         manifold_indices: &'s [ContactManifoldIndex],
         impulse_joints: &'s mut Vec<JointGraphEdge>,
         joint_indices: &[JointIndex],
-        multibody_joints: &mut MultibodyJointSet,
+        multibodies: &mut MultibodyJointSet,
     ) where
         Bodies: ComponentSet<RigidBodyForces>
             + ComponentSetMut<RigidBodyPosition>
             + ComponentSetMut<RigidBodyVelocity>
-            + ComponentSet<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyMassProps>
             + ComponentSet<RigidBodyDamping>
             + ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyType>,
@@ -182,7 +209,7 @@ impl ParallelIslandSolver {
             island_id,
             islands,
             bodies,
-            multibody_joints,
+            multibodies,
             manifolds,
             &self.parallel_groups,
         );
@@ -190,7 +217,7 @@ impl ParallelIslandSolver {
             island_id,
             islands,
             bodies,
-            multibody_joints,
+            multibodies,
             impulse_joints,
             &self.parallel_joint_groups,
         );
@@ -207,6 +234,7 @@ impl ParallelIslandSolver {
             let velocity_solver =
                 std::sync::atomic::AtomicPtr::new(&mut self.velocity_solver as *mut _);
             let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _);
+            let multibodies = std::sync::atomic::AtomicPtr::new(multibodies as *mut _);
             let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _);
             let impulse_joints = std::sync::atomic::AtomicPtr::new(impulse_joints as *mut _);
             let parallel_contact_constraints =
@@ -220,6 +248,8 @@ impl ParallelIslandSolver {
                     unsafe { std::mem::transmute(velocity_solver.load(Ordering::Relaxed)) };
                 let bodies: &mut Bodies =
                     unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
+                let multibodies: &mut MultibodyJointSet =
+                    unsafe { std::mem::transmute(multibodies.load(Ordering::Relaxed)) };
                 let manifolds: &mut Vec<&mut ContactManifold> =
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
                 let impulse_joints: &mut Vec<JointGraphEdge> =
@@ -257,8 +287,10 @@ impl ParallelIslandSolver {
                 }
 
 
+                let mut j_id = 0; // TODO
+                let mut jacobians = DVector::zeros(0); // TODO
                 parallel_contact_constraints.fill_constraints(&thread, params, bodies, manifolds);
-                parallel_joint_constraints.fill_constraints(&thread, params, bodies, impulse_joints);
+                parallel_joint_constraints.fill_constraints(&thread, params, bodies, multibodies, impulse_joints, &mut j_id, &mut jacobians);
                 ThreadContext::lock_until_ge(
                     &thread.num_initialized_constraints,
                     parallel_contact_constraints.constraint_descs.len(),
@@ -271,42 +303,14 @@ impl ParallelIslandSolver {
                 velocity_solver.solve(
                         &thread,
                         params,
+                        island_id,
+                        islands,
+                        bodies,
                         manifolds,
                         impulse_joints,
                         parallel_contact_constraints,
                         parallel_joint_constraints,
                 );
-
-                // Write results back to rigid bodies and integrate velocities.
-                let island_range = islands.active_island_range(island_id);
-                let active_bodies = &islands.active_dynamic_set[island_range];
-
-                concurrent_loop! {
-                    let batch_size = thread.batch_size;
-                    for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
-                        let (rb_ids, rb_pos, rb_vels, rb_damping, rb_mprops): (
-                            &RigidBodyIds,
-                            &RigidBodyPosition,
-                            &RigidBodyVelocity,
-                            &RigidBodyDamping,
-                            &RigidBodyMassProps,
-                        ) = bodies.index_bundle(handle.0);
-
-                        let mut new_rb_pos = *rb_pos;
-                        let mut new_rb_vels = *rb_vels;
-
-                        let dvels = velocity_solver.mj_lambdas[rb_ids.active_set_offset];
-                        new_rb_vels.linvel += dvels.linear;
-                        new_rb_vels.angvel += rb_mprops.effective_world_inv_inertia_sqrt.transform_vector(dvels.angular);
-
-                        let new_rb_vels = new_rb_vels.apply_damping(params.dt, rb_damping);
-                        new_rb_pos.next_position =
-                            new_rb_vels.integrate(params.dt, &rb_pos.position, &rb_mprops.local_mprops.local_com);
-
-                        bodies.set_internal(handle.0, new_rb_vels);
-                        bodies.set_internal(handle.0, new_rb_pos);
-                    }
-                }
             })
         }
     }
diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs
index 3871731..9f96df2 100644
--- a/src/dynamics/solver/parallel_solver_constraints.rs
+++ b/src/dynamics/solver/parallel_solver_constraints.rs
@@ -87,7 +87,7 @@ macro_rules! impl_init_constraints_group {
                 island_id: usize,
                 islands: &IslandManager,
                 bodies: &Bodies,
-                multibody_joints: &MultibodyJointSet,
+                multibodies: &MultibodyJointSet,
                 interactions: &mut [$Interaction],
                 interaction_groups: &ParallelInteractionGroups,
             ) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
@@ -107,7 +107,7 @@ macro_rules! impl_init_constraints_group {
                     self.ground_interactions.clear();
                     $categorize(
                         bodies,
-                        multibody_joints,
+                        multibodies,
                         interactions,
                         group,
                         &mut self.ground_interactions,
@@ -276,7 +276,10 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
         thread: &ThreadContext,
         params: &IntegrationParameters,
         bodies: &Bodies,
+        multibodies: &MultibodyJointSet,
         joints_all: &[JointGraphEdge],
+        j_id: &mut usize,
+        jacobians: &mut DVector<Real>,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -284,9 +287,6 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
             + ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyType>,
     {
-        return;
-
-        /*
         let descs = &self.constraint_descs;
 
         crate::concurrent_loop! {
@@ -295,30 +295,24 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
                 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);
-                        self.velocity_constraints[joint.constraint_index] = velocity_constraint;
+                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, j_id, jacobians, &mut self.velocity_constraints, false);
                     }
                     ConstraintDesc::GroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        let velocity_constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
-                        self.velocity_constraints[joint.constraint_index] = velocity_constraint;
+                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, j_id, jacobians, &mut self.velocity_constraints, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(joint_id) => {
                         let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies);
-                        self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
+                        AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(joint_id) => {
                         let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies);
-                        self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
+                        AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
                     }
                 }
             }
         }
-
-         */
     }
 }
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 00668b1..4a01409 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -1,9 +1,15 @@
 use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext};
-use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
-use crate::dynamics::solver::ParallelSolverConstraints;
-use crate::dynamics::{IntegrationParameters, JointGraphEdge};
+use crate::concurrent_loop;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
+use crate::dynamics::{
+    solver::{GenericVelocityConstraint, ParallelSolverConstraints},
+    IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyDamping, RigidBodyForces,
+    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+};
 use crate::geometry::ContactManifold;
 use crate::math::Real;
+use crate::utils::WAngularInertia;
+
 use na::DVector;
 use std::sync::atomic::Ordering;
 
@@ -20,10 +26,13 @@ impl ParallelVelocitySolver {
         }
     }
 
-    pub fn solve(
+    pub fn solve<Bodies>(
         &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
+        island_id: usize,
+        islands: &IslandManager,
+        bodies: &mut Bodies,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
         contact_constraints: &mut ParallelSolverConstraints<
@@ -31,84 +40,203 @@ impl ParallelVelocitySolver {
             GenericVelocityConstraint,
         >,
         joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
-    ) {
-        if contact_constraints.constraint_descs.is_empty()
-            && joint_constraints.constraint_descs.is_empty()
-        {
-            return;
+    ) where
+        Bodies: ComponentSet<RigidBodyForces>
+            + ComponentSet<RigidBodyIds>
+            + ComponentSet<RigidBodyType>
+            + ComponentSetMut<RigidBodyVelocity>
+            + ComponentSetMut<RigidBodyMassProps>
+            + ComponentSetMut<RigidBodyPosition>
+            + ComponentSet<RigidBodyDamping>,
+    {
+        let mut start_index = thread
+            .solve_interaction_index
+            .fetch_add(thread.batch_size, Ordering::SeqCst);
+        let mut batch_size = thread.batch_size;
+        let contact_descs = &contact_constraints.constraint_descs[..];
+        let joint_descs = &joint_constraints.constraint_descs[..];
+        let mut target_num_desc = 0;
+        let mut shift = 0;
+        let cfm_factor = params.cfm_factor();
+
+        // Each thread will concurrently grab thread.batch_size constraint desc to
+        // solve. If the batch size is large enough to cross the boundary of
+        // a parallel_desc_group, we have to wait util the current group is finished
+        // before starting the next one.
+        macro_rules! solve {
+            ($part: expr, $($solve_args: expr),*) => {
+                for group in $part.parallel_desc_groups.windows(2) {
+                    let num_descs_in_group = group[1] - group[0];
+                    target_num_desc += num_descs_in_group;
+
+                    while start_index < group[1] {
+                        let end_index = (start_index + batch_size).min(group[1]);
+
+                        // TODO: remove the first branch case?
+                        let constraints = if end_index == $part.constraint_descs.len() {
+                            &mut $part.velocity_constraints
+                                [$part.constraint_descs[start_index].0..]
+                        } else {
+                            &mut $part.velocity_constraints
+                                [$part.constraint_descs[start_index].0
+                                ..$part.constraint_descs[end_index].0]
+                        };
+
+                        for constraint in constraints {
+                            constraint.solve(
+                                $($solve_args),*
+                            );
+                        }
+
+                        let num_solved = end_index - start_index;
+                        batch_size -= num_solved;
+
+                        thread
+                            .num_solved_interactions
+                            .fetch_add(num_solved, Ordering::SeqCst);
+
+                        if batch_size == 0 {
+                            start_index = thread
+                                .solve_interaction_index
+                                .fetch_add(thread.batch_size, Ordering::SeqCst);
+                            start_index -= shift;
+                            batch_size = thread.batch_size;
+                        } else {
+                            start_index += num_solved;
+                        }
+                    }
+                    ThreadContext::lock_until_ge(
+                        &thread.num_solved_interactions,
+                        target_num_desc,
+                    );
+                }
+            };
         }
 
         /*
          * Solve constraints.
          */
         {
-            // Each thread will concurrently grab thread.batch_size constraint desc to
-            // solve. If the batch size is large enough for to cross the boundary of
-            // a parallel_desc_group, we have to wait util the current group is finished
-            // before starting the next one.
-            let mut start_index = thread
-                .solve_interaction_index
-                .fetch_add(thread.batch_size, Ordering::SeqCst);
-            let mut batch_size = thread.batch_size;
-            let contact_descs = &contact_constraints.constraint_descs[..];
-            let joint_descs = &joint_constraints.constraint_descs[..];
-            let mut target_num_desc = 0;
-            let mut shift = 0;
-            let cfm_factor = params.cfm_factor();
-
-            for _ in 0..params.max_velocity_iterations {
-                macro_rules! solve {
-                    ($part: expr, $($solve_args: expr),*) => {
-                        // ImpulseJoint groups.
-                        for group in $part.parallel_desc_groups.windows(2) {
-                            let num_descs_in_group = group[1] - group[0];
-
-                            target_num_desc += num_descs_in_group;
-
-                            while start_index < group[1] {
-                                let end_index = (start_index + batch_size).min(group[1]);
-
-                                let constraints = if end_index == $part.constraint_descs.len() {
-                                    &mut $part.velocity_constraints
-                                        [$part.constraint_descs[start_index].0..]
-                                } else {
-                                    &mut $part.velocity_constraints[$part.constraint_descs
-                                        [start_index]
-                                        .0
-                                        ..$part.constraint_descs[end_index].0]
-                                };
-
-                                for constraint in constraints {
-                                    constraint.solve(
-                                        $($solve_args),*
-                                    );
-                                }
-
-                                let num_solved = end_index - start_index;
-                                batch_size -= num_solved;
-
-                                thread
-                                    .num_solved_interactions
-                                    .fetch_add(num_solved, Ordering::SeqCst);
-
-                                if batch_size == 0 {
-                                    start_index = thread
-                                        .solve_interaction_index
-                                        .fetch_add(thread.batch_size, Ordering::SeqCst);
-                                    start_index -= shift;
-                                    batch_size = thread.batch_size;
-                                } else {
-                                    start_index += num_solved;
-                                }
-                            }
-                            ThreadContext::lock_until_ge(
-                                &thread.num_solved_interactions,
-                                target_num_desc,
-                            );
-                        }
-                    };
+            for i in 0..params.max_velocity_iterations {
+                let solve_friction = params.interleave_restitution_and_friction_resolution
+                    && params.max_velocity_friction_iterations + i
+                        >= params.max_velocity_iterations;
+
+                solve!(
+                    joint_constraints,
+                    &joint_constraints.generic_jacobians,
+                    &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas
+                );
+                shift += joint_descs.len();
+                start_index -= joint_descs.len();
+                solve!(
+                    contact_constraints,
+                    cfm_factor,
+                    &mut self.mj_lambdas,
+                    true,
+                    false
+                );
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+
+                if solve_friction {
+                    solve!(
+                        contact_constraints,
+                        cfm_factor,
+                        &mut self.mj_lambdas,
+                        false,
+                        true
+                    );
+                    shift += contact_descs.len();
+                    start_index -= contact_descs.len();
                 }
+            }
+
+            // Solve the remaining friction iterations.
+            let remaining_friction_iterations =
+                if !params.interleave_restitution_and_friction_resolution {
+                    params.max_velocity_friction_iterations
+                } else if params.max_velocity_friction_iterations > params.max_velocity_iterations {
+                    params.max_velocity_friction_iterations - params.max_velocity_iterations
+                } else {
+                    0
+                };
 
+            for _ in 0..remaining_friction_iterations {
+                solve!(
+                    contact_constraints,
+                    cfm_factor,
+                    &mut self.mj_lambdas,
+                    false,
+                    true
+                );
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+            }
+        }
+
+        // Integrate positions.
+        {
+            let island_range = islands.active_island_range(island_id);
+            let active_bodies = &islands.active_dynamic_set[island_range];
+
+            concurrent_loop! {
+                let batch_size = thread.batch_size;
+                for handle in active_bodies[thread.body_integration_pos_index, thread.num_integrated_pos_bodies] {
+                    let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
+                    bodies.index_bundle(handle.0);
+
+                    let dvel = self.mj_lambdas[rb_ids.active_set_offset];
+                    let dangvel = rb_mprops
+                        .effective_world_inv_inertia_sqrt
+                        .transform_vector(dvel.angular);
+
+                    // Update positions.
+                    let (rb_pos, rb_vels, rb_damping, rb_mprops): (
+                        &RigidBodyPosition,
+                        &RigidBodyVelocity,
+                        &RigidBodyDamping,
+                        &RigidBodyMassProps,
+                    ) = bodies.index_bundle(handle.0);
+
+                    let mut new_pos = *rb_pos;
+                    let mut new_vels = *rb_vels;
+                    new_vels.linvel += dvel.linear;
+                    new_vels.angvel += dangvel;
+                    new_vels = new_vels.apply_damping(params.dt, rb_damping);
+                    new_pos.next_position = new_vels.integrate(
+                        params.dt,
+                        &rb_pos.position,
+                        &rb_mprops.local_mprops.local_com,
+                    );
+                    bodies.set_internal(handle.0, new_pos);
+                }
+            }
+        }
+
+        // Remove bias from constraints.
+        {
+            let joint_constraints = &mut joint_constraints.velocity_constraints;
+            let contact_constraints = &mut contact_constraints.velocity_constraints;
+
+            crate::concurrent_loop! {
+                 let batch_size = thread.batch_size;
+                 for constraint in &mut joint_constraints[thread.joint_rm_bias_index] {
+                     constraint.remove_bias_from_rhs();
+                 }
+            }
+            crate::concurrent_loop! {
+                 let batch_size = thread.batch_size;
+                 for constraint in &mut contact_constraints[thread.impulse_rm_bias_index] {
+                     constraint.remove_bias_from_rhs();
+                 }
+            }
+        }
+
+        // Stabiliziton resolution.
+        {
+            for _ in 0..params.max_stabilization_iterations {
                 solve!(
                     joint_constraints,
                     &joint_constraints.generic_jacobians,
@@ -117,11 +245,22 @@ impl ParallelVelocitySolver {
                 );
                 shift += joint_descs.len();
                 start_index -= joint_descs.len();
+
                 solve!(
                     contact_constraints,
                     cfm_factor,
                     &mut self.mj_lambdas,
                     true,
+                    false
+                );
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+
+                solve!(
+                    contact_constraints,
+                    cfm_factor,
+                    &mut self.mj_lambdas,
+                    false,
                     true
                 );
                 shift += contact_descs.len();
@@ -129,6 +268,35 @@ impl ParallelVelocitySolver {
             }
         }
 
+        // Update velocities.
+        {
+            let island_range = islands.active_island_range(island_id);
+            let active_bodies = &islands.active_dynamic_set[island_range];
+
+            concurrent_loop! {
+                let batch_size = thread.batch_size;
+                for handle in active_bodies[thread.body_integration_vel_index, thread.num_integrated_vel_bodies] {
+                    let (rb_ids, rb_damping, rb_mprops): (
+                        &RigidBodyIds,
+                        &RigidBodyDamping,
+                        &RigidBodyMassProps,
+                    ) = bodies.index_bundle(handle.0);
+
+                    let dvel = self.mj_lambdas[rb_ids.active_set_offset];
+                    let dangvel = rb_mprops
+                        .effective_world_inv_inertia_sqrt
+                        .transform_vector(dvel.angular);
+                    let damping = *rb_damping; // To avoid borrow issues.
+
+                    bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
+                        vels.linvel += dvel.linear;
+                        vels.angvel += dangvel;
+                        *vels = vels.apply_damping(params.dt, &damping);
+                    });
+                }
+            }
+        }
+
         /*
          * Writeback impulses.