Start fixing the parallel version.

examples3d/articulations3.rs

@@ -1,6 +1,35 @@
 use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
 
+fn create_coupled_joints(
+    bodies: &mut RigidBodySet,
+    colliders: &mut ColliderSet,
+    impulse_joints: &mut ImpulseJointSet,
+    multibody_joints: &mut MultibodyJointSet,
+    origin: Point<f32>,
+    use_articulations: bool,
+) {
+    let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
+    let body1 = bodies.insert(
+        RigidBodyBuilder::new_dynamic()
+            .translation(origin.coords)
+            .linvel(vector![5.0, 5.0, 5.0]),
+    );
+    colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), body1, bodies);
+
+    let joint1 = GenericJointBuilder::new(JointAxesMask::empty())
+        .limits(JointAxis::X, [-3.0, 3.0])
+        .limits(JointAxis::Y, [0.0, 3.0])
+        .limits(JointAxis::Z, [0.0, 3.0])
+        .coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
+
+    if use_articulations {
+        multibody_joints.insert(ground, body1, joint1);
+    } else {
+        impulse_joints.insert(ground, body1, joint1);
+    }
+}
+
 fn create_prismatic_joints(
     bodies: &mut RigidBodySet,
     colliders: &mut ColliderSet,
@@ -450,7 +479,7 @@ fn create_actuated_revolute_joints(
             if i == 1 {
                 joint = joint
                     .local_anchor2(point![0.0, 2.0, -shift])
-                    .motor_velocity(-2.0, 100.0);
+                    .motor_velocity(-2.0, 1000.0);
             }
 
             if use_articulations {
@@ -617,6 +646,14 @@ fn do_init_world(testbed: &mut Testbed, use_articulations: bool) {
         point![-5.0, 0.0, 0.0],
         use_articulations,
     );
+    create_coupled_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut impulse_joints,
+        &mut multibody_joints,
+        point![0.0, 20.0, 0.0],
+        use_articulations,
+    );
 
     /*
      * Set up the testbed.

src/dynamics/joint/generic_joint.rs

@@ -26,6 +26,8 @@ bitflags::bitflags! {
         const FREE_PRISMATIC_AXES = Self::X.bits;
         const FREE_FIXED_AXES = 0;
         const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits;
+        const LIN_AXES = Self::X.bits() | Self::Y.bits() | Self::Z.bits();
+        const ANG_AXES = Self::ANG_X.bits() | Self::ANG_Y.bits() | Self::ANG_Z.bits();
     }
 }
 
@@ -42,6 +44,8 @@ bitflags::bitflags! {
         const FREE_REVOLUTE_AXES = Self::ANG_X.bits;
         const FREE_PRISMATIC_AXES = Self::X.bits;
         const FREE_FIXED_AXES = 0;
+        const LIN_AXES = Self::X.bits() | Self::Y.bits();
+        const ANG_AXES = Self::ANG_X.bits();
     }
 }
 
@@ -104,7 +108,7 @@ impl Default for JointMotor {
             damping: 0.0,
             max_force: Real::MAX,
             impulse: 0.0,
-            model: MotorModel::AccelerationBased, // VelocityBased,
+            model: MotorModel::AccelerationBased,
         }
     }
 }
@@ -134,6 +138,7 @@ pub struct GenericJoint {
     pub locked_axes: JointAxesMask,
     pub limit_axes: JointAxesMask,
     pub motor_axes: JointAxesMask,
+    pub coupled_axes: JointAxesMask,
     pub limits: [JointLimits<Real>; SPATIAL_DIM],
     pub motors: [JointMotor; SPATIAL_DIM],
 }
@@ -146,6 +151,7 @@ impl Default for GenericJoint {
             locked_axes: JointAxesMask::empty(),
             limit_axes: JointAxesMask::empty(),
             motor_axes: JointAxesMask::empty(),
+            coupled_axes: JointAxesMask::empty(),
             limits: [JointLimits::default(); SPATIAL_DIM],
             motors: [JointMotor::default(); SPATIAL_DIM],
         }
@@ -445,6 +451,12 @@ impl GenericJointBuilder {
         self
     }
 
+    #[must_use]
+    pub fn coupled_axes(mut self, axes: JointAxesMask) -> Self {
+        self.0.coupled_axes = axes;
+        self
+    }
+
     /// Set the spring-like model used by the motor to reach the desired target velocity and position.
     #[must_use]
     pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
@@ -499,3 +511,9 @@ impl GenericJointBuilder {
         self.0
     }
 }
+
+impl Into<GenericJoint> for GenericJointBuilder {
+    fn into(self) -> GenericJoint {
+        self.0
+    }
+}

src/dynamics/joint/impulse_joint/impulse_joint.rs

@@ -15,6 +15,5 @@ pub struct ImpulseJoint {
 
     // A joint needs to know its handle to simplify its removal.
     pub(crate) handle: ImpulseJointHandle,
-    #[cfg(feature = "parallel")]
     pub(crate) constraint_index: usize,
 }

src/dynamics/joint/impulse_joint/impulse_joint_set.rs

@@ -187,7 +187,6 @@ impl ImpulseJointSet {
             data,
             impulses: na::zero(),
             handle: ImpulseJointHandle(handle),
-            #[cfg(feature = "parallel")]
             constraint_index: 0,
         };
 

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 {
+    pub fn num_active_constraints(joint: &ImpulseJoint) -> usize {
-        1
+        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) {
+            if push {
-                out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
+                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) {
+            if push {
-                out.push(AnyJointVelocityConstraint::JointConstraint(c));
+                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) {
+        if push {
-            out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
+            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) {
+            if push {
-                out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
+                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) {
+            if push {
-                out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
+                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) {
+        if push {
-            out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
+            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);
+            }
         }
     }
 

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),
+            body_integration_pos_index: AtomicUsize::new(0),
-            num_integrated_bodies: 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);
-                    }
-                }
             })
         }
     }

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);
+                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, j_id, jacobians, &mut self.velocity_constraints, false);
-                        self.velocity_constraints[joint.constraint_index] = velocity_constraint;
                     }
                     ConstraintDesc::GroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        let velocity_constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies);
+                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, j_id, jacobians, &mut self.velocity_constraints, false);
-                        self.velocity_constraints[joint.constraint_index] = velocity_constraint;
                     }
                     #[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);
+                        AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
-                        self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
                     }
                     #[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);
+                        AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
-                        self.velocity_constraints[impulse_joints[0].constraint_index] = velocity_constraint;
                     }
                 }
             }
         }
-
-         */
     }
 }

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::concurrent_loop;
-use crate::dynamics::solver::ParallelSolverConstraints;
+use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
-use crate::dynamics::{IntegrationParameters, JointGraphEdge};
+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,83 +40,87 @@ impl ParallelVelocitySolver {
             GenericVelocityConstraint,
         >,
         joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
-    ) {
+    ) where
-        if contact_constraints.constraint_descs.is_empty()
+        Bodies: ComponentSet<RigidBodyForces>
-            && joint_constraints.constraint_descs.is_empty()
+            + ComponentSet<RigidBodyIds>
-        {
+            + ComponentSet<RigidBodyType>
-            return;
+            + 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
+            for i in 0..params.max_velocity_iterations {
-            // solve. If the batch size is large enough for to cross the boundary of
+                let solve_friction = params.interleave_restitution_and_friction_resolution
-            // a parallel_desc_group, we have to wait util the current group is finished
+                    && params.max_velocity_friction_iterations + i
-            // before starting the next one.
+                        >= params.max_velocity_iterations;
-            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,
-                            );
-                        }
-                    };
-                }
 
                 solve!(
                     joint_constraints,
@@ -122,6 +135,40 @@ impl ParallelVelocitySolver {
                     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();
@@ -129,6 +176,127 @@ impl ParallelVelocitySolver {
             }
         }
 
+        // 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,
+                    &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();
+
+                solve!(
+                    contact_constraints,
+                    cfm_factor,
+                    &mut self.mj_lambdas,
+                    false,
+                    true
+                );
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+            }
+        }
+
+        // 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.
          */

src/dynamics/solver/solver_constraints.rs

@@ -496,6 +496,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
                 &mut j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
+                true,
             );
         }
     }
@@ -526,6 +527,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
                 impulse_joints,
                 bodies,
                 &mut self.velocity_constraints,
+                true,
             );
         }
     }
@@ -554,6 +556,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
                 j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
+                true,
             );
         }
     }
@@ -582,6 +585,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
                 j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
+                true,
             )
         }
     }
@@ -611,6 +615,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
                 j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
+                true,
             )
         }
     }
@@ -640,6 +645,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
                 impulse_joints,
                 bodies,
                 &mut self.velocity_constraints,
+                true,
             );
         }
     }

src/dynamics/solver/velocity_solver.rs

@@ -3,13 +3,12 @@ use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::AnyGenericVelocityConstraint;
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
-    IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType,
+    IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
+    RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
     RigidBodyVelocity,
 };
-use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
 use crate::geometry::ContactManifold;
 use crate::math::Real;
-use crate::prelude::{RigidBodyDamping, RigidBodyPosition};
 use crate::utils::WAngularInertia;
 use na::DVector;
 
@@ -151,7 +150,7 @@ impl VelocitySolver {
             }
         }
 
-        // Update positions.
+        // Integrate positions.
         for handle in islands.active_island(island_id) {
             if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
                 let multibody = multibodies
@@ -169,30 +168,33 @@ impl VelocitySolver {
                     multibody.velocities = prev_vels;
                 }
             } else {
-                let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) =
+                let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
                     bodies.index_bundle(handle.0);
 
-                let dvel = self.mj_lambdas[ids.active_set_offset];
+                let dvel = self.mj_lambdas[rb_ids.active_set_offset];
-                let dangvel = mprops
+                let dangvel = rb_mprops
                     .effective_world_inv_inertia_sqrt
                     .transform_vector(dvel.angular);
 
                 // Update positions.
-                let (poss, vels, damping, mprops): (
+                let (rb_pos, rb_vels, rb_damping, rb_mprops): (
                     &RigidBodyPosition,
                     &RigidBodyVelocity,
                     &RigidBodyDamping,
                     &RigidBodyMassProps,
                 ) = bodies.index_bundle(handle.0);
 
-                let mut new_poss = *poss;
+                let mut new_pos = *rb_pos;
-                let mut new_vels = *vels;
+                let mut new_vels = *rb_vels;
                 new_vels.linvel += dvel.linear;
                 new_vels.angvel += dangvel;
-                new_vels = new_vels.apply_damping(params.dt, damping);
+                new_vels = new_vels.apply_damping(params.dt, rb_damping);
-                new_poss.next_position =
+                new_pos.next_position = new_vels.integrate(
-                    new_vels.integrate(params.dt, &poss.position, &mprops.local_mprops.local_com);
+                    params.dt,
-                bodies.set_internal(handle.0, new_poss);
+                    &rb_pos.position,
+                    &rb_mprops.local_mprops.local_com,
+                );
+                bodies.set_internal(handle.0, new_pos);
             }
         }
 
@@ -260,17 +262,17 @@ impl VelocitySolver {
                     multibody.velocities += mj_lambdas;
                 }
             } else {
-                let (ids, damping, mprops): (
+                let (rb_ids, rb_damping, rb_mprops): (
                     &RigidBodyIds,
                     &RigidBodyDamping,
                     &RigidBodyMassProps,
                 ) = bodies.index_bundle(handle.0);
 
-                let dvel = self.mj_lambdas[ids.active_set_offset];
+                let dvel = self.mj_lambdas[rb_ids.active_set_offset];
-                let dangvel = mprops
+                let dangvel = rb_mprops
                     .effective_world_inv_inertia_sqrt
                     .transform_vector(dvel.angular);
-                let damping = *damping; // To avoid borrow issues.
+                let damping = *rb_damping; // To avoid borrow issues.
 
                 bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
                     vels.linvel += dvel.linear;

src_testbed/physx_backend.rs

@@ -311,7 +311,6 @@ impl PhysxWorld {
                                 );
                             }
                         } else if !parent_body.is_dynamic() {
-                            println!("Ground collider");
                             let actor = rapier2static.get_mut(&parent_handle).unwrap();
                             actor.attach_shape(&mut px_shape);
                         } else {