Complete the parallel solver fix

.vscode/tasks.json

@@ -56,6 +56,21 @@
             ],
             "group": "build"
         },
+        {
+            "label": "run 3d (simd - parallel - debug) ",
+            "type": "shell",
+            "command": "cargo",
+            "args": [
+                "run",
+                "--bin",
+                "all_examples3",
+                "--features",
+                "simd-stable,parallel",
+                "--",
+                "--pause"
+            ],
+            "group": "build"
+        },
         {
             "label": "run 2d (no-simd - release) ",
             "type": "shell",

Cargo.toml

@@ -27,6 +27,9 @@ resolver = "2"
 #opt-level = 1
 #lto = true
 
+[profile.dev]
+opt-level = 1
+
 
 #[profile.dev.package.rapier3d]
 #opt-level = 3

src/dynamics/joint/impulse_joint/impulse_joint.rs

@@ -15,5 +15,4 @@ pub struct ImpulseJoint {
 
     // A joint needs to know its handle to simplify its removal.
     pub(crate) handle: ImpulseJointHandle,
-    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),
-            constraint_index: 0,
         };
 
         let default_id = InteractionGraph::<(), ()>::invalid_graph_index();

src/dynamics/joint/multibody_joint/multibody.rs

@@ -970,6 +970,16 @@ impl Multibody {
             .any(|link| link.joint().num_velocity_constraints() != 0)
     }
 
+    #[inline]
+    pub fn num_active_internal_constraints_and_jacobian_lines(&self) -> (usize, usize) {
+        let num_constraints: usize = self
+            .links
+            .iter()
+            .map(|l| l.joint().num_velocity_constraints())
+            .sum();
+        (num_constraints, num_constraints)
+    }
+
     #[inline]
     pub fn generate_internal_constraints(
         &self,
@@ -977,21 +987,32 @@ impl Multibody {
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
         out: &mut Vec<AnyJointVelocityConstraint>,
+        mut insert_at: Option<usize>,
     ) {
-        let num_constraints: usize = self
+        if !cfg!(feature = "parallel") {
-            .links
+            let num_constraints: usize = self
-            .iter()
+                .links
-            .map(|l| l.joint().num_velocity_constraints())
+                .iter()
-            .sum();
+                .map(|l| l.joint().num_velocity_constraints())
+                .sum();
 
-        let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
+            let required_jacobian_len = *j_id + num_constraints * self.ndofs * 2;
-        if jacobians.nrows() < required_jacobian_len {
+            if jacobians.nrows() < required_jacobian_len {
-            jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+                jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
+            }
         }
 
         for link in self.links.iter() {
-            link.joint()
+            link.joint().velocity_constraints(
-                .velocity_constraints(params, self, link, 0, j_id, jacobians, out);
+                params,
+                self,
+                link,
+                0,
+                j_id,
+                jacobians,
+                out,
+                &mut insert_at,
+            );
         }
     }
 }

src/dynamics/joint/multibody_joint/multibody_joint.rs

@@ -255,6 +255,7 @@ impl MultibodyJoint {
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
         constraints: &mut Vec<AnyJointVelocityConstraint>,
+        insert_at: &mut Option<usize>,
     ) {
         let locked_bits = self.data.locked_axes.bits();
         let limit_bits = self.data.limit_axes.bits();
@@ -281,6 +282,7 @@ impl MultibodyJoint {
                         j_id,
                         jacobians,
                         constraints,
+                        insert_at,
                     );
                 }
 
@@ -295,6 +297,7 @@ impl MultibodyJoint {
                         j_id,
                         jacobians,
                         constraints,
+                        insert_at,
                     );
                 }
                 curr_free_dof += 1;
@@ -329,6 +332,7 @@ impl MultibodyJoint {
                         j_id,
                         jacobians,
                         constraints,
+                        insert_at,
                     );
                     Some(limits)
                 } else {
@@ -347,6 +351,7 @@ impl MultibodyJoint {
                         j_id,
                         jacobians,
                         constraints,
+                        insert_at,
                     );
                 }
                 curr_free_dof += 1;

src/dynamics/joint/multibody_joint/unit_multibody_joint.rs

@@ -20,6 +20,7 @@ pub fn unit_joint_limit_constraint(
     j_id: &mut usize,
     jacobians: &mut DVector<Real>,
     constraints: &mut Vec<AnyJointVelocityConstraint>,
+    insert_at: &mut Option<usize>,
 ) {
     let ndofs = multibody.ndofs();
     let joint_velocity = multibody.joint_velocity(link);
@@ -60,9 +61,14 @@ pub fn unit_joint_limit_constraint(
         writeback_id: WritebackId::Limit(dof_id),
     };
 
-    constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+    if let Some(at) = insert_at {
-        constraint,
+        constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
-    ));
+        *at += 1;
+    } else {
+        constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+            constraint,
+        ));
+    }
     *j_id += 2 * ndofs;
 }
 
@@ -79,6 +85,7 @@ pub fn unit_joint_motor_constraint(
     j_id: &mut usize,
     jacobians: &mut DVector<Real>,
     constraints: &mut Vec<AnyJointVelocityConstraint>,
+    insert_at: &mut Option<usize>,
 ) {
     let inv_dt = params.inv_dt();
     let ndofs = multibody.ndofs();
@@ -128,8 +135,13 @@ pub fn unit_joint_motor_constraint(
         writeback_id: WritebackId::Limit(dof_id),
     };
 
-    constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+    if let Some(at) = insert_at {
-        constraint,
+        constraints[*at] = AnyJointVelocityConstraint::JointGenericGroundConstraint(constraint);
-    ));
+        *at += 1;
+    } else {
+        constraints.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(
+            constraint,
+        ));
+    }
     *j_id += 2 * ndofs;
 }

src/dynamics/solver/generic_velocity_constraint.rs

@@ -36,7 +36,7 @@ impl GenericVelocityConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -99,7 +99,7 @@ impl GenericVelocityConstraint {
         let required_jacobian_len =
             *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
 
-        if jacobians.nrows() < required_jacobian_len {
+        if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
             jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
         }
 
@@ -320,11 +320,10 @@ impl GenericVelocityConstraint {
                 generic_constraint_mask,
             };
 
-            if push {
+            if let Some(at) = insert_at {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
+                out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGeneric(constraint);
             } else {
-                out_constraints[manifold.data.constraint_index + _l] =
+                out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
-                    AnyVelocityConstraint::NongroupedGeneric(constraint);
             }
         }
     }

src/dynamics/solver/generic_velocity_ground_constraint.rs

@@ -34,7 +34,7 @@ impl GenericVelocityGroundConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -87,7 +87,7 @@ impl GenericVelocityGroundConstraint {
         let required_jacobian_len =
             *jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
 
-        if jacobians.nrows() < required_jacobian_len {
+        if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
             jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
         }
 
@@ -200,11 +200,11 @@ impl GenericVelocityGroundConstraint {
                 ndofs2: mb2.ndofs(),
             };
 
-            if push {
+            if let Some(at) = insert_at {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
+                out_constraints[at + _l] =
-            } else {
-                out_constraints[manifold.data.constraint_index + _l] =
                     AnyVelocityConstraint::NongroupedGenericGround(constraint);
+            } else {
+                out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
             }
         }
     }

src/dynamics/solver/interaction_groups.rs

@@ -131,14 +131,14 @@ impl ParallelInteractionGroups {
                 (true, false) => {
                     let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
                     let color_mask = bcolors[rb_ids2.active_set_offset];
-                    *color = (!color_mask).leading_zeros() as usize;
+                    *color = 127 - (!color_mask).leading_zeros() as usize;
                     color_len[*color] += 1;
                     bcolors[rb_ids2.active_set_offset] |= 1 << *color;
                 }
                 (false, true) => {
                     let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
                     let color_mask = bcolors[rb_ids1.active_set_offset];
-                    *color = (!color_mask).leading_zeros() as usize;
+                    *color = 127 - (!color_mask).leading_zeros() as usize;
                     color_len[*color] += 1;
                     bcolors[rb_ids1.active_set_offset] |= 1 << *color;
                 }

src/dynamics/solver/joint_constraint/joint_constraint.rs

@@ -30,20 +30,21 @@ pub enum AnyJointVelocityConstraint {
 
 impl AnyJointVelocityConstraint {
     #[cfg(feature = "parallel")]
-    pub fn num_active_constraints(joint: &ImpulseJoint) -> usize {
+    pub fn num_active_constraints_and_jacobian_lines(joint: &ImpulseJoint) -> (usize, 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
+        let num_constraints = (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
+            + ((limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0) as usize;
+        (num_constraints, num_constraints)
     }
 
     pub fn from_joint<Bodies>(
@@ -55,7 +56,7 @@ impl AnyJointVelocityConstraint {
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
         out: &mut Vec<Self>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -122,7 +123,7 @@ impl AnyJointVelocityConstraint {
             // TODO: is this count correct when we take both motors and limits into account?
             let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
 
-            if jacobians.nrows() < required_jacobian_len {
+            if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
                 jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
             }
 
@@ -143,14 +144,13 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            if push {
+            if let Some(at) = insert_at {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
+                    out[at + i] = AnyJointVelocityConstraint::JointGenericConstraint(c);
                 }
             } else {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out[joint.constraint_index + i] =
+                    out.push(AnyJointVelocityConstraint::JointGenericConstraint(c));
-                        AnyJointVelocityConstraint::JointGenericConstraint(c);
                 }
             }
         } else {
@@ -167,14 +167,13 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            if push {
+            if let Some(at) = insert_at {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out.push(AnyJointVelocityConstraint::JointConstraint(c));
+                    out[at + i] = AnyJointVelocityConstraint::JointConstraint(c);
                 }
             } else {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out[joint.constraint_index + i] =
+                    out.push(AnyJointVelocityConstraint::JointConstraint(c));
-                        AnyJointVelocityConstraint::JointConstraint(c);
                 }
             }
         }
@@ -187,7 +186,7 @@ impl AnyJointVelocityConstraint {
         impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
         bodies: &Bodies,
         out: &mut Vec<Self>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -260,14 +259,13 @@ impl AnyJointVelocityConstraint {
             &mut out_tmp,
         );
 
-        if push {
+        if let Some(at) = insert_at {
-            for c in out_tmp.into_iter().take(out_tmp_len) {
+            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
+                out[at + i] = AnyJointVelocityConstraint::JointConstraintSimd(c);
             }
         } else {
-            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out[impulse_joints[0].constraint_index + i] =
+                out.push(AnyJointVelocityConstraint::JointConstraintSimd(c));
-                    AnyJointVelocityConstraint::JointConstraintSimd(c);
             }
         }
     }
@@ -281,7 +279,7 @@ impl AnyJointVelocityConstraint {
         j_id: &mut usize,
         jacobians: &mut DVector<Real>,
         out: &mut Vec<Self>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyType>
@@ -350,7 +348,7 @@ impl AnyJointVelocityConstraint {
             // TODO: is this count correct when we take both motors and limits into account?
             let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
 
-            if jacobians.nrows() < required_jacobian_len {
+            if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
                 jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
             }
 
@@ -370,14 +368,13 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            if push {
+            if let Some(at) = insert_at {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
+                    out[at + i] = AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
                 }
             } else {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out[joint.constraint_index + i] =
+                    out.push(AnyJointVelocityConstraint::JointGenericGroundConstraint(c));
-                        AnyJointVelocityConstraint::JointGenericGroundConstraint(c);
                 }
             }
         } else {
@@ -394,14 +391,13 @@ impl AnyJointVelocityConstraint {
                 &mut out_tmp,
             );
 
-            if push {
+            if let Some(at) = insert_at {
-                for c in out_tmp.into_iter().take(out_tmp_len) {
+                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                    out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
+                    out[at + i] = AnyJointVelocityConstraint::JointGroundConstraint(c);
                 }
             } else {
-                for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+                for c in out_tmp.into_iter().take(out_tmp_len) {
-                    out[joint.constraint_index + i] =
+                    out.push(AnyJointVelocityConstraint::JointGroundConstraint(c));
-                        AnyJointVelocityConstraint::JointGroundConstraint(c);
                 }
             }
         }
@@ -414,7 +410,7 @@ impl AnyJointVelocityConstraint {
         impulse_joints: [&ImpulseJoint; SIMD_WIDTH],
         bodies: &Bodies,
         out: &mut Vec<Self>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyType>
@@ -506,14 +502,13 @@ impl AnyJointVelocityConstraint {
             &mut out_tmp,
         );
 
-        if push {
+        if let Some(at) = insert_at {
-            for c in out_tmp.into_iter().take(out_tmp_len) {
+            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
-                out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
+                out[at + i] = AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
             }
         } else {
-            for (i, c) in out_tmp.into_iter().take(out_tmp_len).enumerate() {
+            for c in out_tmp.into_iter().take(out_tmp_len) {
-                out[impulse_joints[0].constraint_index + i] =
+                out.push(AnyJointVelocityConstraint::JointGroundConstraintSimd(c));
-                    AnyJointVelocityConstraint::JointGroundConstraintSimd(c);
             }
         }
     }

src/dynamics/solver/parallel_solver_constraints.rs

@@ -8,12 +8,12 @@ use crate::dynamics::solver::{
     VelocityGroundConstraint,
 };
 use crate::dynamics::{
-    ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet,
+    ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
-    RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+    MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
-    RigidBodyVelocity,
+    RigidBodyType, RigidBodyVelocity,
 };
 use crate::geometry::ContactManifold;
-use crate::math::{Real, SPATIAL_DIM};
+use crate::math::{Real, DIM, SPATIAL_DIM};
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
     dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
@@ -46,6 +46,7 @@ pub(crate) enum ConstraintDesc {
     GroundGrouped([usize; SIMD_WIDTH]),
     GenericNongroundNongrouped(usize, usize),
     GenericGroundNongrouped(usize, usize),
+    GenericMultibodyInternal(MultibodyIndex, usize),
 }
 
 pub(crate) struct ParallelSolverConstraints<VelocityConstraint> {
@@ -81,10 +82,10 @@ impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
 macro_rules! impl_init_constraints_group {
     ($VelocityConstraint: ty, $Interaction: ty,
      $categorize: ident, $group: ident,
-     $data: ident$(.$constraint_index: ident)*,
      $body1: ident,
      $body2: ident,
-     $num_active_constraints: path,
+     $generate_internal_constraints: expr,
+     $num_active_constraints_and_jacobian_lines: path,
      $empty_velocity_constraint: expr $(, $weight: ident)*) => {
         impl ParallelSolverConstraints<$VelocityConstraint> {
             pub fn init_constraint_groups<Bodies>(
@@ -151,12 +152,11 @@ macro_rules! impl_init_constraints_group {
                     // Compute constraint indices.
                     for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
-                        interaction.$data$(.$constraint_index)* = total_num_constraints;
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::NongroundNongrouped(*interaction_i),
                         ));
-                        total_num_constraints += $num_active_constraints(interaction);
+                        total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
                     }
 
                     #[cfg(feature = "simd-is-enabled")]
@@ -164,26 +164,24 @@ macro_rules! impl_init_constraints_group {
                         self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH)
                     {
                         let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
-                        interaction.$data$(.$constraint_index)* = total_num_constraints;
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::NongroundGrouped(
                                 gather![|ii| interaction_i[ii]],
                             ),
                         ));
-                        total_num_constraints += $num_active_constraints(interaction);
+                        total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
                     }
 
                     for interaction_i in
                         &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..]
                     {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
-                        interaction.$data$(.$constraint_index)* = total_num_constraints;
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::GroundNongrouped(*interaction_i),
                         ));
-                        total_num_constraints += $num_active_constraints(interaction);
+                        total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
                     }
 
                     #[cfg(feature = "simd-is-enabled")]
@@ -192,14 +190,13 @@ macro_rules! impl_init_constraints_group {
                         .chunks(SIMD_WIDTH)
                     {
                         let interaction = &mut interactions[interaction_i[0]]$(.$weight)*;
-                        interaction.$data$(.$constraint_index)* = total_num_constraints;
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::GroundGrouped(
                                 gather![|ii| interaction_i[ii]],
                             ),
                         ));
-                        total_num_constraints += $num_active_constraints(interaction);
+                        total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
                     }
 
                     let multibody_ndofs = |handle| {
@@ -215,38 +212,62 @@ macro_rules! impl_init_constraints_group {
 
                     for interaction_i in &self.generic_not_ground_interactions[..] {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
-                        interaction.$data$(.$constraint_index)* = total_num_constraints;
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::GenericNongroundNongrouped(*interaction_i, *j_id),
                         ));
-                        let num_constraints = $num_active_constraints(interaction);
+                        let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
                         let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0);
                         let ndofs2 = $body2(interaction).map(multibody_ndofs).unwrap_or(0);
 
-                        *j_id += num_constraints * (ndofs1 + ndofs2) * 2;
+                        *j_id += (ndofs1 + ndofs2) * 2 * num_jac_lines;
                         total_num_constraints += num_constraints;
                     }
 
                     for interaction_i in &self.generic_ground_interactions[..] {
                         let interaction = &mut interactions[*interaction_i]$(.$weight)*;
-                        interaction.$data$(.$constraint_index)* = total_num_constraints;
                         self.constraint_descs.push((
                             total_num_constraints,
                             ConstraintDesc::GenericGroundNongrouped(*interaction_i, *j_id),
                         ));
 
-                        let num_constraints = $num_active_constraints(interaction);
+                        let (num_constraints, num_jac_lines) = $num_active_constraints_and_jacobian_lines(interaction);
                         let ndofs1 = $body1(interaction).map(multibody_ndofs).unwrap_or(0);
                         let ndofs2 = $body2(interaction).map(multibody_ndofs).unwrap_or(0);
 
-                        *j_id += num_constraints * (ndofs1 + ndofs2) * 2;
+                        *j_id += (ndofs1 + ndofs2) * 2 * num_jac_lines;
                         total_num_constraints += num_constraints;
                     }
 
                     self.parallel_desc_groups.push(self.constraint_descs.len());
                 }
 
+                if $generate_internal_constraints {
+                    let mut had_any_internal_constraint = false;
+                    for handle in islands.active_island(island_id) {
+                        if let Some(link) = multibodies.rigid_body_link(*handle) {
+                            let multibody = multibodies.get_multibody(link.multibody).unwrap();
+                            if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                                let (num_constraints, num_jac_lines) = multibody.num_active_internal_constraints_and_jacobian_lines();
+                                let ndofs = multibody.ndofs();
+
+                                self.constraint_descs.push((
+                                    total_num_constraints,
+                                    ConstraintDesc::GenericMultibodyInternal(link.multibody, *j_id)
+                                ));
+
+                                *j_id += ndofs * 2 * num_jac_lines;
+                                total_num_constraints += num_constraints;
+                                had_any_internal_constraint = true;
+                            }
+                        }
+                    }
+
+                    if had_any_internal_constraint {
+                        self.parallel_desc_groups.push(self.constraint_descs.len());
+                    }
+                }
+
                 // Resize the constraint sets.
                 self.velocity_constraints.clear();
                 self.velocity_constraints
@@ -274,10 +295,10 @@ impl_init_constraints_group!(
     &mut ContactManifold,
     categorize_contacts,
     group_manifolds,
-    data.constraint_index,
     manifold_body1,
     manifold_body2,
-    VelocityConstraint::num_active_constraints,
+    false,
+    VelocityConstraint::num_active_constraints_and_jacobian_lines,
     AnyVelocityConstraint::Empty
 );
 
@@ -286,10 +307,10 @@ impl_init_constraints_group!(
     JointGraphEdge,
     categorize_joints,
     group_joints,
-    constraint_index,
     joint_body1,
     joint_body2,
-    AnyJointVelocityConstraint::num_active_constraints,
+    true,
+    AnyJointVelocityConstraint::num_active_constraints_and_jacobian_lines,
     AnyJointVelocityConstraint::Empty,
     weight
 );
@@ -317,32 +338,33 @@ impl ParallelSolverConstraints<AnyVelocityConstraint> {
                 match &desc.1 {
                     ConstraintDesc::NongroundNongrouped(manifold_id) => {
                         let manifold = &*manifolds_all[*manifold_id];
-                        VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
+                        VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     ConstraintDesc::GroundNongrouped(manifold_id) => {
                         let manifold = &*manifolds_all[*manifold_id];
-                        VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, false);
+                        VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(manifold_id) => {
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
-                        WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
+                        WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(manifold_id) => {
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
-                        WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
+                        WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     ConstraintDesc::GenericNongroundNongrouped(manifold_id, j_id) => {
                         let mut j_id = *j_id;
                         let manifold = &*manifolds_all[*manifold_id];
-                        GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies,  &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, false);
+                        GenericVelocityConstraint::generate(params, *manifold_id, manifold, bodies, multibodies,  &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
                     }
                     ConstraintDesc::GenericGroundNongrouped(manifold_id, j_id) => {
                         let mut j_id = *j_id;
                         let manifold = &*manifolds_all[*manifold_id];
-                        GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, false);
+                        GenericVelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, multibodies, &mut self.velocity_constraints, &mut self.generic_jacobians, &mut j_id, Some(desc.0));
                     }
+                    ConstraintDesc::GenericMultibodyInternal(..) => unreachable!()
                 }
             }
         }
@@ -372,31 +394,36 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
                 match &desc.1 {
                     ConstraintDesc::NongroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
+                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
                     ConstraintDesc::GroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
+                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut 0, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(joint_id) => {
                         let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
+                        AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::GroundGrouped(joint_id) => {
                         let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
-                        AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, false);
+                        AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
                     }
                     ConstraintDesc::GenericNongroundNongrouped(joint_id, j_id) => {
                         let mut j_id = *j_id;
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
+                        AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
                     ConstraintDesc::GenericGroundNongrouped(joint_id, j_id) => {
                         let mut j_id = *j_id;
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, false);
+                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
+                    }
+                    ConstraintDesc::GenericMultibodyInternal(multibody_id, j_id) => {
+                        let mut j_id = *j_id;
+                        let multibody = multibodies.get_multibody(*multibody_id).unwrap();
+                        multibody.generate_internal_constraints(params, &mut j_id, &mut self.generic_jacobians, &mut self.velocity_constraints, Some(desc.0));
                     }
                 }
             }

src/dynamics/solver/solver_constraints.rs

@@ -189,7 +189,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
                 manifolds,
                 bodies,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -213,7 +213,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
                 manifold,
                 bodies,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -243,7 +243,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
                 &mut self.velocity_constraints,
                 &mut self.generic_jacobians,
                 jacobian_id,
-                true,
+                None,
             );
         }
     }
@@ -273,7 +273,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
                 &mut self.velocity_constraints,
                 &mut self.generic_jacobians,
                 jacobian_id,
-                true,
+                None,
             );
         }
     }
@@ -303,7 +303,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
                 manifolds,
                 bodies,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -327,7 +327,7 @@ impl SolverConstraints<AnyVelocityConstraint> {
                 manifold,
                 bodies,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -457,6 +457,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                         j_id,
                         &mut self.generic_jacobians,
                         &mut self.velocity_constraints,
+                        None,
                     )
                 }
             }
@@ -488,7 +489,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                 &mut j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -519,7 +520,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                 impulse_joints,
                 bodies,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -548,7 +549,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                 j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }
@@ -577,7 +578,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                 j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
-                true,
+                None,
             )
         }
     }
@@ -607,7 +608,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                 j_id,
                 &mut self.generic_jacobians,
                 &mut self.velocity_constraints,
-                true,
+                None,
             )
         }
     }
@@ -637,7 +638,7 @@ impl SolverConstraints<AnyJointVelocityConstraint> {
                 impulse_joints,
                 bodies,
                 &mut self.velocity_constraints,
-                true,
+                None,
             );
         }
     }

src/dynamics/solver/velocity_constraint.rs

@@ -136,9 +136,12 @@ pub(crate) struct VelocityConstraint {
 
 impl VelocityConstraint {
     #[cfg(feature = "parallel")]
-    pub fn num_active_constraints(manifold: &ContactManifold) -> usize {
+    pub fn num_active_constraints_and_jacobian_lines(manifold: &ContactManifold) -> (usize, usize) {
         let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0;
-        manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
+        (
+            manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize,
+            manifold.data.solver_contacts.len() * DIM,
+        )
     }
 
     pub fn generate<Bodies>(
@@ -147,7 +150,7 @@ impl VelocityConstraint {
         manifold: &ContactManifold,
         bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -209,7 +212,7 @@ impl VelocityConstraint {
             // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
             // for the moment.
             #[cfg(target_arch = "wasm32")]
-            let constraint = if push {
+            let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
                     out_constraints.resize_with(new_len, || {
@@ -331,11 +334,10 @@ impl VelocityConstraint {
             }
 
             #[cfg(not(target_arch = "wasm32"))]
-            if push {
+            if let Some(at) = insert_at {
-                out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
+                out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint);
             } else {
-                out_constraints[manifold.data.constraint_index + _l] =
+                out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
-                    AnyVelocityConstraint::Nongrouped(constraint);
             }
         }
     }

src/dynamics/solver/velocity_constraint_wide.rs

@@ -36,7 +36,7 @@ impl WVelocityConstraint {
         manifolds: [&ContactManifold; SIMD_WIDTH],
         bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -190,11 +190,11 @@ impl WVelocityConstraint {
                 }
             }
 
-            if push {
+            if let Some(at) = insert_at {
-                out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
+                out_constraints[at + l / MAX_MANIFOLD_POINTS] =
-            } else {
-                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::Grouped(constraint);
+            } else {
+                out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
             }
         }
     }

src/dynamics/solver/velocity_ground_constraint.rs

@@ -33,7 +33,7 @@ impl VelocityGroundConstraint {
         manifold: &ContactManifold,
         bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -104,7 +104,7 @@ impl VelocityGroundConstraint {
             // NOTE: impulse_joints have the same problem, but it is not easy to refactor the code that way
             // for the moment.
             #[cfg(target_arch = "wasm32")]
-            let constraint = if push {
+            let constraint = if insert_at.is_none() {
                 let new_len = out_constraints.len() + 1;
                 unsafe {
                     out_constraints.resize_with(new_len, || {
@@ -212,11 +212,10 @@ impl VelocityGroundConstraint {
             }
 
             #[cfg(not(target_arch = "wasm32"))]
-            if push {
+            if let Some(at) = insert_at {
-                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
+                out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint);
             } else {
-                out_constraints[manifold.data.constraint_index + _l] =
+                out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
-                    AnyVelocityConstraint::NongroupedGround(constraint);
             }
         }
     }

src/dynamics/solver/velocity_ground_constraint_wide.rs

@@ -35,7 +35,7 @@ impl WVelocityGroundConstraint {
         manifolds: [&ContactManifold; SIMD_WIDTH],
         bodies: &Bodies,
         out_constraints: &mut Vec<AnyVelocityConstraint>,
-        push: bool,
+        insert_at: Option<usize>,
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyVelocity>
@@ -188,11 +188,11 @@ impl WVelocityGroundConstraint {
                 }
             }
 
-            if push {
+            if let Some(at) = insert_at {
-                out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
+                out_constraints[at + l / MAX_MANIFOLD_POINTS] =
-            } else {
-                out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::GroupedGround(constraint);
+            } else {
+                out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
             }
         }
     }

src/geometry/contact_pair.rs

@@ -122,9 +122,6 @@ pub struct ContactManifoldData {
     pub rigid_body1: Option<RigidBodyHandle>,
     /// The second rigid-body involved in this contact manifold.
     pub rigid_body2: Option<RigidBodyHandle>,
-    // The two following are set by the constraints solver.
-    #[cfg_attr(feature = "serde-serialize", serde(skip))]
-    pub(crate) constraint_index: usize,
     // We put the following fields here to avoids reading the colliders inside of the
     // contact preparation method.
     /// Flags used to control some aspects of the constraints solver for this contact manifold.
@@ -211,7 +208,6 @@ impl ContactManifoldData {
         Self {
             rigid_body1,
             rigid_body2,
-            constraint_index: 0,
             solver_flags,
             normal: Vector::zeros(),
             solver_contacts: Vec::new(),