Second round to fix the parallel solver.

.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "rust-analyzer.cargo.features": [
+        "simd-stable",
+        "parallel"
+    ]
+}

src/dynamics/solver/generic_velocity_constraint.rs

@@ -8,64 +8,13 @@ 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 super::{
-use crate::dynamics::solver::GenericVelocityGroundConstraint;
+    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,
@@ -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);
             }
         }
     }

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 super::{
-use crate::dynamics::solver::AnyGenericVelocityConstraint;
+    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);
             }
         }
     }

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 {
+            if color_len[i] != 0 {
-                break;
+                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

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,
+    AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints,
-    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>,
+    contact_constraints: SolverConstraints<AnyVelocityConstraint>,
-    joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
+    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,

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:
+    parallel_contact_constraints: ParallelSolverConstraints<AnyVelocityConstraint>,
-        ParallelSolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
+    parallel_joint_constraints: ParallelSolverConstraints<AnyJointVelocityConstraint>,
-    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();
+        if self.parallel_contact_constraints.generic_jacobians.len() < contact_j_id {
-        self.velocity_solver
+            self.parallel_contact_constraints.generic_jacobians = DVector::zeros(contact_j_id);
-            .mj_lambdas
+        } else {
-            .resize(islands.active_island(island_id).len(), DeltaVel::zero());
+            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);
+                            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
-                            let dvel = &mut velocity_solver.mj_lambdas[rb_ids.active_set_offset];
+                                let multibody = multibodies
+                                    .get_multibody_mut_internal(link.multibody)
+                                    .unwrap();
                 
-                            // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
+                                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                            //       by the square root of the inertia tensor:
+                                    let mut mj_lambdas = velocity_solver
-                            dvel.angular += rb_mass_props.effective_world_inv_inertia_sqrt * rb_forces.torque * params.dt;
+                                        .generic_mj_lambdas
-                            dvel.linear += rb_forces.force.component_mul(&rb_mass_props.effective_inv_mass) * params.dt;
+                                        .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
+                parallel_contact_constraints.fill_constraints(&thread, params, bodies, multibodies, manifolds);
-                let mut jacobians = DVector::zeros(0); // TODO
+                parallel_joint_constraints.fill_constraints(&thread, params, bodies, multibodies, impulse_joints);
-                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);
                 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,

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,
+    ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet,
-    RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    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>
+impl<VelocityConstraint> ParallelSolverConstraints<VelocityConstraint> {
-    ParallelSolverConstraints<VelocityConstraint, GenVelocityConstraint>
-{
     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)*) => {
+     $body1: ident,
-        impl ParallelSolverConstraints<$VelocityConstraint, $GenVelocityConstraint> {
+     $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);
+                    }
                 }
             }
         }

src/dynamics/solver/parallel_velocity_solver.rs

@@ -3,8 +3,9 @@ use crate::concurrent_loop;
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
 use crate::dynamics::{
     solver::{GenericVelocityConstraint, ParallelSolverConstraints},
-    IntegrationParameters, IslandManager, JointGraphEdge, RigidBodyDamping, RigidBodyForces,
+    IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
-    RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
+    RigidBodyVelocity,
 };
 use crate::geometry::ContactManifold;
 use crate::math::Real;
@@ -33,13 +34,11 @@ impl ParallelVelocitySolver {
         island_id: usize,
         islands: &IslandManager,
         bodies: &mut Bodies,
+        multibodies: &mut MultibodyJointSet,
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
-        contact_constraints: &mut ParallelSolverConstraints<
+        contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint>,
-            AnyVelocityConstraint,
+        joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint>,
-            GenericVelocityConstraint,
-        >,
-        joint_constraints: &mut ParallelSolverConstraints<AnyJointVelocityConstraint, ()>,
     ) where
         Bodies: ComponentSet<RigidBodyForces>
             + ComponentSet<RigidBodyIds>
@@ -121,7 +120,7 @@ impl ParallelVelocitySolver {
                 let solve_friction = params.interleave_restitution_and_friction_resolution
                     && params.max_velocity_friction_iterations + i
                         >= params.max_velocity_iterations;
-
+                // Solve joints.
                 solve!(
                     joint_constraints,
                     &joint_constraints.generic_jacobians,
@@ -130,10 +129,27 @@ impl ParallelVelocitySolver {
                 );
                 shift += joint_descs.len();
                 start_index -= joint_descs.len();
+
+                // Solve rigid-body contacts.
                 solve!(
                     contact_constraints,
                     cfm_factor,
+                    &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas,
+                    true,
+                    false
+                );
+                shift += contact_descs.len();
+                start_index -= contact_descs.len();
+
+                // Solve generic rigid-body contacts.
+                solve!(
+                    contact_constraints,
+                    cfm_factor,
+                    &contact_constraints.generic_jacobians,
+                    &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas,
                     true,
                     false
                 );
@@ -144,7 +160,9 @@ impl ParallelVelocitySolver {
                     solve!(
                         contact_constraints,
                         cfm_factor,
+                        &contact_constraints.generic_jacobians,
                         &mut self.mj_lambdas,
+                        &mut self.generic_mj_lambdas,
                         false,
                         true
                     );
@@ -167,7 +185,9 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     cfm_factor,
+                    &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas,
                     false,
                     true
                 );
@@ -184,35 +204,54 @@ impl ParallelVelocitySolver {
             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) =
+                    if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
-                    bodies.index_bundle(handle.0);
+                        let multibody = multibodies
+                            .get_multibody_mut_internal(link.multibody)
+                            .unwrap();
 
-                    let dvel = self.mj_lambdas[rb_ids.active_set_offset];
+                        if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                    let dangvel = rb_mprops
+                            let mj_lambdas = self
-                        .effective_world_inv_inertia_sqrt
+                                .generic_mj_lambdas
-                        .transform_vector(dvel.angular);
+                                .rows(multibody.solver_id, multibody.ndofs());
+                            let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations.
+                            multibody.velocities += mj_lambdas;
+                            multibody.integrate(params.dt);
+                            multibody.forward_kinematics(bodies, false);
+                            multibody.velocities = prev_vels;
+                        }
+                    } else {
+                        let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
+                            bodies.index_bundle(handle.0);
 
-                    // Update positions.
+                        let dvel = self.mj_lambdas[rb_ids.active_set_offset];
-                    let (rb_pos, rb_vels, rb_damping, rb_mprops): (
+                        let dangvel = rb_mprops
-                        &RigidBodyPosition,
+                            .effective_world_inv_inertia_sqrt
-                        &RigidBodyVelocity,
+                            .transform_vector(dvel.angular);
-                        &RigidBodyDamping,
-                        &RigidBodyMassProps,
-                    ) = bodies.index_bundle(handle.0);
 
-                    let mut new_pos = *rb_pos;
+                        // Update positions.
-                    let mut new_vels = *rb_vels;
+                        let (rb_pos, rb_vels, rb_damping, rb_mprops): (
-                    new_vels.linvel += dvel.linear;
+                            &RigidBodyPosition,
-                    new_vels.angvel += dangvel;
+                            &RigidBodyVelocity,
-                    new_vels = new_vels.apply_damping(params.dt, rb_damping);
+                            &RigidBodyDamping,
-                    new_pos.next_position = new_vels.integrate(
+                            &RigidBodyMassProps,
-                        params.dt,
+                        ) = bodies.index_bundle(handle.0);
-                        &rb_pos.position,
+
-                        &rb_mprops.local_mprops.local_com,
+                        let mut new_pos = *rb_pos;
-                    );
+                        let mut new_vels = *rb_vels;
-                    bodies.set_internal(handle.0, new_pos);
+                        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);
+                    }
                 }
             }
+
+            ThreadContext::lock_until_ge(&thread.num_integrated_pos_bodies, active_bodies.len());
         }
 
         // Remove bias from constraints.
@@ -249,7 +288,9 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     cfm_factor,
+                    &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas,
                     true,
                     false
                 );
@@ -259,7 +300,9 @@ impl ParallelVelocitySolver {
                 solve!(
                     contact_constraints,
                     cfm_factor,
+                    &contact_constraints.generic_jacobians,
                     &mut self.mj_lambdas,
+                    &mut self.generic_mj_lambdas,
                     false,
                     true
                 );
@@ -276,23 +319,36 @@ impl ParallelVelocitySolver {
             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): (
+                    if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
-                        &RigidBodyIds,
+                        let multibody = multibodies
-                        &RigidBodyDamping,
+                            .get_multibody_mut_internal(link.multibody)
-                        &RigidBodyMassProps,
+                            .unwrap();
-                    ) = bodies.index_bundle(handle.0);
 
-                    let dvel = self.mj_lambdas[rb_ids.active_set_offset];
+                        if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
-                    let dangvel = rb_mprops
+                            let mj_lambdas = self
-                        .effective_world_inv_inertia_sqrt
+                                .generic_mj_lambdas
-                        .transform_vector(dvel.angular);
+                                .rows(multibody.solver_id, multibody.ndofs());
-                    let damping = *rb_damping; // To avoid borrow issues.
+                            multibody.velocities += mj_lambdas;
+                        }
+                    } else {
+                        let (rb_ids, rb_damping, rb_mprops): (
+                            &RigidBodyIds,
+                            &RigidBodyDamping,
+                            &RigidBodyMassProps,
+                        ) = bodies.index_bundle(handle.0);
 
-                    bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
+                        let dvel = self.mj_lambdas[rb_ids.active_set_offset];
-                        vels.linvel += dvel.linear;
+                        let dangvel = rb_mprops
-                        vels.angvel += dangvel;
+                            .effective_world_inv_inertia_sqrt
-                        *vels = vels.apply_damping(params.dt, &damping);
+                            .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);
+                        });
+                    }
                 }
             }
         }

src/dynamics/solver/solver_constraints.rs

@@ -6,7 +6,6 @@ use super::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::data::ComponentSet;
 use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
 use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint;
-use crate::dynamics::solver::AnyGenericVelocityConstraint;
 use crate::dynamics::solver::GenericVelocityConstraint;
 use crate::dynamics::{
     solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex,
@@ -19,7 +18,7 @@ use crate::math::Real;
 use crate::math::SIMD_WIDTH;
 use na::DVector;
 
-pub(crate) struct SolverConstraints<VelocityConstraint, GenVelocityConstraint> {
+pub(crate) struct SolverConstraints<VelocityConstraint> {
     pub generic_jacobians: DVector<Real>,
     pub not_ground_interactions: Vec<usize>,
     pub ground_interactions: Vec<usize>,
@@ -28,12 +27,9 @@ pub(crate) struct SolverConstraints<VelocityConstraint, GenVelocityConstraint> {
     pub interaction_groups: InteractionGroups,
     pub ground_interaction_groups: InteractionGroups,
     pub velocity_constraints: Vec<VelocityConstraint>,
-    pub generic_velocity_constraints: Vec<GenVelocityConstraint>,
 }
 
-impl<VelocityConstraint, GenVelocityConstraint>
+impl<VelocityConstraint> SolverConstraints<VelocityConstraint> {
-    SolverConstraints<VelocityConstraint, GenVelocityConstraint>
-{
     pub fn new() -> Self {
         Self {
             generic_jacobians: DVector::zeros(0),
@@ -44,7 +40,6 @@ impl<VelocityConstraint, GenVelocityConstraint>
             interaction_groups: InteractionGroups::new(),
             ground_interaction_groups: InteractionGroups::new(),
             velocity_constraints: vec![],
-            generic_velocity_constraints: vec![],
         }
     }
 
@@ -56,11 +51,10 @@ impl<VelocityConstraint, GenVelocityConstraint>
         self.interaction_groups.clear();
         self.ground_interaction_groups.clear();
         self.velocity_constraints.clear();
-        self.generic_velocity_constraints.clear();
     }
 }
 
-impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
+impl SolverConstraints<AnyVelocityConstraint> {
     pub fn init_constraint_groups<Bodies>(
         &mut self,
         island_id: usize,
@@ -132,7 +126,6 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
             + ComponentSet<RigidBodyType>,
     {
         self.velocity_constraints.clear();
-        self.generic_velocity_constraints.clear();
 
         self.init_constraint_groups(
             island_id,
@@ -247,7 +240,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
                 manifold,
                 bodies,
                 multibody_joints,
-                &mut self.generic_velocity_constraints,
+                &mut self.velocity_constraints,
                 &mut self.generic_jacobians,
                 jacobian_id,
                 true,
@@ -277,7 +270,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
                 manifold,
                 bodies,
                 multibody_joints,
-                &mut self.generic_velocity_constraints,
+                &mut self.velocity_constraints,
                 &mut self.generic_jacobians,
                 jacobian_id,
                 true,
@@ -340,7 +333,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
     }
 }
 
-impl SolverConstraints<AnyJointVelocityConstraint, ()> {
+impl SolverConstraints<AnyJointVelocityConstraint> {
     pub fn init<Bodies>(
         &mut self,
         island_id: usize,
@@ -375,7 +368,6 @@ impl SolverConstraints<AnyJointVelocityConstraint, ()> {
         );
 
         self.velocity_constraints.clear();
-        self.generic_velocity_constraints.clear();
 
         self.interaction_groups.clear_groups();
         self.interaction_groups.group_joints(

src/dynamics/solver/velocity_constraint.rs

@@ -1,11 +1,14 @@
 use crate::data::{BundleSet, ComponentSet};
-use crate::dynamics::solver::VelocityGroundConstraint;
+use crate::dynamics::solver::{
+    GenericVelocityConstraint, GenericVelocityGroundConstraint, VelocityGroundConstraint,
+};
 #[cfg(feature = "simd-is-enabled")]
 use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
 use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
 use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
 use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot, WReal};
+use na::DVector;
 
 use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
 
@@ -18,6 +21,8 @@ pub(crate) enum AnyVelocityConstraint {
     GroupedGround(WVelocityGroundConstraint),
     #[cfg(feature = "simd-is-enabled")]
     Grouped(WVelocityConstraint),
+    NongroupedGenericGround(GenericVelocityGroundConstraint),
+    NongroupedGeneric(GenericVelocityConstraint),
     #[allow(dead_code)] // The Empty variant is only used with parallel code.
     Empty,
 }
@@ -49,32 +54,51 @@ impl AnyVelocityConstraint {
             AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(),
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(),
-            AnyVelocityConstraint::Empty => {}
+            AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_bias_from_rhs(),
+            AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
 
     pub fn solve(
         &mut self,
         cfm_factor: Real,
+        jacobians: &DVector<Real>,
         mj_lambdas: &mut [DeltaVel<Real>],
-        solve_normal: bool,
+        generic_mj_lambdas: &mut DVector<Real>,
+        solve_restitution: bool,
         solve_friction: bool,
     ) {
         match self {
             AnyVelocityConstraint::NongroupedGround(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
             }
             AnyVelocityConstraint::Nongrouped(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
             }
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::GroupedGround(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
             }
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::Grouped(c) => {
-                c.solve(cfm_factor, mj_lambdas, solve_normal, solve_friction)
+                c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction)
             }
+            AnyVelocityConstraint::NongroupedGeneric(c) => c.solve(
+                cfm_factor,
+                jacobians,
+                mj_lambdas,
+                generic_mj_lambdas,
+                solve_restitution,
+                solve_friction,
+            ),
+            AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve(
+                cfm_factor,
+                jacobians,
+                generic_mj_lambdas,
+                solve_restitution,
+                solve_friction,
+            ),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }
@@ -87,6 +111,8 @@ impl AnyVelocityConstraint {
             AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all),
             #[cfg(feature = "simd-is-enabled")]
             AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::NongroupedGeneric(c) => c.writeback_impulses(manifold_all),
+            AnyVelocityConstraint::NongroupedGenericGround(c) => c.writeback_impulses(manifold_all),
             AnyVelocityConstraint::Empty => unreachable!(),
         }
     }

src/dynamics/solver/velocity_solver.rs

@@ -1,6 +1,5 @@
 use super::AnyJointVelocityConstraint;
 use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
-use crate::dynamics::solver::AnyGenericVelocityConstraint;
 use crate::dynamics::{
     solver::{AnyVelocityConstraint, DeltaVel},
     IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
@@ -35,7 +34,6 @@ impl VelocitySolver {
         manifolds_all: &mut [&mut ContactManifold],
         joints_all: &mut [JointGraphEdge],
         contact_constraints: &mut [AnyVelocityConstraint],
-        generic_contact_constraints: &mut [AnyGenericVelocityConstraint],
         generic_contact_jacobians: &DVector<Real>,
         joint_constraints: &mut [AnyJointVelocityConstraint],
         generic_joint_jacobians: &DVector<Real>,
@@ -58,22 +56,28 @@ impl VelocitySolver {
 
         // Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
         for handle in islands.active_island(island_id) {
-            let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
+            if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
-                bodies.index_bundle(handle.0);
+                let multibody = multibodies
+                    .get_multibody_mut_internal(link.multibody)
+                    .unwrap();
 
-            let dvel = &mut self.mj_lambdas[ids.active_set_offset];
+                if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
+                    let mut mj_lambdas = self
+                        .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);
 
-            // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
+                let dvel = &mut self.mj_lambdas[ids.active_set_offset];
-            //       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;
-        }
 
-        for (_, multibody) in multibodies.multibodies.iter_mut() {
+                // NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
-            let mut mj_lambdas = self
+                //       by the square root of the inertia tensor:
-                .generic_mj_lambdas
+                dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
-                .rows_mut(multibody.solver_id, multibody.ndofs());
+                dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
-            mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
+            }
         }
 
         /*
@@ -92,10 +96,6 @@ impl VelocitySolver {
             }
 
             for constraint in &mut *contact_constraints {
-                constraint.solve(cfm_factor, &mut self.mj_lambdas[..], true, false);
-            }
-
-            for constraint in &mut *generic_contact_constraints {
                 constraint.solve(
                     cfm_factor,
                     generic_contact_jacobians,
@@ -108,10 +108,6 @@ impl VelocitySolver {
 
             if solve_friction {
                 for constraint in &mut *contact_constraints {
-                    constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
-                }
-
-                for constraint in &mut *generic_contact_constraints {
                     constraint.solve(
                         cfm_factor,
                         generic_contact_jacobians,
@@ -135,10 +131,6 @@ impl VelocitySolver {
 
         for _ in 0..remaining_friction_iterations {
             for constraint in &mut *contact_constraints {
-                constraint.solve(cfm_factor, &mut self.mj_lambdas[..], false, true);
-            }
-
-            for constraint in &mut *generic_contact_constraints {
                 constraint.solve(
                     cfm_factor,
                     generic_contact_jacobians,
@@ -204,9 +196,6 @@ impl VelocitySolver {
         for constraint in &mut *contact_constraints {
             constraint.remove_bias_from_rhs();
         }
-        for constraint in &mut *generic_contact_constraints {
-            constraint.remove_bias_from_rhs();
-        }
 
         for _ in 0..params.max_stabilization_iterations {
             for constraint in &mut *joint_constraints {
@@ -218,10 +207,6 @@ impl VelocitySolver {
             }
 
             for constraint in &mut *contact_constraints {
-                constraint.solve(1.0, &mut self.mj_lambdas[..], true, false);
-            }
-
-            for constraint in &mut *generic_contact_constraints {
                 constraint.solve(
                     1.0,
                     generic_contact_jacobians,
@@ -233,10 +218,6 @@ impl VelocitySolver {
             }
 
             for constraint in &mut *contact_constraints {
-                constraint.solve(1.0, &mut self.mj_lambdas[..], false, true);
-            }
-
-            for constraint in &mut *generic_contact_constraints {
                 constraint.solve(
                     1.0,
                     generic_contact_jacobians,
@@ -290,9 +271,5 @@ impl VelocitySolver {
         for constraint in &*contact_constraints {
             constraint.writeback_impulses(manifolds_all);
         }
-
-        for constraint in &*generic_contact_constraints {
-            constraint.writeback_impulses(manifolds_all);
-        }
     }
 }