Second round to fix the parallel solver.

10 files changed, 397 insertions, 235 deletions

diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index a304008..99dc40e 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -8,65 +8,14 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::{WAngularInertia, WCross, WDot};
 
-use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
-use crate::dynamics::solver::GenericVelocityGroundConstraint;
+use super::{
+    AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
+};
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use na::DVector;
 
 #[derive(Copy, Clone, Debug)]
-pub(crate) enum AnyGenericVelocityConstraint {
-    NongroupedGround(GenericVelocityGroundConstraint),
-    Nongrouped(GenericVelocityConstraint),
-}
-
-impl AnyGenericVelocityConstraint {
-    pub fn solve(
-        &mut self,
-        cfm_factor: Real,
-        jacobians: &DVector<Real>,
-        mj_lambdas: &mut [DeltaVel<Real>],
-        generic_mj_lambdas: &mut DVector<Real>,
-        solve_restitution: bool,
-        solve_friction: bool,
-    ) {
-        match self {
-            AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
-                cfm_factor,
-                jacobians,
-                mj_lambdas,
-                generic_mj_lambdas,
-                solve_restitution,
-                solve_friction,
-            ),
-            AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
-                cfm_factor,
-                jacobians,
-                generic_mj_lambdas,
-                solve_restitution,
-                solve_friction,
-            ),
-        }
-    }
-
-    pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
-        match self {
-            AnyGenericVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifolds_all),
-            AnyGenericVelocityConstraint::NongroupedGround(c) => {
-                c.writeback_impulses(manifolds_all)
-            }
-        }
-    }
-
-    pub fn remove_bias_from_rhs(&mut self) {
-        match self {
-            AnyGenericVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
-            AnyGenericVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
-        }
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
 pub(crate) struct GenericVelocityConstraint {
     // We just build the generic constraint on top of the velocity constraint,
     // adding some information we can use in the generic case.
@@ -84,7 +33,7 @@ impl GenericVelocityConstraint {
         manifold: &ContactManifold,
         bodies: &Bodies,
         multibodies: &MultibodyJointSet,
-        out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
         push: bool,
@@ -372,10 +321,10 @@ impl GenericVelocityConstraint {
             };
 
             if push {
-                out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint));
+                out_constraints.push(AnyVelocityConstraint::NongroupedGeneric(constraint));
             } else {
                 out_constraints[manifold.data.constraint_index + _l] =
-                    AnyGenericVelocityConstraint::Nongrouped(constraint);
+                    AnyVelocityConstraint::NongroupedGeneric(constraint);
             }
         }
     }
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
index 336954b..b2e5878 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs
@@ -8,8 +8,9 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::WCross;
 
-use super::{VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart};
-use crate::dynamics::solver::AnyGenericVelocityConstraint;
+use super::{
+    AnyVelocityConstraint, VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
+};
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use na::DVector;
@@ -30,7 +31,7 @@ impl GenericVelocityGroundConstraint {
         manifold: &ContactManifold,
         bodies: &Bodies,
         multibodies: &MultibodyJointSet,
-        out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
+        out_constraints: &mut Vec<AnyVelocityConstraint>,
         jacobians: &mut DVector<Real>,
         jacobian_id: &mut usize,
         push: bool,
@@ -145,7 +146,7 @@ impl GenericVelocityGroundConstraint {
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
-                    rhs_wo_bias *= is_bouncy + is_resting ;
+                    rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
 
@@ -200,10 +201,10 @@ impl GenericVelocityGroundConstraint {
             };
 
             if push {
-                out_constraints.push(AnyGenericVelocityConstraint::NongroupedGround(constraint));
+                out_constraints.push(AnyVelocityConstraint::NongroupedGenericGround(constraint));
             } else {
                 out_constraints[manifold.data.constraint_index + _l] =
-                    AnyGenericVelocityConstraint::NongroupedGround(constraint);
+                    AnyVelocityConstraint::NongroupedGenericGround(constraint);
             }
         }
     }
diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs
index 67cc805..4adbc18 100644
--- a/src/dynamics/solver/interaction_groups.rs
+++ b/src/dynamics/solver/interaction_groups.rs
@@ -1,7 +1,7 @@
 use crate::data::ComponentSet;
 #[cfg(feature = "parallel")]
 use crate::dynamics::RigidBodyHandle;
-use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, RigidBodyIds};
+use crate::dynamics::{IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet, RigidBodyIds};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 #[cfg(feature = "simd-is-enabled")]
 use {
@@ -64,6 +64,7 @@ impl ParallelInteractionGroups {
         island_id: usize,
         islands: &IslandManager,
         bodies: &Bodies,
+        multibodies: &MultibodyJointSet,
         interactions: &[Interaction],
         interaction_indices: &[usize],
     ) where
@@ -88,7 +89,7 @@ impl ParallelInteractionGroups {
             .iter()
             .zip(self.interaction_colors.iter_mut())
         {
-            let body_pair = interactions[*interaction_id].body_pair();
+            let mut body_pair = interactions[*interaction_id].body_pair();
             let is_static1 = body_pair
                 .0
                 .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
@@ -98,6 +99,24 @@ impl ParallelInteractionGroups {
                 .map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_static())
                 .unwrap_or(true);
 
+            let representative = |handle: RigidBodyHandle| {
+                if let Some(link) = multibodies.rigid_body_link(handle).copied() {
+                    let multibody = multibodies.get_multibody(link.multibody).unwrap();
+                    multibody
+                        .link(1) // Use the link 1 to cover the case where the multibody root is static.
+                        .or(multibody.link(0)) // TODO: Never happens?
+                        .map(|l| l.rigid_body)
+                        .unwrap()
+                } else {
+                    handle
+                }
+            };
+
+            body_pair = (
+                body_pair.0.map(representative),
+                body_pair.1.map(representative),
+            );
+
             match (is_static1, is_static2) {
                 (false, false) => {
                     let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
@@ -112,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).trailing_zeros() as usize;
+                    *color = (!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).trailing_zeros() as usize;
+                    *color = (!color_mask).leading_zeros() as usize;
                     color_len[*color] += 1;
                     bcolors[rb_ids1.active_set_offset] |= 1 << *color;
                 }
@@ -131,13 +150,11 @@ impl ParallelInteractionGroups {
         let mut last_offset = 0;
 
         for i in 0..128 {
-            if color_len[i] == 0 {
-                break;
+            if color_len[i] != 0 {
+                self.groups.push(last_offset);
+                sort_offsets[i] = last_offset;
+                last_offset += color_len[i];
             }
-
-            self.groups.push(last_offset);
-            sort_offsets[i] = last_offset;
-            last_offset += color_len[i];
         }
 
         self.sorted_interactions
diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs
index 36c42bd..90c8d90 100644
--- a/src/dynamics/solver/island_solver.rs
+++ b/src/dynamics/solver/island_solver.rs
@@ -2,8 +2,7 @@ use super::VelocitySolver;
 use crate::counters::Counters;
 use crate::data::{ComponentSet, ComponentSetMut};
 use crate::dynamics::solver::{
-    AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
-    SolverConstraints,
+    AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
 };
 use crate::dynamics::{
     IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
@@ -14,8 +13,8 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::prelude::MultibodyJointSet;
 
 pub struct IslandSolver {
-    contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
-    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
+    contact_constraints: SolverConstraints<AnyVelocityConstraint>,
+    joint_constraints: SolverConstraints<AnyJointVelocityConstraint>,
     velocity_solver: VelocitySolver,
 }
 
@@ -94,7 +93,6 @@ impl IslandSolver {
             manifolds,
             impulse_joints,
             &mut self.contact_constraints.velocity_constraints,
-            &mut self.contact_constraints.generic_velocity_constraints,
             &self.contact_constraints.generic_jacobians,
             &mut self.joint_constraints.velocity_constraints,
             &self.joint_constraints.generic_jacobians,
diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs
index 96dc403..463845b 100644
--- a/src/dynamics/solver/parallel_island_solver.rs
+++ b/src/dynamics/solver/parallel_island_solver.rs
@@ -142,9 +142,8 @@ pub struct ParallelIslandSolver {
     positions: Vec<Isometry<Real>>,
     parallel_groups: ParallelInteractionGroups,
     parallel_joint_groups: ParallelInteractionGroups,
-    parallel_contact_constraints:
-        ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
-    parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
+    parallel_contact_constraints: ParallelSolverConstraints<AnyVelocityConstraint>,
+    parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint>,
     thread: ThreadContext,
 }
 
@@ -191,10 +190,14 @@ impl ParallelIslandSolver {
         let num_threads = rayon::current_num_threads();
         let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
         self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
+    
+        // Interactions grouping.
+        let mut j_id = 0;
         self.parallel_groups.group_interactions(
             island_id,
             islands,
             bodies,
+            multibodies,
             manifolds,
             manifold_indices,
         );
@@ -202,9 +205,12 @@ impl ParallelIslandSolver {
             island_id,
             islands,
             bodies,
+            multibodies,
             impulse_joints,
             joint_indices,
         );
+
+        let mut contact_j_id = 0;
         self.parallel_contact_constraints.init_constraint_groups(
             island_id,
             islands,
@@ -212,7 +218,9 @@ impl ParallelIslandSolver {
             multibodies,
             manifolds,
             &self.parallel_groups,
+            &mut contact_j_id,
         );
+        let mut joint_j_id = 0;
         self.parallel_joint_constraints.init_constraint_groups(
             island_id,
             islands,
@@ -220,12 +228,49 @@ impl ParallelIslandSolver {
             multibodies,
             impulse_joints,
             &self.parallel_joint_groups,
+            &mut joint_j_id,
         );
 
-        self.velocity_solver.mj_lambdas.clear();
-        self.velocity_solver
-            .mj_lambdas
-            .resize(islands.active_island(island_id).len(), DeltaVel::zero());
+        if self.parallel_contact_constraints.generic_jacobians.len() < contact_j_id {
+            self.parallel_contact_constraints.generic_jacobians = DVector::zeros(contact_j_id);
+        } else {
+            self.parallel_contact_constraints.generic_jacobians.fill(0.0);
+        }
+
+        if self.parallel_joint_constraints.generic_jacobians.len() < joint_j_id {
+            self.parallel_joint_constraints.generic_jacobians = DVector::zeros(joint_j_id);
+        } else {
+            self.parallel_joint_constraints.generic_jacobians.fill(0.0);
+        }
+
+        // Init solver ids for multibodies.
+        {
+            let mut solver_id = 0;
+            let island_range = islands.active_island_range(island_id);
+            let active_bodies = &islands.active_dynamic_set[island_range];
+            for handle in active_bodies {
+                if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
+                    let multibody = multibodies
+                        .get_multibody_mut_internal(link.multibody)
+                        .unwrap();
+                    if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                        multibody.solver_id = solver_id;
+                        solver_id += multibody.ndofs();
+                    }
+                }
+            }
+
+            if self.velocity_solver.generic_mj_lambdas.len() < solver_id {
+                self.velocity_solver.generic_mj_lambdas = DVector::zeros(solver_id);
+            } else {
+                self.velocity_solver.generic_mj_lambdas.fill(0.0);
+            }
+
+            self.velocity_solver.mj_lambdas.clear();
+            self.velocity_solver
+                .mj_lambdas
+                .resize(islands.active_island(island_id).len(), DeltaVel::zero());
+        }
 
         for _ in 0..num_task_per_island {
             // We use AtomicPtr because it is Send+Sync while *mut is not.
@@ -254,10 +299,10 @@ impl ParallelIslandSolver {
                     unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
                 let impulse_joints: &mut Vec<JointGraphEdge> =
                     unsafe { std::mem::transmute(impulse_joints.load(Ordering::Relaxed)) };
-                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> = unsafe {
+                let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint> = unsafe {
                     std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
                 };
-                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()> = unsafe {
+                let parallel_joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint> = unsafe {
                     std::mem::transmute(parallel_joint_constraints.load(Ordering::Relaxed))
                 };
 
@@ -271,13 +316,28 @@ impl ParallelIslandSolver {
                     concurrent_loop! {
                         let batch_size = thread.batch_size;
                         for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
-                            let (rb_ids, rb_forces, rb_mass_props): (&RigidBodyIds, &RigidBodyForces, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
-                            let dvel = &mut velocity_solver.mj_lambdas[rb_ids.active_set_offset];
-
-                            // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
-                            //       by the square root of the inertia tensor:
-                            dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt;
-                            dvel.linear += rb_forces.force.component_mul(&rb_mass_props.effective_inv_mass) * params.dt;
+                            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
+                                let multibody = multibodies
+                                    .get_multibody_mut_internal(link.multibody)
+                                    .unwrap();
+                
+                                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                                    let mut mj_lambdas = velocity_solver
+                                        .generic_mj_lambdas
+                                        .rows_mut(multibody.solver_id, multibody.ndofs());
+                                    mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
+                                }
+                            } else {
+                                let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
+                                    bodies.index_bundle(handle.0);
+                
+                                let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset];
+                
+                                // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
+                                //       by the square root of the inertia tensor:
+                                dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
+                                dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
+                            }
                         }
                     }
 
@@ -287,10 +347,8 @@ 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, multibodies, impulse_joints, &mut j_id, &mut jacobians);
+                parallel_contact_constraints.fill_constraints(&thread, params, bodies, multibodies, manifolds);
+                parallel_joint_constraints.fill_constraints(&thread, params, bodies, multibodies, impulse_joints);
                 ThreadContext::lock_until_ge(
                     &thread.num_initialized_constraints,
                     parallel_contact_constraints.constraint_descs.len(),
@@ -306,6 +364,7 @@ impl ParallelIslandSolver {
                         island_id,
                         islands,
                         bodies,
+                        multibodies,
                         manifolds,
                         impulse_joints,
                         parallel_contact_constraints,
diff --git a/src/dynamics/solver/parallel_solver_constraints.rs b/src/dynamics/solver/parallel_solver_constraints.rs
index 9f96df2..3a8f9c8 100644
--- a/src/dynamics/solver/parallel_solver_constraints.rs
+++ b/src/dynamics/solver/parallel_solver_constraints.rs
@@ -3,13 +3,17 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, ThreadContext};
 use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
 use crate::dynamics::solver::generic_velocity_constraint::GenericVelocityConstraint;
-use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint};
+use crate::dynamics::solver::{
+    GenericVelocityGroundConstraint, InteractionGroups, VelocityConstraint,
+    VelocityGroundConstraint,
+};
 use crate::dynamics::{
-    IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyIds,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet,
+    RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+    RigidBodyVelocity,
 };
 use crate::geometry::ContactManifold;
-use crate::math::Real;
+use crate::math::{Real, SPATIAL_DIM};
 #[cfg(feature = "simd-is-enabled")]
 use crate::{
     dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint},
@@ -40,9 +44,11 @@ pub(crate) enum ConstraintDesc {
     NongroundGrouped([usize; SIMD_WIDTH]),
     #[cfg(feature = "simd-is-enabled")]
     GroundGrouped([usize; SIMD_WIDTH]),
+    GenericNongroundNongrouped(usize, usize),
+    GenericGroundNongrouped(usize, usize),
 }
 
-pub(crate) struct ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint> {
+pub(crate) struct ParallelSolverConstraints<VelocityConstraint> {
     pub generic_jacobians: DVector<Real>,
     pub not_ground_interactions: Vec<usize>,
     pub ground_interactions: Vec<usize>,
@@ -51,14 +57,11 @@ pub(crate) struct ParallelSolverConstraints<VelocityConstraint, GenVelocityConst
     pub interaction_groups: InteractionGroups,
     pub ground_interaction_groups: InteractionGroups,
     pub velocity_constraints: Vec<VelocityConstraint>,
-    pub generic_velocity_constraints: Vec<GenVelocityConstraint>,
     pub constraint_descs: Vec<(usize, ConstraintDesc)>,
     pub parallel_desc_groups: Vec<usize>,
 }
 
-impl<VelocityConstraint, GenVelocityConstraint>
-    ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint>
-{
+impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
     pub fn new() -> Self {
         Self {
             generic_jacobians: DVector::zeros(0),
@@ -69,7 +72,6 @@ impl<VelocityConstraint, GenVelocityConstraint>
             interaction_groups: InteractionGroups::new(),
             ground_interaction_groups: InteractionGroups::new(),
             velocity_constraints: vec![],
-            generic_velocity_constraints: vec![],
             constraint_descs: vec![],
             parallel_desc_groups: vec![],
         }
@@ -77,11 +79,14 @@ impl<VelocityConstraint, GenVelocityConstraint>
 }
 
 macro_rules! impl_init_constraints_group {
-    ($VelocityConstraint: ty, $GenVelocityConstraint: ty, $Interaction: ty,
+    ($VelocityConstraint: ty, $Interaction: ty,
      $categorize: ident, $group: ident,
      $data: ident$(.$constraint_index: ident)*,
-     $num_active_constraints: path, $empty_velocity_constraint: expr $(, $weight: ident)*) => {
-        impl ParallelSolverConstraints<$VelocityConstraint, $GenVelocityConstraint> {
+     $body1: ident,
+     $body2: ident,
+     $num_active_constraints: path,
+     $empty_velocity_constraint: expr $(, $weight: ident)*) => {
+        impl ParallelSolverConstraints<$VelocityConstraint> {
             pub fn init_constraint_groups<Bodies>(
                 &mut self,
                 island_id: usize,
@@ -90,6 +95,7 @@ macro_rules! impl_init_constraints_group {
                 multibodies: &MultibodyJointSet,
                 interactions: &mut [$Interaction],
                 interaction_groups: &ParallelInteractionGroups,
+                j_id: &mut usize,
             ) where Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds> {
                 let mut total_num_constraints = 0;
                 let num_groups = interaction_groups.num_groups();
@@ -105,6 +111,9 @@ macro_rules! impl_init_constraints_group {
 
                     self.not_ground_interactions.clear();
                     self.ground_interactions.clear();
+                    self.generic_not_ground_interactions.clear();
+                    self.generic_ground_interactions.clear();
+
                     $categorize(
                         bodies,
                         multibodies,
@@ -119,6 +128,7 @@ macro_rules! impl_init_constraints_group {
                     #[cfg(feature = "simd-is-enabled")]
                     let start_grouped = self.interaction_groups.grouped_interactions.len();
                     let start_nongrouped = self.interaction_groups.nongrouped_interactions.len();
+
                     #[cfg(feature = "simd-is-enabled")]
                     let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len();
                     let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len();
@@ -192,6 +202,48 @@ macro_rules! impl_init_constraints_group {
                         total_num_constraints += $num_active_constraints(interaction);
                     }
 
+                    let multibody_ndofs = |handle| {
+                        if let Some(link) = multibodies.rigid_body_link(handle).copied() {
+                            let multibody = multibodies
+                                .get_multibody(link.multibody)
+                                .unwrap();
+                            multibody.ndofs()
+                        } else {
+                            SPATIAL_DIM
+                        }
+                    };
+
+                    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 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;
+                        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 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;
+                        total_num_constraints += num_constraints;
+                    }
+
                     self.parallel_desc_groups.push(self.constraint_descs.len());
                 }
 
@@ -204,41 +256,58 @@ macro_rules! impl_init_constraints_group {
     }
 }
 
+fn joint_body1(joint: &ImpulseJoint) -> Option<RigidBodyHandle> {
+    Some(joint.body1)
+}
+fn joint_body2(joint: &ImpulseJoint) -> Option<RigidBodyHandle> {
+    Some(joint.body2)
+}
+fn manifold_body1(manifold: &ContactManifold) -> Option<RigidBodyHandle> {
+    manifold.data.rigid_body1
+}
+fn manifold_body2(manifold: &ContactManifold) -> Option<RigidBodyHandle> {
+    manifold.data.rigid_body2
+}
+
 impl_init_constraints_group!(
     AnyVelocityConstraint,
-    GenericVelocityConstraint,
     &mut ContactManifold,
     categorize_contacts,
     group_manifolds,
     data.constraint_index,
+    manifold_body1,
+    manifold_body2,
     VelocityConstraint::num_active_constraints,
     AnyVelocityConstraint::Empty
 );
 
 impl_init_constraints_group!(
     AnyJointVelocityConstraint,
-    (),
     JointGraphEdge,
     categorize_joints,
     group_joints,
     constraint_index,
+    joint_body1,
+    joint_body2,
     AnyJointVelocityConstraint::num_active_constraints,
     AnyJointVelocityConstraint::Empty,
     weight
 );
 
-impl ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
+impl ParallelSolverConstraints<AnyVelocityConstraint> {
     pub fn fill_constraints<Bodies>(
         &mut self,
         thread: &ThreadContext,
         params: &IntegrationParameters,
         bodies: &Bodies,
+        multibodies: &MultibodyJointSet,
         manifolds_all: &[&mut ContactManifold],
     ) where
         Bodies: ComponentSet<RigidBodyIds>
             + ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
-            + ComponentSet<RigidBodyMassProps>,
+            + ComponentSet<RigidBodyMassProps>
+            + ComponentSet<RigidBodyType>,
     {
         let descs = &self.constraint_descs;
 
@@ -264,13 +333,23 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>
                         let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
                         WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
                     }
+                    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);
+                    }
+                    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);
+                    }
                 }
             }
         }
     }
 }
 
-impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
+impl ParallelSolverConstraints<AnyJointVelocityConstraint> {
     pub fn fill_constraints<Bodies>(
         &mut self,
         thread: &ThreadContext,
@@ -278,8 +357,6 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
         bodies: &Bodies,
         multibodies: &MultibodyJointSet,
         joints_all: &[JointGraphEdge],
-        j_id: &mut usize,
-        jacobians: &mut DVector<Real>,
     ) where
         Bodies: ComponentSet<RigidBodyPosition>
             + ComponentSet<RigidBodyVelocity>
@@ -295,11 +372,11 @@ 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, j_id, 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, false);
                     }
                     ConstraintDesc::GroundNongrouped(joint_id) => {
                         let joint = &joints_all[*joint_id].weight;
-                        AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies, multibodies, j_id, 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, false);
                     }
                     #[cfg(feature = "simd-is-enabled")]
                     ConstraintDesc::NongroundGrouped(joint_id) => {
@@ -311,6 +388,16 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, ()> {
                         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);
                     }
+                    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);
+                    }
+                    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);
+                    }
                 }
             }
         }
diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 4a01409..d56c358 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs<