diff --git a/.run/all_examples3.run.xml b/.run/all_examples3.run.xml
index c53f5f1..a1d673c 100644
--- a/.run/all_examples3.run.xml
+++ b/.run/all_examples3.run.xml
@@ -1,7 +1,7 @@
 <component name="ProjectRunConfigurationManager">
   <configuration default="false" name="all_examples3" type="CargoCommandRunConfiguration" factoryName="Cargo Command">
     <option name="buildProfileId" value="release" />
-    <option name="command" value="run --package nexus_examples_3d --bin all_examples3 --features cpu" />
+    <option name="command" value="run --package nexus_examples_3d --bin all_examples3 --features cpu,metal" />
     <option name="workingDirectory" value="file://$PROJECT_DIR$" />
     <envs />
     <option name="emulateTerminal" value="false" />
diff --git a/crates/examples3d/mujoco_menagerie3.rs b/crates/examples3d/mujoco_menagerie3.rs
index bde6198..5da2377 100644
--- a/crates/examples3d/mujoco_menagerie3.rs
+++ b/crates/examples3d/mujoco_menagerie3.rs
@@ -320,6 +320,12 @@ async fn load_scene(
     // MJCF is Z-up: gravity points along -Z (set after `finalize`, which builds
     // the rigid-body state with the default -Y gravity).
     state.set_rbd_gravity(viewer.backend(), [0.0, 0.0, -9.81]);
+    // MuJoCo-style explicit coriolis: the mass matrix / LU / gravity solve
+    // runs once per step instead of once per substep. This matches how
+    // MuJoCo integrates these models and saves ~25% of the step time.
+    if let Some(rbd) = state.rbd.as_mut() {
+        rbd.multibodies_mut().set_implicit_coriolis(false);
+    }
     Ok(state)
 }
 
diff --git a/src_rbd/broad_phase/lbvh.rs b/src_rbd/broad_phase/lbvh.rs
index ead559c..067a43c 100644
--- a/src_rbd/broad_phase/lbvh.rs
+++ b/src_rbd/broad_phase/lbvh.rs
@@ -7,9 +7,9 @@ use crate::math::Pose;
 use crate::shaders::PaddedVector;
 use crate::shaders::bounding_volumes::Aabb;
 use crate::shaders::broad_phase::{
-    CollisionPair, GpuLbvhBuild, GpuLbvhComputeDomain, GpuLbvhComputeMorton,
-    GpuLbvhFindCollisionPairs, GpuLbvhInitDispatch, GpuLbvhRefitInternal, GpuLbvhRefitLeaves,
-    GpuLbvhResetCollisionPairs, LbvhNode,
+    CollisionPair, GpuBfComputeAabbs, GpuBfFindPairs, GpuLbvhBuild, GpuLbvhComputeDomain,
+    GpuLbvhComputeMorton, GpuLbvhFindCollisionPairs, GpuLbvhInitDispatch, GpuLbvhRefitInternal,
+    GpuLbvhRefitLeaves, GpuLbvhResetCollisionPairs, LbvhNode,
 };
 use crate::shaders::shapes::Shape;
 use crate::utils::{RadixSort, RadixSortWorkspace};
@@ -33,6 +33,10 @@ pub struct GpuLbvh {
     reset_collision_pairs: GpuLbvhResetCollisionPairs,
     find_collision_pairs: GpuLbvhFindCollisionPairs,
     lbvh_init_indirect_args: GpuLbvhInitDispatch,
+    // Kernels for brute-force broad-phase for small scenes
+    // (typically, small scenes but many batches).
+    bf_compute_aabbs: GpuBfComputeAabbs,
+    bf_find_pairs: GpuBfFindPairs,
 }
 
 /// GPU-resident state for LBVH construction and queries.
@@ -53,6 +57,10 @@ pub struct LbvhState {
     sorted_colliders: Tensor<u32>,
     tree: Tensor<LbvhNode>,
     sort_workspace: RadixSortWorkspace,
+    /// Per-collider world AABBs, only used by the brute-force tiny-batch path
+    /// (strided by the per-batch collider capacity, like the other
+    /// per-collider buffers).
+    aabbs: Tensor<Aabb>,
 }
 
 /// High-level LBVH broad-phase interface.
@@ -79,6 +87,7 @@ impl LbvhState {
             sorted_colliders: Tensor::vector_uninit(backend, 0, usages).unwrap(),
             tree: Tensor::vector_uninit(backend, 0, usages).unwrap(),
             sort_workspace: RadixSortWorkspace::new(backend),
+            aabbs: Tensor::vector_uninit(backend, 0, usages).unwrap(),
             buffer_usages: usages,
         }
     }
@@ -135,6 +144,15 @@ impl LbvhState {
             self.n_sort_active = None;
         }
     }
+
+    /// Sizes the brute-force path's AABB buffer (per-collider, capacity
+    /// stride). Kept separate from [`Self::resize_buffers`] so the tree
+    /// buffers aren't allocated when only the brute-force path runs.
+    fn resize_bf_buffers(&mut self, backend: &GpuBackend, colliders_len: u32) {
+        if (self.aabbs.len() as u32) < colliders_len {
+            self.aabbs = Tensor::vector_uninit(backend, colliders_len, self.buffer_usages).unwrap();
+        }
+    }
 }
 
 impl Lbvh {
@@ -269,13 +287,14 @@ impl Lbvh {
         collision_pairs_len: &mut Tensor<u32>,
         collision_pairs_indirect: &mut Tensor<[u32; 3]>,
         collision_groups: &Tensor<crate::rapier::geometry::InteractionGroups>,
+        pair_filter: &Tensor<[u32; 2]>,
     ) -> Result<(), GpuBackendError> {
         // One thread per live collider (leaf); padding slots aren't in the tree.
         let colliders_per_batch = active_per_batch;
 
         self.shaders.reset_collision_pairs.call(
             pass,
-            [1u32, num_batches, 1],
+            [num_batches, 1, 1],
             collision_pairs_len,
         )?;
         self.shaders.find_collision_pairs.call(
@@ -286,13 +305,78 @@ impl Lbvh {
             collision_pairs_len,
             collision_groups,
             batch_indices,
+            pair_filter,
+        )?;
+        self.shaders.lbvh_init_indirect_args.call(
+            pass,
+            256u32,
+            collision_pairs_len,
+            collision_pairs_indirect,
+        )?;
+        Ok(())
+    }
+
+    /// Brute-force O(n²) replacement for [`Self::update_tree`] +
+    /// [`Self::find_pairs`], used when each batch holds at most
+    /// [`Self::BRUTE_FORCE_MAX_COLLIDERS`] colliders. One AABB pass and one
+    /// all-pairs pass emit the same pair set as the whole tree pipeline.
+    #[allow(clippy::too_many_arguments)]
+    pub fn brute_force_pairs(
+        &self,
+        backend: &GpuBackend,
+        pass: &mut GpuPass,
+        state: &mut LbvhState,
+        colliders_len: u32,
+        active_per_batch: u32,
+        num_batches: u32,
+        poses: &Tensor<Pose>,
+        vertex_buffers: &Tensor<PaddedVector>,
+        shapes: &Tensor<Shape>,
+        batch_indices: &Tensor<crate::shaders::utils::BatchIndices>,
+        collision_pairs: &mut Tensor<CollisionPair>,
+        collision_pairs_len: &mut Tensor<u32>,
+        collision_pairs_indirect: &mut Tensor<[u32; 3]>,
+        collision_groups: &Tensor<crate::rapier::geometry::InteractionGroups>,
+        pair_filter: &Tensor<[u32; 2]>,
+    ) -> Result<(), GpuBackendError> {
+        state.resize_bf_buffers(backend, colliders_len);
+
+        self.shaders.bf_compute_aabbs.call(
+            pass,
+            [active_per_batch * num_batches, 1, 1],
+            poses,
+            shapes,
+            &mut state.aabbs,
+            batch_indices,
+            vertex_buffers,
+        )?;
+        self.shaders.reset_collision_pairs.call(
+            pass,
+            [num_batches, 1, 1],
+            collision_pairs_len,
+        )?;
+        self.shaders.bf_find_pairs.call(
+            pass,
+            [active_per_batch * active_per_batch * num_batches, 1, 1],
+            &state.aabbs,
+            collision_pairs,
+            collision_pairs_len,
+            collision_groups,
+            batch_indices,
+            pair_filter,
         )?;
+        // Single 256-lane workgroup: parallel max over the per-batch counts.
         self.shaders.lbvh_init_indirect_args.call(
             pass,
-            1u32,
+            256u32,
             collision_pairs_len,
             collision_pairs_indirect,
         )?;
         Ok(())
     }
 }
+
+/// Per-batch collider count at or below which the pipeline uses
+/// [`Lbvh::brute_force_pairs`] instead of building a tree.
+/// `NEXUS_DISABLE_BF=1` forces the LBVH path (A/B debugging).
+pub const BRUTE_FORCE_MAX_COLLIDERS: u32 = 64;
diff --git a/src_rbd/broad_phase/narrow_phase.rs b/src_rbd/broad_phase/narrow_phase.rs
index b37cfb5..82d6ed6 100644
--- a/src_rbd/broad_phase/narrow_phase.rs
+++ b/src_rbd/broad_phase/narrow_phase.rs
@@ -42,6 +42,7 @@ impl GpuNarrowPhase {
         contacts: &mut Tensor<GpuIndexedContact>,
         contacts_len: &mut Tensor<u32>,
         contacts_indirect: &mut Tensor<[u32; 3]>,
+        mb_sweep_indirect: &mut Tensor<[u32; 3]>,
         pfm_pairs: &mut Tensor<NarrowPhasePfmPair>,
         pfm_pairs_len: &mut Tensor<u32>,
         pfm_pairs_indirect: &mut Tensor<[u32; 3]>,
@@ -51,7 +52,7 @@ impl GpuNarrowPhase {
     ) -> Result<(), GpuBackendError> {
         let num_batches = contacts_len.len() as u32;
         self.reset_narrow_phase
-            .call(pass, [1u32, num_batches, 1], contacts_len, pfm_pairs_len)?;
+            .call(pass, [num_batches, 1, 1], contacts_len, pfm_pairs_len)?;
 
         self.narrow_phase.call(
             pass,
@@ -84,7 +85,7 @@ impl GpuNarrowPhase {
         )?;
 
         self.init_pfm_pfm_indirect_args
-            .call(pass, 1u32, pfm_pairs_len, pfm_pairs_indirect)?;
+            .call(pass, 256u32, pfm_pairs_len, pfm_pairs_indirect)?;
         self.narrow_phase_pfm_pfm.call(
             pass,
             &*pfm_pairs_indirect,
@@ -98,8 +99,14 @@ impl GpuNarrowPhase {
             collider_parent,
             collider_materials,
         )?;
-        self.init_contacts_indirect_args
-            .call(pass, 1u32, contacts_len, contacts_indirect)?;
+        self.init_contacts_indirect_args.call(
+            pass,
+            256u32,
+            contacts_len,
+            contacts_indirect,
+            mb_sweep_indirect,
+            batch_indices,
+        )?;
 
         Ok(())
     }
diff --git a/src_rbd/dynamics/coloring.rs b/src_rbd/dynamics/coloring.rs
index 84b2cb5..f187bfd 100644
--- a/src_rbd/dynamics/coloring.rs
+++ b/src_rbd/dynamics/coloring.rs
@@ -10,6 +10,7 @@
 use crate::pipeline::RunStats;
 use crate::shaders::dynamics::TwoBodyConstraint;
 use crate::shaders::dynamics::{
+    GpuColorBucketsCount, GpuColorBucketsReset, GpuColorBucketsScan, GpuColorBucketsScatter,
     GpuFixConflictsTopoGc, GpuResetCompletionFlagTopoGc, GpuResetLuby, GpuResetTopoGc,
     GpuStepGraphColoringLuby, GpuStepGraphColoringTopoGc,
 };
@@ -33,6 +34,32 @@ pub struct GpuColoring {
     /// Detects and fixes conflicts in TOPO-GC coloring.
     fix_conflicts_topo_gc_kernel: GpuFixConflictsTopoGc,
     reset_completion_flag_topo_gc: GpuResetCompletionFlagTopoGc,
+    // Workspace for bucket-sorting constraint ids by color so each color iteration
+    // only touches their own constraint.
+    color_buckets_reset: GpuColorBucketsReset,
+    color_buckets_count: GpuColorBucketsCount,
+    color_buckets_scan: GpuColorBucketsScan,
+    color_buckets_scatter: GpuColorBucketsScatter,
+}
+
+/// Buffers for the per-color constraint bucket sort.
+pub struct ColorBucketsArgs<'a> {
+    /// Indirect dispatch arguments based on contact count.
+    pub contacts_len_indirect: &'a Tensor<[u32; 3]>,
+    /// Color assigned to each constraint by graph coloring.
+    pub constraints_colors: &'a Tensor<u32>,
+    /// Number of contacts per batch.
+    pub contacts_len: &'a Tensor<u32>,
+    /// Per-batch per-color counts (stride `solver_color_buckets_stride`).
+    pub color_bucket_counts: &'a mut Tensor<u32>,
+    /// Per-batch per-color exclusive prefix sums.
+    pub color_bucket_starts: &'a mut Tensor<u32>,
+    /// Scatter cursors (seeded from the starts).
+    pub color_bucket_cursors: &'a mut Tensor<u32>,
+    /// Constraint ids bucket-sorted by color (contacts layout).
+    pub color_sorted_ids: &'a mut Tensor<u32>,
+    /// Shared per-batch capacity / section-offset uniform.
+    pub batch_indices: &'a Tensor<crate::shaders::utils::BatchIndices>,
 }
 
 /// Arguments for graph coloring dispatch.
@@ -221,6 +248,48 @@ impl GpuColoring {
         num_colors
     }
 
+    /// Bucket-sorts the constraint ids by their color.
+    pub fn dispatch_build_color_buckets(
+        &self,
+        pass: &mut GpuPass,
+        args: ColorBucketsArgs<'_>,
+        color_buckets_stride: u32,
+        num_batches: u32,
+    ) -> Result<(), GpuBackendError> {
+        self.color_buckets_reset.call(
+            pass,
+            [color_buckets_stride, num_batches, 1],
+            args.color_bucket_counts,
+            args.batch_indices,
+        )?;
+        self.color_buckets_count.call(
+            pass,
+            args.contacts_len_indirect,
+            args.constraints_colors,
+            args.contacts_len,
+            args.color_bucket_counts,
+            args.batch_indices,
+        )?;
+        self.color_buckets_scan.call(
+            pass,
+            [1, num_batches, 1],
+            args.color_bucket_counts,
+            args.color_bucket_starts,
+            args.color_bucket_cursors,
+            args.batch_indices,
+        )?;
+        self.color_buckets_scatter.call(
+            pass,
+            args.contacts_len_indirect,
+            args.constraints_colors,
+            args.contacts_len,
+            args.color_bucket_cursors,
+            args.color_sorted_ids,
+            args.batch_indices,
+        )?;
+        Ok(())
+    }
+
     /// Runs a fixed number of iterations of the topo-gc coloring.
     pub fn dispatch_topo_gc_bounded<'a>(
         &self,
@@ -230,6 +299,28 @@ impl GpuColoring {
     ) -> Result<(), GpuBackendError> {
         // Reset coloring state.
         self.dispatch_reset_topo_gc(pass, &mut args)?;
+        self.dispatch_topo_gc_iterations(pass, args, max_colors)
+    }
+
+    /// Resets the topo-gc coloring state (all constraints uncolored). Public
+    /// so a seeding pass (e.g. warmstart color transfer) can run between the
+    /// reset and [`Self::dispatch_topo_gc_iterations`].
+    pub fn dispatch_topo_gc_reset<'a>(
+        &self,
+        pass: &mut GpuPass,
+        mut args: ColoringArgs<'a>,
+    ) -> Result<(), GpuBackendError> {
+        self.dispatch_reset_topo_gc(pass, &mut args)
+    }
+
+    /// Runs the bounded topo-gc step/fix-conflicts iterations, assuming the
+    /// coloring state was already reset (and possibly seeded).
+    pub fn dispatch_topo_gc_iterations<'a>(
+        &self,
+        pass: &mut GpuPass,
+        mut args: ColoringArgs<'a>,
+        max_colors: u32,
+    ) -> Result<(), GpuBackendError> {
         for _ in 0..max_colors {
             self.reset_completion_flag_topo_gc
                 .call(pass, 1u32, args.uncolored)?;
diff --git a/src_rbd/dynamics/joint.rs b/src_rbd/dynamics/joint.rs
index 08f29d9..8e35a58 100644
--- a/src_rbd/dynamics/joint.rs
+++ b/src_rbd/dynamics/joint.rs
@@ -5,7 +5,7 @@
 
 use crate::math::Pose;
 use crate::shaders::dynamics::{
-    GpuIncJointColor, GpuInitJointConstraints, GpuRemoveJointBias, GpuResetJointColor,
+    GpuInitJointConstraints,
     GpuSolveJointConstraints, GpuUpdateJointConstraints, ImpulseJoint, JointConstraint,
     JointConstraintBuilder, LocalMassProperties, RbdSimParams, Velocity, WorldMassProperties,
 };
@@ -83,8 +83,9 @@ pub struct GpuImpulseJointSet {
     /// Identical across batches by the equal-topology invariant.
     num_active_joints: u32,
     num_colors: u32,
-    max_color_group_len: u32,
-    curr_color: Tensor<u32>,
+    /// Host copy of the per-color prefix sums in `color_groups` (identical
+    /// across batches), used to size each per-color solve dispatch exactly.
+    color_groups_cpu: Vec<u32>,
     color_groups: Tensor<u32>,
     joints: Tensor<ImpulseJoint>,
     builders: Tensor<JointConstraintBuilder>,
@@ -170,7 +171,6 @@ impl GpuImpulseJointSet {
         let max_joints = filtered_lens.iter().copied().max().unwrap_or(0);
 
         let mut global_num_colors = 0u32;
-        let mut global_max_color_group_len = 0u32;
 
         // Per-environment sorted joints and color groups.
         let mut per_env_sorted_joints: Vec<Vec<ImpulseJoint>> = Vec::new();
@@ -255,8 +255,6 @@ impl GpuImpulseJointSet {
                 color_groups[*color as usize] += 1;
             }
 
-            let env_max_color_group_len = color_groups.iter().copied().max().unwrap_or_default();
-
             // Prefix sum.
             for i in 0..color_groups.len().saturating_sub(1) {
                 color_groups[i + 1] += color_groups[i];
@@ -273,7 +271,6 @@ impl GpuImpulseJointSet {
             }
 
             global_num_colors = global_num_colors.max(env_num_colors);
-            global_max_color_group_len = global_max_color_group_len.max(env_max_color_group_len);
 
             per_env_sorted_joints.push(sorted_gpu_joints);
             per_env_color_groups.push(color_groups);
@@ -329,8 +326,7 @@ impl GpuImpulseJointSet {
             // invariant, so the per-batch active count is any env's count.
             num_active_joints: filtered_lens.first().copied().unwrap_or(0),
             num_colors: global_num_colors,
-            max_color_group_len: global_max_color_group_len,
-            curr_color: Tensor::scalar(backend, 0u32, usage | BufferUsages::UNIFORM).unwrap(),
+            color_groups_cpu: all_color_groups.clone(),
             color_groups: Tensor::vector(backend, &all_color_groups, usage).unwrap(),
             joints: Tensor::vector(backend, &all_joints, usage).unwrap(),
             builders: Tensor::matrix_uninit(backend, num_batches, max_joints, usage).unwrap(),
@@ -372,12 +368,8 @@ pub struct GpuJointSolver {
     init_joint_constraints: GpuInitJointConstraints,
     /// Updates joint constraints each substep.
     update_joint_constraints: GpuUpdateJointConstraints,
-    reset_joint_color: GpuResetJointColor,
-    inc_joint_color: GpuIncJointColor,
     /// Solves joint constraints.
     solve_joint_constraints: GpuSolveJointConstraints,
-    /// Removes bias from joint constraints.
-    remove_joint_bias: GpuRemoveJointBias,
 }
 
 /// Arguments given to the joint solver.
@@ -394,6 +386,9 @@ pub struct JointSolverArgs<'a> {
     pub local_mprops: &'a Tensor<LocalMassProperties>,
     /// Shared per-batch capacity / section-offset uniform.
     pub batch_indices: &'a Tensor<crate::shaders::utils::BatchIndices>,
+    /// Per-color-index uniform tensors (`color_uniforms[c] == c`),
+    /// shared with the contact solver.
+    pub color_uniforms: &'a [Tensor<u32>],
 }
 
 impl GpuJointSolver {
@@ -455,32 +450,33 @@ impl GpuJointSolver {
             return Ok(());
         }
 
-        if !use_bias {
-            self.remove_joint_bias.call(
-                pass,
-                [args.joints.len, args.num_batches, 1],
-                &mut args.joints.constraints,
-                args.batch_indices,
-            )?;
-        }
-
-        self.reset_joint_color
-            .call(pass, 1u32, &mut args.joints.curr_color)?;
-
-        for _ in 0..args.joints.num_colors {
-            // TODO PERF: figure out a way to dispatch a number of threads that fits
-            //            more tightly the size of the current color.
+        // Convert `use_bias` to its uniform.
+        //  We can use `color_uniforms` since `color_uniforms[k] == k`.
+        let use_bias_uniform = &args.color_uniforms[use_bias as usize];
+
+        // One dispatch per color, sized exactly to that color's group (the
+        // prefix sums are known on the host). The color index is bound as a
+        // tiny pre-built uniform instead of a GPU-incremented cursor.
+        for c in 0..args.joints.num_colors as usize {
+            let start = if c > 0 {
+                args.joints.color_groups_cpu[c - 1]
+            } else {
+                0
+            };
+            let group_len = args.joints.color_groups_cpu[c] - start;
+            if group_len == 0 {
+                continue;
+            }
             self.solve_joint_constraints.call(
                 pass,
-                [args.joints.max_color_group_len, args.num_batches, 1],
+                [group_len, args.num_batches, 1],
                 &mut args.joints.constraints,
                 solver_vels,
                 &args.joints.color_groups,
-                &args.joints.curr_color,
+                &args.color_uniforms[c],
                 args.batch_indices,
+                use_bias_uniform,
             )?;
-            self.inc_joint_color
-                .call(pass, 1u32, &mut args.joints.curr_color)?;
         }
 
         Ok(())
diff --git a/src_rbd/dynamics/mod.rs b/src_rbd/dynamics/mod.rs
index b534ea5..023e512 100644
--- a/src_rbd/dynamics/mod.rs
+++ b/src_rbd/dynamics/mod.rs
@@ -1,7 +1,7 @@
 //! Rigid-body dynamics: forces, velocities, constraints, and solvers.
 
 pub use crate::shaders::dynamics::RbdSimParams;
-pub use coloring::{ColoringArgs, GpuColoring};
+pub use coloring::{ColorBucketsArgs, ColoringArgs, GpuColoring};
 pub use joint::{GpuImpulseJointSet, GpuJointSolver, JointSolverArgs};
 pub use mprops_update::{GpuMpropsUpdate, GpuSyncColliderPosesShader};
 #[cfg(feature = "dim3")]
diff --git a/src_rbd/dynamics/multibody/loop_closing_joints.rs b/src_rbd/dynamics/multibody/loop_closing_joints.rs
index e9ba6c8..92d012a 100644
--- a/src_rbd/dynamics/multibody/loop_closing_joints.rs
+++ b/src_rbd/dynamics/multibody/loop_closing_joints.rs
@@ -303,7 +303,6 @@ impl GpuMultibodySet {
         }
         self.mb_imp_joint_color_groups =
             Tensor::vector(backend, &all_color_groups, storage).unwrap();
-        self.mb_imp_joint_curr_color = Tensor::scalar(backend, 0u32, usage_u).unwrap();
         self.mb_imp_joint_num_colors = global_num_colors;
         self.mb_imp_joint_max_color_group_len = global_max_color_group_len;
     }
diff --git a/src_rbd/dynamics/multibody/multibody_from_rapier.rs b/src_rbd/dynamics/multibody/multibody_from_rapier.rs
index cb390b3..75698c0 100644
--- a/src_rbd/dynamics/multibody/multibody_from_rapier.rs
+++ b/src_rbd/dynamics/multibody/multibody_from_rapier.rs
@@ -3,8 +3,9 @@
 use super::multibody_set::*;
 use crate::shaders::dynamics::{
     ConstraintSoftness, MAX_AXIS_CONSTRAINTS, MAX_MB_CONTACT_CONSTRAINTS_PER_MB,
-    MbImpulseJointBuilder, MbImpulseJointConstraint, MultibodyContactConstraint, MultibodyInfo,
-    MultibodyJointConstraint, MultibodyLinkStatic, MultibodyLinkWorkspace, RbdSimParams,
+    MbDofCoupling, MbImpulseJointBuilder, MbImpulseJointConstraint, MultibodyContactConstraint,
+    MultibodyInfo, MultibodyJointConstraint, MultibodyLinkStatic, MultibodyLinkWorkspace,
+    RbdSimParams,
 };
 use crate::shaders::utils::linalg::MAX_MB_DOFS;
 use glamx::Vec4;
@@ -45,15 +46,24 @@ impl GpuMultibodySet {
         let mut per_env_dof_vels: Vec<Vec<f32>> = Vec::with_capacity(num_batches as usize);
         let mut per_env_dof_damping: Vec<Vec<f32>> = Vec::with_capacity(num_batches as usize);
         let mut per_env_dof_armature: Vec<Vec<f32>> = Vec::with_capacity(num_batches as usize);
+        let mut per_env_dof_stiffness: Vec<Vec<f32>> = Vec::with_capacity(num_batches as usize);
+        let mut per_env_dof_spring_ref: Vec<Vec<f32>> = Vec::with_capacity(num_batches as usize);
+        let mut per_env_dof_kinematic: Vec<Vec<f32>> = Vec::with_capacity(num_batches as usize);
+        let mut per_env_couplings: Vec<Vec<MbDofCoupling>> =
+            Vec::with_capacity(num_batches as usize);
 
         let mut global_max_mb = 0u32;
         let mut global_max_links = 0u32;
+        // Per-multibody maxima (not per-env sums) for the uniform loop bounds.
+        let mut max_mb_ndofs = 0u32;
+        let mut max_mb_links = 0u32;
         let mut global_max_dofs = 0u32;
         let mut global_max_jac = 0u32;
         let mut global_max_mm = 0u32;
         let mut global_max_cor = 0u32;
         let mut global_max_icdt = 0u32;
         let mut global_max_cons = 0u32;
+        let mut global_max_couplings = 0u32;
 
         for (set, body_ids, bodies) in environments {
             let mut infos = Vec::new();
@@ -63,6 +73,10 @@ impl GpuMultibodySet {
             let mut dof_vels = Vec::new();
             let mut dof_damping = Vec::new();
             let mut dof_armature = Vec::new();
+            let mut dof_stiffness = Vec::new();
+            let mut dof_spring_ref = Vec::new();
+            let mut dof_kinematic = Vec::new();
+            let mut couplings = Vec::new();
 
             let mut first_link = 0u32;
             let mut first_dof = 0u32;
@@ -71,6 +85,7 @@ impl GpuMultibodySet {
             let mut cor_off = 0u32;
             let mut icdt_off = 0u32;
             let mut cons_off = 0u32;
+            let mut coupling_off = 0u32;
 
             for (mb_idx, mb) in set.multibodies().enumerate() {
                 if mb.ndofs() > MAX_MB_DOFS {
@@ -99,6 +114,8 @@ impl GpuMultibodySet {
                 };
                 let ndofs = mb.ndofs() as u32 - root_ndof_adjust;
                 let num_links = mb.num_links() as u32;
+                max_mb_ndofs = max_mb_ndofs.max(ndofs);
+                max_mb_links = max_mb_links.max(num_links);
 
                 // Count maximum constraint slots this multibody could need: for
                 // each non-root non-kinematic joint, every free axis with a limit
@@ -133,6 +150,11 @@ impl GpuMultibodySet {
                     })
                     .sum::<u32>();
 
+                // One extra constraint slot per DoF coupling (they are solved
+                // among the joint constraints).
+                let num_couplings = mb.couplings().len() as u32;
+                let max_constraints = max_constraints + num_couplings;
+
                 infos.push(MultibodyInfo {
                     first_link,
                     num_links,
@@ -147,6 +169,9 @@ impl GpuMultibodySet {
                     max_constraints,
                     self_contacts_enabled: if mb.self_contacts_enabled() { 1 } else { 0 },
                     contact_constraint_count: 0,
+                    batch_contacts_len: 0,
+                    first_coupling: coupling_off,
+                    num_couplings,
                 });
 
                 // `assembly_id` is not exposed publicly on `MultibodyLink`, so we
@@ -162,6 +187,7 @@ impl GpuMultibodySet {
                 let mb_damping = mb.damping();
                 let mb_armature = mb.armature();
                 let mut rapier_assembly = 0usize;
+                let mb_statics_start = statics.len();
                 for (link_idx, link) in mb.links().enumerate() {
                     let rb_id = body_ids
                         .get(&link.rigid_body_handle())
@@ -241,17 +267,53 @@ impl GpuMultibodySet {
                     // Both are added to the mass-matrix diagonal in the dynamics
                     // shader, exactly as rapier's `update_mass_matrix` does
                     // (`diag = damping·dt + armature`).
+                    // Per-DoF spring stiffness / rest position (rapier's
+                    // `MultibodyJoint::spring`, MuJoCo `<joint stiffness
+                    // springref>`): flattened per free axis in DoF order,
+                    // exactly like damping. The kinematic mask marks DoFs whose
+                    // velocities are user-controlled (identity mass-matrix rows,
+                    // zero acceleration).
+                    let locked = link.joint().data.locked_axes.bits() as u32;
+                    let is_kin = link.joint().kinematic;
+                    let mut free_axis_of_dof = [0usize; 6];
+                    let mut nfree = 0usize;
+                    for ax in 0..6usize {
+                        if (locked >> ax) & 1 == 0 {
+                            free_axis_of_dof[nfree] = ax;
+                            nfree += 1;
+                        }
+                    }
                     for d in 0..link_ndofs as usize {
                         dof_vals.push(0.0);
                         dof_vels.push(0.0);
                         dof_damping.push(mb_damping[rapier_assembly + d]);
                         dof_armature.push(mb_armature[rapier_assembly + d]);
+                        let (k_s, rest) = link.joint().spring(free_axis_of_dof[d]);
+                        dof_stiffness.push(k_s);
+                        dof_spring_ref.push(rest);
+                        dof_kinematic.push(if is_kin { 1.0 } else { 0.0 });
                     }
                     // Advance by rapier's full link DoF count (which includes a
                     // fixed root's DoFs, where `link_ndofs` above is 0).
                     rapier_assembly += link.joint().ndofs();
                 }
 
+                // DoF couplings, converted to the GPU's re-numbered assembly
+                // ids.
+                for c in mb.couplings() {
+                    let g1 = statics[mb_statics_start + c.link1].assembly_id + c.dof1 as u32;
+                    let g2 = statics[mb_statics_start + c.link2].assembly_id + c.dof2 as u32;
+                    couplings.push(MbDofCoupling {
+                        dof1: g1,
+                        dof2: g2,
+                        link_axis1: c.link1 as u32 | ((c.axis1 as u32) << 16),
+                        link_axis2: c.link2 as u32 | ((c.axis2 as u32) << 16),
+                        coeff: c.coeff,
+                        offset: c.offset,
+                        _pad: [0; 2],
+                    });
+                }
+
                 first_link += num_links;
                 first_dof += ndofs;
                 jac_off += num_links * 6 * ndofs;
@@ -259,6 +321,7 @@ impl GpuMultibodySet {
                 cor_off += num_links * 3 * ndofs;
                 icdt_off += 6 * ndofs;
                 cons_off += max_constraints;
+                coupling_off += num_couplings;
             }
 
             global_max_mb = global_max_mb.max(infos.len() as u32);
@@ -269,6 +332,7 @@ impl GpuMultibodySet {
             global_max_cor = global_max_cor.max(cor_off);
             global_max_icdt = global_max_icdt.max(icdt_off);
             global_max_cons = global_max_cons.max(cons_off);
+            global_max_couplings = global_max_couplings.max(coupling_off);
 
             per_env_infos.push(infos);
             per_env_links_static.push(statics);
@@ -277,6 +341,10 @@ impl GpuMultibodySet {
             per_env_dof_vels.push(dof_vels);
             per_env_dof_damping.push(dof_damping);
             per_env_dof_armature.push(dof_armature);
+            per_env_dof_stiffness.push(dof_stiffness);
+            per_env_dof_spring_ref.push(dof_spring_ref);
+            per_env_dof_kinematic.push(dof_kinematic);
+            per_env_couplings.push(couplings);
         }
 
         // Pad capacities (avoid empty buffers — GPU dislikes size-zero storage bindings).
@@ -288,6 +356,7 @@ impl GpuMultibodySet {
         let cor_cap = global_max_cor.max(1);
         let icdt_cap = global_max_icdt.max(1);
         let cons_cap = global_max_cons.max(1);
+        let couplings_cap = global_max_couplings.max(1);
         // One length-`dofs_cap` column of `M⁻¹` per constraint slot.
         let cons_col_cap = cons_cap.saturating_mul(dofs_cap).max(1);
 
@@ -330,6 +399,12 @@ impl GpuMultibodySet {
         let mut all_dof_vels: Vec<f32> = Vec::with_capacity((dofs_cap * num_batches) as usize);
         let mut all_dof_damping: Vec<f32> = Vec::with_capacity((dofs_cap * num_batches) as usize);
         let mut all_dof_armature: Vec<f32> = Vec::with_capacity((dofs_cap * num_batches) as usize);
+        let mut all_dof_stiffness: Vec<f32> = Vec::with_capacity((dofs_cap * num_batches) as usize);
+        let mut all_dof_spring_ref: Vec<f32> =
+            Vec::with_capacity((dofs_cap * num_batches) as usize);
+        let mut all_dof_kinematic: Vec<f32> = Vec::with_capacity((dofs_cap * num_batches) as usize);
+        let mut all_couplings: Vec<MbDofCoupling> =
+            Vec::with_capacity((couplings_cap * num_batches) as usize);
 
         let dummy_info = MultibodyInfo::default();
         let dummy_stat: MultibodyLinkStatic = bytemuck::Zeroable::zeroed();
@@ -363,10 +438,44 @@ impl GpuMultibodySet {
             all_dof_armature.extend_from_slice(&per_env_dof_armature[i]);
             let pad = (dofs_cap as usize).saturating_sub(per_env_dof_armature[i].len());
             all_dof_armature.resize(all_dof_armature.len() + pad, 0.0);
+            all_dof_stiffness.extend_from_slice(&per_env_dof_stiffness[i]);
+            let pad = (dofs_cap as usize).saturating_sub(per_env_dof_stiffness[i].len());
+            all_dof_stiffness.resize(all_dof_stiffness.len() + pad, 0.0);
+            all_dof_spring_ref.extend_from_slice(&per_env_dof_spring_ref[i]);
+            let pad = (dofs_cap as usize).saturating_sub(per_env_dof_spring_ref[i].len());
+            all_dof_spring_ref.resize(all_dof_spring_ref.len() + pad, 0.0);
+            all_dof_kinematic.extend_from_slice(&per_env_dof_kinematic[i]);
+            let pad = (dofs_cap as usize).saturating_sub(per_env_dof_kinematic[i].len());
+            all_dof_kinematic.resize(all_dof_kinematic.len() + pad, 0.0);
+            all_couplings.extend_from_slice(&per_env_couplings[i]);
+            let pad = (couplings_cap as usize).saturating_sub(per_env_couplings[i].len());
+            all_couplings.resize(all_couplings.len() + pad, MbDofCoupling::default());
         }
 
         let storage = BufferUsages::STORAGE | BufferUsages::COPY_DST;
-        let usage_u = storage | BufferUsages::UNIFORM;
+
+        // Batch-interleaved (batch-minor) layout for the
+        // dynamics buffers: element `k` of batch `b` lives at `k · nb + b`.
+        fn interleave<T: Copy>(data: &[T], cap: u32, nb: usize) -> Vec<T> {
+            let cap = cap as usize;
+            let mut out = Vec::with_capacity(data.len());
+            for k in 0..cap {
+                for b in 0..nb {
+                    out.push(data[b * cap + k]);
+                }
+            }
+            out
+        }
+        let nb = num_batches as usize;
+        let all_infos = interleave(&all_infos, mb_cap, nb);
+        let all_statics = interleave(&all_statics, links_cap, nb);
+        let all_dof_vals = interleave(&all_dof_vals, dofs_cap, nb);
+        let all_dof_vels = interleave(&all_dof_vels, dofs_cap, nb);
+        let all_dof_damping = interleave(&all_dof_damping, dofs_cap, nb);
+        let all_dof_armature = interleave(&all_dof_armature, dofs_cap, nb);
+        let all_dof_stiffness = interleave(&all_dof_stiffness, dofs_cap, nb);
+        let all_dof_spring_ref = interleave(&all_dof_spring_ref, dofs_cap, nb);
+        let all_dof_kinematic = interleave(&all_dof_kinematic, dofs_cap, nb);
 
         Self {
             num_batches,
@@ -378,26 +487,39 @@ impl GpuMultibodySet {
             mass_matrix_entries_per_batch: mm_cap,
             coriolis_entries_per_batch: cor_cap,
             i_coriolis_dt_entries_per_batch: icdt_cap,
-            implicit_coriolis: true,
+            // Rapier's scheme (the default): the coriolis-melded matrix only
+            // drives the free-acceleration solve, constraints see the plain
+            // matrix, and everything is built once per step. The implicit
+            // legacy mode (single melded matrix, per-substep rebuilds) stays
+            // available via `set_implicit_coriolis(true)`.
+            implicit_coriolis: false,
             has_joint_constraints: all_infos.iter().any(|info| info.max_constraints > 0),
 
             multibody_info: Tensor::vector(backend, &all_infos, storage).unwrap(),
             links_static: Tensor::vector(backend, &all_statics, storage | BufferUsages::COPY_DST)
                 .unwrap(),
             links_static_mirror: all_statics.clone(),
-            links_workspace: Tensor::vector(backend, &all_ws, storage).unwrap(),
+            links_workspace: Tensor::vector(
+                backend,
+                &crate::shaders::dynamics::ws_soa_from_structs(&all_ws, links_cap, num_batches),
+                storage,
+            )
+            .unwrap(),
             dof_values: Tensor::vector(backend, &all_dof_vals, storage).unwrap(),
             dof_state: {
-                // Pack [velocities (N), damping (N), armature (N)] back-to-back
-                // where N = dofs_cap * num_batches. The damping section starts at
-                // `N` (`dof_damping_section_offset`) and the armature section at
-                // `2·N` (computed in-shader as `2 · dof_damping_section_offset`).
+                // Pack [velocities, damping, armature, spring stiffness,
+                // spring rest, kinematic mask] back-to-back, each section
+                // N = dofs_cap * num_batches long. The shaders address
+                // section `s` at intra-batch offset `s · dof_batch_capacity`.
                 let n = (dofs_cap * num_batches) as usize;
-                let mut buf = Vec::with_capacity(3 * n);
+                let mut buf = Vec::with_capacity(6 * n);
                 buf.extend_from_slice(&all_dof_vels);
                 buf.extend_from_slice(&all_dof_damping);
                 buf.extend_from_slice(&all_dof_armature);
-                debug_assert_eq!(buf.len(), 3 * n);
+                buf.extend_from_slice(&all_dof_stiffness);
+                buf.extend_from_slice(&all_dof_spring_ref);
+                buf.extend_from_slice(&all_dof_kinematic);
+                debug_assert_eq!(buf.len(), 6 * n);
                 Tensor::vector(backend, &buf, storage).unwrap()
             },
             gen_forces: Tensor::vector(
@@ -414,7 +536,10 @@ impl GpuMultibodySet {
             .unwrap(),
             mass_matrices: Tensor::vector(
                 backend,
-                vec![0.0f32; (mm_cap * num_batches) as usize],
+                // Two sections: the plain matrix (constraint LU) at intra-batch
+                // offset 0 and the coriolis-melded acceleration matrix at intra-batch
+                // offset `mm_cap`.
+                vec![0.0f32; (2 * mm_cap * num_batches) as usize],
                 storage,
             )
             .unwrap(),
@@ -444,6 +569,8 @@ impl GpuMultibodySet {
                 storage,
             )
             .unwrap(),
+            dof_couplings: Tensor::vector(backend, &all_couplings, storage).unwrap(),
+            couplings_per_batch: couplings_cap,
             body_to_link: Tensor::vector(backend, &all_body_to_link, storage).unwrap(),
             contact_constraints: Tensor::vector(
                 backend,
@@ -466,6 +593,30 @@ impl GpuMultibodySet {
                 storage,
             )
             .unwrap(),
+            // Per-multibody Delassus blocks for the constraint-space contact
+            // sweep: MAX_MB_CONTACT_CONSTRAINTS_PER_MB² floats each (147 KB
+            // in 3D), so only small total multibody counts get them; larger
+            // batched scenes keep the dof-space solve.
+            contact_delassus: {
+                // Sized by the capacity stride (the kernels index blocks by
+                // `batch · multibodies_batch_capacity + mb_idx`).
+                let total_mbs = mb_cap * num_batches;
+                if global_max_mb > 0 && total_mbs <= MAX_DELASSUS_MULTIBODIES {
+                    let block = (MAX_MB_CONTACT_CONSTRAINTS_PER_MB
+                        * MAX_MB_CONTACT_CONSTRAINTS_PER_MB)
+                        as usize;
+                    Some(
+                        Tensor::vector_uninit(
+                            backend,
+                            (total_mbs as usize * block) as u32,
+                            storage,
+                        )
+                        .unwrap(),
+                    )
+                } else {
+                    None
+                }
+            },
 
             // Impulse-joint buffers are sized for "no MB-touching joints" by
             // default — `set_impulse_joints` resizes them at pipeline build
@@ -506,9 +657,10 @@ impl GpuMultibodySet {
                 storage,
             )
             .unwrap(),
-            mb_imp_joint_curr_color: Tensor::scalar(backend, 0u32, usage_u).unwrap(),
             mb_imp_joint_num_colors: 0,
             mb_imp_joint_max_color_group_len: 0,
+            max_ndofs: max_mb_ndofs,
+            max_links: max_mb_links,
             joint_constraints_per_batch: cons_cap,
             joint_constraint_columns_per_batch: cons_col_cap,
             contact_constraints_per_batch: contact_cons_cap,
diff --git a/src_rbd/dynamics/multibody/multibody_set.rs b/src_rbd/dynamics/multibody/multibody_set.rs
index 89734fe..9693f5f 100644
--- a/src_rbd/dynamics/multibody/multibody_set.rs
+++ b/src_rbd/dynamics/multibody/multibody_set.rs
@@ -3,9 +3,9 @@
 
 use crate::math::Pose;
 use crate::shaders::dynamics::{
-    ConstraintSoftness, LocalMassProperties, MbImpulseJointBuilder, MbImpulseJointConstraint,
-    MultibodyContactConstraint, MultibodyInfo, MultibodyJointConstraint, MultibodyLinkStatic,
-    MultibodyLinkWorkspace, RbdSimParams,
+    ConstraintSoftness, LocalMassProperties, MbDofCoupling, MbImpulseJointBuilder,
+    MbImpulseJointConstraint, MultibodyContactConstraint, MultibodyInfo,
+    MultibodyJointConstraint, MultibodyLinkStatic, MultibodyLinkWorkspace, RbdSimParams,
 };
 use crate::shaders::utils::BatchIndices;
 use glamx::Vec4;
@@ -19,6 +19,12 @@ use vortx::tensor::Tensor;
 /// `gpu_mb_lu_solve`.
 pub(super) const MB_LU_LANES: u32 = 64;
 
+/// Maximum total multibody count (capacity × batches) for which the
+/// constraint-space (Delassus) contact solve is enabled: each multibody's
+/// Delassus block costs `MAX_MB_CONTACT_CONSTRAINTS_PER_MB²` floats (~147 KB
+/// in 3D), so huge batched scenes would run out of memory.
+pub(super) const MAX_DELASSUS_MULTIBODIES: u32 = 128;
+
 use crate::shaders::dynamics::{GenericJoint, JointLimits, JointMotor};
 
 /// GPU-resident articulated multibody set, packed across simulation batches.
@@ -39,9 +45,6 @@ pub struct GpuMultibodySet {
     pub(super) mass_matrix_entries_per_batch: u32,
     pub(super) coriolis_entries_per_batch: u32,
     pub(super) i_coriolis_dt_entries_per_batch: u32,
-    /// When `true`, the Coriolis / gyroscopic terms are folded into the mass
-    /// matrix (implicit integration). When `false`, they are applied explicitly
-    /// as part of the RHS.
     pub(super) implicit_coriolis: bool,
     /// When `false` (no joint limits / motors anywhere), the joint constraint
     /// kernel chain is skipped on the host side.
@@ -54,8 +57,8 @@ pub struct GpuMultibodySet {
     /// CPU-side mirror of [`Self::links_static`] used to support runtime
     /// mutations like motor changes without round-tripping through a GPU read.
     pub(super) links_static_mirror: Vec<MultibodyLinkStatic>,
-    /// Per-batch per-step link workspace.
-    pub(super) links_workspace: Tensor<MultibodyLinkWorkspace>,
+    /// Per-batch per-step link workspace, SoA quad layout.
+    pub(super) links_workspace: Tensor<glamx::Vec4>,
     /// Generalized coordinates (flat).
     pub(super) dof_values: Tensor<f32>,
     /// Packed buffer holding generalized velocities (offset 0) and per-DOF
@@ -78,6 +81,12 @@ pub struct GpuMultibodySet {
     pub(super) joint_constraints: Tensor<MultibodyJointConstraint>,
     /// Per-constraint columns of `M⁻¹` (length `ndofs` each, contiguous per multibody).
     pub(super) joint_constraint_columns: Tensor<f32>,
+    /// Per-batch slab of DoF couplings (rapier's `MultibodyDofCoupling`),
+    /// batch-major; each multibody's slice is
+    /// `[first_coupling, first_coupling + num_couplings)`.
+    pub(super) dof_couplings: Tensor<MbDofCoupling>,
+    /// Per-batch capacity (stride) of [`Self::dof_couplings`].
+    pub(super) couplings_per_batch: u32,
 
     /// Per-body lookup `[multibody_idx, link_idx]` (`u32::MAX` sentinel for
     /// free / non-multibody bodies). Indexed by the per-batch local body id.
@@ -91,6 +100,10 @@ pub struct GpuMultibodySet {
     pub(super) contact_constraint_jacs: Tensor<f32>,
     /// Per-constraint M⁻¹·Jᵀ column (length `ndofs`).
     pub(super) contact_constraint_columns: Tensor<f32>,
+    /// Per-multibody Delassus blocks (`MAX_MB_CONTACT_CONSTRAINTS_PER_MB²`
+    /// floats each) only allocated when the total multibody count is at most
+    /// [`MAX_DELASSUS_MULTIBODIES`].
+    pub(super) contact_delassus: Option<Tensor<f32>>,
 
     /// Per-batch number of multibody-touching impulse joints (body1 OR body2
     /// part of any multibody).
@@ -112,10 +125,15 @@ pub struct GpuMultibodySet {
     /// Per-batch prefix-sum over the color-sorted `mb_imp_joint_builders`.
     /// Built at init time by `set_impulse_joints` (greedy graph coloring).
     pub(super) mb_imp_joint_color_groups: Tensor<u32>,
-    /// Scalar color cursor incremented by the host color loop.
-    pub(super) mb_imp_joint_curr_color: Tensor<u32>,
-    /// Number of colors (host color-loop trip count). CPU mirror.
-    pub(super) mb_imp_joint_num_colors: u32,
+    /// Number of colors (per-batch stride of `mb_imp_joint_color_groups`,
+    /// and the host color-loop trip count). CPU mirror.
+    pub(crate) mb_imp_joint_num_colors: u32,
+    /// Max `ndofs` across every multibody in every batch (CPU mirror of
+    /// `BatchIndices::mb_max_ndofs`).
+    pub(super) max_ndofs: u32,
+    /// Max link count across every multibody in every batch (CPU mirror of
+    /// `BatchIndices::mb_max_links`).
+    pub(super) max_links: u32,
     /// Largest color group across batches — the per-color dispatch width.
     pub(super) mb_imp_joint_max_color_group_len: u32,
     /// Per-batch capacities of the joint / contact constraint slabs (CPU-side
@@ -148,15 +166,48 @@ impl GpuMultibodySet {
         self.multibodies_per_batch
     }
 
-    /// True if the set contains no multibodies in any batch.
+    /// Thread-count grid for the per-multibody kernels, with `(multibody,
+    /// batch)` flattened into X. The kernels decode
+    /// `batch_id = x / multibodies_len`, `mb_idx = x % multibodies_len`.
+    pub(crate) fn flat_mb_dispatch(&self) -> [u32; 3] {
+        [self.num_active_multibodies * self.num_batches, 1, 1]
+    }
+
+    /// Lanes per multibody for the packed per-multibody dynamics kernels
+    /// (`compute_dynamics_pre`, `gravity_and_lu`).
+    pub(crate) fn pack_lanes(&self) -> u32 {
+        let total_mb = self.num_active_multibodies * self.num_batches;
+        if self.max_ndofs <= 8 && total_mb >= 1024 {
+            1
+        } else {
+            self.max_ndofs.next_power_of_two().clamp(8, MB_LU_LANES)
+        }
+    }
+
+    /// Thread-count grid for the packed per-multibody workgroup kernels
+    /// (`compute_dynamics_pre`, `gravity_and_lu`): `64 / pack_lanes`
+    /// multibodies per 64-lane workgroup, flattened `(multibody, batch)`.
+    pub(crate) fn packed_wg_dispatch(&self) -> [u32; 3] {
+        let slots = MB_LU_LANES / self.pack_lanes();
+        let total = self.num_active_multibodies * self.num_batches;
+        [total.div_ceil(slots) * MB_LU_LANES, 1, 1]
+    }
+
+    /// True if the set contains no active multibodies in any batch.
     pub fn is_empty(&self) -> bool {
-        self.multibodies_per_batch == 0 || self.links_per_batch == 0
+        self.num_active_multibodies == 0 || self.links_per_batch == 0
     }
 
-    /// GPU buffer holding generalized velocities followed by per-DOF damping.
-    /// The velocity section is `[0, dof_batch_capacity * num_batches)`; the
-    /// damping section follows. Callers reading velocities should use only the
-    /// first half.
+    /// Number of colors used by the colored multibody impulse-joint sweeps.
+    pub fn mb_imp_joint_num_colors(&self) -> u32 {
+        self.mb_imp_joint_num_colors
+    }
+
+    /// GPU buffer holding six back-to-back per-DOF sections of
+    /// `dof_batch_capacity * num_batches` floats each: generalized
+    /// velocities, damping, armature, spring stiffness, spring rest position,
+    /// and the kinematic-DOF mask. Callers reading velocities should use only
+    /// the first section.
     pub fn dof_state(&self) -> &Tensor<f32> {
         &self.dof_state
     }
@@ -172,9 +223,7 @@ impl GpuMultibodySet {
         &self.gen_forces
     }
 
-    /// Enable implicit integration of the Coriolis / gyroscopic terms. Implicit
-    /// treatment stabilizes the integrator at large time-steps; the explicit
-    /// form is slightly cheaper but can become unstable for fast rotations.
+    /// Indicates if the implicit treatment of coriolis forces is enabled.
     pub fn set_implicit_coriolis(&mut self, enabled: bool) {
         self.implicit_coriolis = enabled;
     }
@@ -238,8 +287,9 @@ impl GpuMultibodySet {
         axis: JointAxis,
         target_vel: f32,
     ) -> Result<(), GpuBackendError> {
-        let stride = self.links_per_batch;
-        let global_idx = (batch * stride + link_id) as usize;
+        // Batch-interleaved links layout: element `link_id` of batch `batch`
+        // lives at `link_id · num_batches + batch` (mirror included).
+        let global_idx = (link_id * self.num_batches + batch) as usize;
         let axis_id = axis as usize;
         let entry = match self.links_static_mirror.get_mut(global_idx) {
             Some(e) => e,
@@ -292,9 +342,18 @@ impl GpuMultibodySet {
         dst.mb_imp_joint_constraints_batch_capacity = self.mb_imp_joint_constraints_per_batch;
         dst.mb_imp_joint_jacobians_batch_capacity = self.mb_imp_joint_jacobians_per_batch;
         dst.mb_imp_joint_color_groups_batch_capacity = self.mb_imp_joint_num_colors.max(1);
+        dst.mb_max_ndofs = self.max_ndofs;
+        dst.mb_max_links = self.max_links;
+        dst.mb_pack_lanes = self.pack_lanes();
         dst.coriolis_w_section_offset = self.coriolis_entries_per_batch * self.num_batches;
         dst.i_coriolis_dt_section_offset = 2 * self.coriolis_entries_per_batch * self.num_batches;
         dst.dof_damping_section_offset = self.dofs_per_batch * self.num_batches;
+        dst.mass_matrix_acc_section_offset = if self.implicit_coriolis {
+            0
+        } else {
+            self.mass_matrix_entries_per_batch
+        };
+        dst.mb_dof_couplings_batch_capacity = self.couplings_per_batch;
     }
 
     /// Upload a new integration timestep.
@@ -350,10 +409,6 @@ pub(super) fn convert_generic_joint(j: crate::rapier::dynamics::GenericJoint) ->
     }
 }
 
-//
-// Zero-initialised workspaces would leave `joint_rot` as the all-zero quaternion, which
-// is not a valid rotation — seed it with the identity instead.
-//
 pub(super) fn make_workspace_init() -> MultibodyLinkWorkspace {
     let mut w: MultibodyLinkWorkspace = bytemuck::Zeroable::zeroed();
     w.joint_rot = glamx::Quat::IDENTITY;
diff --git a/src_rbd/dynamics/multibody/multibody_solver.rs b/src_rbd/dynamics/multibody/multibody_solver.rs
index 20ef22f..0c9a408 100644
--- a/src_rbd/dynamics/multibody/multibody_solver.rs
+++ b/src_rbd/dynamics/multibody/multibody_solver.rs
@@ -4,14 +4,16 @@ use super::multibody_set::*;
 use crate::math::Pose;
 use crate::queries::GpuIndexedContact;
 use crate::shaders::dynamics::{
-    GpuIncJointColor, GpuMbComputeDynamicsPre, GpuMbComputeDynamicsWithoutCoriolisPre,
-    GpuMbFinalizeContactConstraints, GpuMbFinalizeImpulseJointConstraints, GpuMbGravityAndLu,
-    GpuMbInitContactConstraints, GpuMbInitJointConstraints, GpuMbIntegrate,
-    GpuMbIntegrateVelocities, GpuMbRemoveContactConstraintBias,
-    GpuMbRemoveImpulseJointConstraintBias, GpuMbRemoveSolveJointNoBias, GpuMbResetContactWarmstart,
-    GpuMbSolveContactConstraints, GpuMbSolveImpulseJointConstraints, GpuMbSolveJointConstraints,
-    GpuMbUpdateImpulseJointConstraints, GpuMbWarmstartContactConstraints, GpuResetJointColor,
-    Velocity, WorldMassProperties,
+    GpuMbBuildContactDelassus, GpuMbComputeDynamicsPre,
+    GpuMbFinalizeContactConstraints, GpuMbGravityAndLu, GpuMbGravityAndLuT1, GpuMbGravityAndLuT8,
+    GpuMbGravityAndLuT16, GpuMbGravityAndLuT32, GpuMbInitContactConstraints,
+    GpuMbInitJointConstraints, GpuMbIntegrate, GpuMbIntegrateVelocities,
+    GpuMbRemoveImpulseJointConstraintBias,
+    GpuMbResetContactWarmstart, GpuMbStashContactsLen, GpuMbWarmstartContactConstraints,
+    GpuMbSolveConstraints, GpuMbSolveContactsDelassus, GpuMbSolveImpulseJointConstraints,
+    GpuMbSolveJoints,
+    GpuMbFinalizeImpulseJointConstraints,
+    GpuMbUpdateImpulseJointConstraints, Velocity, WorldMassProperties,
 };
 use crate::shaders::utils::BatchIndices;
 use khal::Shader;
@@ -21,30 +23,45 @@ use vortx::tensor::Tensor;
 /// GPU shader bundle for multibody dynamics.
 #[derive(Shader)]
 pub struct GpuMultibodySolver {
+    // Five variants of the same function, selected by the per-batch DOF
+    // count, mainly to optimize small robots simulated across thousands
+    // of batches.
     gravity_and_lu: GpuMbGravityAndLu,
+    gravity_and_lu_t1: GpuMbGravityAndLuT1,
+    gravity_and_lu_t8: GpuMbGravityAndLuT8,
+    gravity_and_lu_t16: GpuMbGravityAndLuT16,
+    gravity_and_lu_t32: GpuMbGravityAndLuT32,
     compute_dynamics_pre: GpuMbComputeDynamicsPre,
-    compute_dynamics_without_coriolis_pre: GpuMbComputeDynamicsWithoutCoriolisPre,
-    solve_joint_with_bias: GpuMbSolveJointConstraints,
     init_joint_with_bias: GpuMbInitJointConstraints,
-    /// Fused remove-bias + solve-without-bias for the stabilization sweep.
-    remove_solve_joint_no_bias: GpuMbRemoveSolveJointNoBias,
     init_contact_constraints: GpuMbInitContactConstraints,
     finalize_contact_constraints: GpuMbFinalizeContactConstraints,
-    solve_contact_constraints: GpuMbSolveContactConstraints,
+    /// Fused joint+contact PGS sweep (one workgroup per multibody, shared-
+    /// memory dof velocities).
+    solve_constraints: GpuMbSolveConstraints,
+    /// Joint-only half of the sweep, used with the Delassus contact path
+    /// (one kernel binding both joint and Delassus buffers would exceed the
+    /// 8-storage-buffer budget).
+    solve_joints: GpuMbSolveJoints,
+    /// Fills the per-multibody Delassus blocks (`D = J M⁻¹ Jᵀ` + free-body
+    /// coupling) right after the contact columns are finalized.
+    build_contact_delassus: GpuMbBuildContactDelassus,
+    /// Constraint-space contact sweep: `a = J·u` tracked incrementally in
+    /// shared memory via the Delassus rows, breaking the per-iteration
+    /// dof-space latency chain.
+    solve_contacts_delassus: GpuMbSolveContactsDelassus,
     /// Zero the accumulated contact impulses once per frame (warmstart reset).
     reset_contact_warmstart: GpuMbResetContactWarmstart,
+    /// Copy `contacts_len[batch]` into each `MultibodyInfo` once per step so
+    /// `init_contact_constraints` (at the 8-storage-buffer limit) can bound
+    /// its manifold scan by the actual count instead of the capacity.
+    stash_contacts_len: GpuMbStashContactsLen,
     /// Re-apply the accumulated contact impulse each substep (warmstart).
     warmstart_contact_constraints: GpuMbWarmstartContactConstraints,
-    remove_contact_constraint_bias: GpuMbRemoveContactConstraintBias,
     update_impulse_joint_constraints: GpuMbUpdateImpulseJointConstraints,
     /// Finalize pass for the impulse-joint build (LU back-solve + `inv_lhs`),
     /// split out so the build pass fits 8 storage buffers.
     finalize_impulse_joint_constraints: GpuMbFinalizeImpulseJointConstraints,
     solve_impulse_joint_constraints: GpuMbSolveImpulseJointConstraints,
-    /// Color cursor reset / increment for the colored impulse-joint solve
-    /// loop. Reuses the free-body joint color kernels (generic `&mut u32`).
-    reset_imp_joint_color: GpuResetJointColor,
-    inc_imp_joint_color: GpuIncJointColor,
     remove_impulse_joint_constraint_bias: GpuMbRemoveImpulseJointConstraintBias,
     integrate_velocities: GpuMbIntegrateVelocities,
     integrate: GpuMbIntegrate,
@@ -71,6 +88,12 @@ pub struct MultibodySolverArgs<'a> {
     /// Shared `BatchIndices` uniform — per-batch caps and packed-section
     /// offsets read by every multibody kernel. Owned by `RbdState`.
     pub batch_indices: &'a Tensor<BatchIndices>,
+    /// Per-color-index uniform tensors (`color_uniforms[c]` holds `c`),
+    /// shared with the contact/joint solvers.
+    pub color_uniforms: &'a [Tensor<u32>],
+    /// GPU-written workgroup grid for the per-multibody contact-constraint
+    /// dispatches: `[multibodies_batch_capacity, num_batches, 1]`.
+    pub mb_sweep_indirect: &'a Tensor<[u32; 3]>,
 }
 
 impl GpuMultibodySolver {
@@ -83,67 +106,11 @@ impl GpuMultibodySolver {
         mb: &mut GpuMultibodySet,
         args: MultibodySolverArgs<'_>,
     ) -> Result<(), GpuBackendError> {
+        let mut args = args;
         if mb.is_empty() {
             return Ok(());
         }
-        // Fused FK + body-jacobians + velocity propagation + CRBA-with-Coriolis
-        // mass-matrix assembly (4 dispatches → 1) — see
-        // `gpu_mb_compute_dynamics_pre`. Only the implicit-Coriolis path is
-        // wired through the fused kernel; the explicit-Coriolis fallback keeps
-        // the legacy split path.
-        let pre_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
-        if mb.implicit_coriolis {
-            self.compute_dynamics_pre.call(
-                pass,
-                pre_dispatch,
-                &mb.multibody_info,
-                &mb.links_static,
-                &mut mb.links_workspace,
-                args.poses,
-                &mut mb.body_jacobians,
-                &mut mb.mass_matrices,
-                &mut mb.coriolis_packed,
-                &mb.dof_state,
-                &mb.dt,
-                args.batch_indices,
-            )?;
-        } else {
-            self.compute_dynamics_without_coriolis_pre.call(
-                pass,
-                pre_dispatch,
-                &mb.multibody_info,
-                &mb.links_static,
-                &mut mb.links_workspace,
-                args.poses,
-                &mut mb.body_jacobians,
-                &mut mb.mass_matrices,
-                &mb.dof_state,
-                &mb.dt,
-                args.batch_indices,
-            )?;
-        }
-
-        // Fused: gravity / Coriolis force assembly + LU factor + LU solve in
-        // a single dispatch. Replaces the previous 2-dispatch chain
-        // (apply_gravity_with_coriolis → lu_factor_and_solve) — drops one
-        // WebGPU dispatch per `compute_dynamics` call.
-        let grav_lu_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
-        self.gravity_and_lu.call(
-            pass,
-            grav_lu_dispatch,
-            &mb.multibody_info,
-            &mb.links_static,
-            &mut mb.links_workspace,
-            &mb.body_jacobians,
-            &mut mb.gen_forces,
-            &mut mb.mass_matrices,
-            &mut mb.lu_pivots,
-            &mb.dof_state,
-            &mb.gravity,
-            args.batch_indices,
-        )?;
-
-        Ok(())
+        self.compute_dynamics(pass, mb, &mut args)
     }
 
     /// Once-per-visible-step setup. After this call, `gen_forces` holds the
@@ -152,23 +119,55 @@ impl GpuMultibodySolver {
     /// the last call carrying `is_last_substep = true`.
     pub fn init_step(
         &self,
-        pass: &mut GpuPass,
+        encoder: &mut khal::backend::GpuEncoder,
+        mut timestamps: Option<&mut khal::backend::GpuTimestamps>,
         mb: &mut GpuMultibodySet,
         args: &mut MultibodySolverArgs<'_>,
     ) -> Result<(), GpuBackendError> {
+        use khal::backend::Encoder;
         if mb.is_empty() {
             return Ok(());
         }
         // Zero the accumulated contact impulses so the first substep's warmstart
         // starts cold (within a frame they are then preserved across substeps).
-        self.reset_contact_warmstart.call(
+        // Flat (slot, multibody, batch) grid (impulse-field-only stores).
+        {
+            let mut pass = encoder.begin_pass("[RBD] mbi/reset", timestamps.as_deref_mut());
+            let total_slots = mb.num_active_multibodies
+                * mb.num_batches
+                * crate::shaders::dynamics::MAX_MB_CONTACT_CONSTRAINTS_PER_MB;
+            self.reset_contact_warmstart.call(
+                &mut pass,
+                [total_slots, 1, 1],
+                &mut mb.contact_constraints,
+                args.batch_indices,
+            )?;
+        }
+        let mut pass = encoder.begin_pass("[RBD] mbi/dynamics", timestamps.as_deref_mut());
+        self.compute_dynamics(&mut pass, mb, args)
+    }
+
+    /// Copy `contacts_len[batch]` into each `MultibodyInfo`.
+    ///
+    /// This is a workaround for kernels that are already at the 8-storage-binding
+    /// web limit and could therefore not bind `contacts_len`.
+    pub fn stash_contacts_len(
+        &self,
+        pass: &mut GpuPass,
+        mb: &mut GpuMultibodySet,
+        args: &mut MultibodySolverArgs<'_>,
+    ) -> Result<(), GpuBackendError> {
+        if mb.is_empty() {
+            return Ok(());
+        }
+        self.stash_contacts_len.call(
             pass,
-            [mb.multibodies_per_batch, mb.num_batches, 1],
-            &mb.multibody_info,
-            &mut mb.contact_constraints,
+            mb.flat_mb_dispatch(),
+            &mut mb.multibody_info,
+            args.contacts_len,
             args.batch_indices,
         )?;
-        self.compute_dynamics(pass, mb, args)
+        Ok(())
     }
 
     // Per-substep work is split into five phases so the pipeline can interleave
@@ -186,7 +185,7 @@ impl GpuMultibodySolver {
         if mb.is_empty() {
             return Ok(());
         }
-        let dispatch = [mb.multibodies_per_batch, mb.num_batches, 1];
+        let dispatch = mb.flat_mb_dispatch();
         self.integrate_velocities.call(
             pass,
             dispatch,
@@ -202,26 +201,66 @@ impl GpuMultibodySolver {
     /// constraints, then warmstart the contacts.
     pub fn substep_build_constraints(
         &self,
-        pass: &mut GpuPass,
+        encoder: &mut khal::backend::GpuEncoder,
+        mut timestamps: Option<&mut khal::backend::GpuTimestamps>,
+        mb: &mut GpuMultibodySet,
+        args: &mut MultibodySolverArgs<'_>,
+        first_substep: bool,
+    ) -> Result<(), GpuBackendError> {
+        use khal::backend::Encoder;
+        if mb.is_empty() {
+            return Ok(());
+        }
+
+        // Full rebuild of the joint + contact constraints every substep.
+        self.build_contact_constraints(encoder, timestamps.as_deref_mut(), mb, args)?;
+
+        // Warmstart: re-apply the accumulated contact impulse to dof_state (and
+        // the free-body solver velocities) so the contact starts "warm" each
+        // substep.
+        // One 64-lane workgroup per multibody (one DOF per lane).
+        if !first_substep {
+            let mut pass =
+                encoder.begin_pass("[RBD] mbb/warmstart-contact", timestamps.as_deref_mut());
+            // Contact-only work: indirect grid collapses to zero workgroups
+            // when no batch has any contact this step.
+            self.warmstart_contact_constraints.call(
+                &mut pass,
+                args.mb_sweep_indirect,
+                &mb.multibody_info,
+                &mb.contact_constraints,
+                &mb.contact_constraint_columns,
+                &mut mb.dof_state,
+                args.solver_vels,
+                args.batch_indices,
+            )?;
+        }
+
+        Ok(())
+    }
+
+    /// Build + finalize the contact constraints (normal + friction slots,
+    /// free-body × multibody and self-contact pairs).
+    pub fn build_contact_constraints(
+        &self,
+        encoder: &mut khal::backend::GpuEncoder,
+        mut timestamps: Option<&mut khal::backend::GpuTimestamps>,
         mb: &mut GpuMultibodySet,
         args: &mut MultibodySolverArgs<'_>,
     ) -> Result<(), GpuBackendError> {
+        use khal::backend::Encoder;
         if mb.is_empty() {
             return Ok(());
         }
-        let dispatch = [mb.multibodies_per_batch, mb.num_batches, 1];
 
+        // Joint limit/motor constraints: one 64-lane workgroup per multibody
+        // (lane 0 emits the metadata serially).
         if mb.has_joint_constraints {
-            // TODO(PERF): joints init could parallelized. We either need to rework
-            //             the flow of the kernel to that the LU parts are not in
-            //             potentially diverging code paths, or we need to have
-            //             each link have its limits/motors generated by a separate
-            //             threadgroups (which might actually be better for lower
-            //             divergence and allow us to theadgroup-parallelize the LU
-            //             solve).
+            let mut pass = encoder.begin_pass("[RBD] mbb/init-joint", timestamps.as_deref_mut());
+            let init_joint_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
             self.init_joint_with_bias.call(
-                pass,
-                dispatch,
+                &mut pass,
+                init_joint_dispatch,
                 &mb.multibody_info,
                 &mb.links_static,
                 &mb.links_workspace,
@@ -229,101 +268,170 @@ impl GpuMultibodySolver {
                 &mb.lu_pivots,
                 &mut mb.joint_constraints,
                 &mut mb.joint_constraint_columns,
+                &mb.dof_couplings,
                 &mb.constraint_softness,
                 args.batch_indices,
             )?;
         }
 
-        // Build + finalize contact constraints (normal-only, free body ×
-        // multibody pairs only). `init` PRESERVES the accumulated impulse across
-        // substeps (zeroed once per frame by `reset_contact_warmstart` in
-        // `init_step`); `finalize` recomputes `inv_lhs` and the M⁻¹Jᵀ columns.
-        self.init_contact_constraints.call(
-            pass,
-            dispatch,
-            &mut mb.multibody_info,
-            &mb.body_jacobians,
-            &mb.body_to_link,
-            &mut mb.contact_constraints,
-            &mut mb.contact_constraint_jacs,
-            &mb.constraint_softness,
-            args.batch_indices,
-            args.mprops,
-            args.collider_world_poses,
-            args.contacts,
-        )?;
+        // One 64-lane workgroup per multibody.
+        {
+            let mut pass =
+                encoder.begin_pass("[RBD] mbb/init-contact", timestamps.as_deref_mut());
+            let init_contact_dispatch =
+                [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
+            self.init_contact_constraints.call(
+                &mut pass,
+                init_contact_dispatch,
+                &mut mb.multibody_info,
+                &mb.body_jacobians,
+                &mb.body_to_link,
+                &mut mb.contact_constraints,
+                &mut mb.contact_constraint_jacs,
+                &mb.constraint_softness,
+                args.batch_indices,
+                args.mprops,
+                args.collider_world_poses,
+                args.contacts,
+            )?;
+        }
 
-        self.finalize_contact_constraints.call(
-            pass,
-            dispatch,
-            &mb.multibody_info,
-            &mb.mass_matrices,
-            &mb.lu_pivots,
-            &mut mb.contact_constraints,
-            &mb.contact_constraint_jacs,
-            &mut mb.contact_constraint_columns,
-            args.batch_indices,
-        )?;
+        // One 64-lane workgroup per multibody.
+        {
+            let mut pass =
+                encoder.begin_pass("[RBD] mbb/finalize-contact", timestamps.as_deref_mut());
+            self.finalize_contact_constraints.call(
+                &mut pass,
+                args.mb_sweep_indirect,
+                &mb.multibody_info,
+                &mb.mass_matrices,
+                &mb.lu_pivots,
+                &mut mb.contact_constraints,
+                &mb.contact_constraint_jacs,
+                &mut mb.contact_constraint_columns,
+                args.batch_indices,
+            )?;
+        }
 
-        // Warmstart: re-apply the accumulated contact impulse to dof_state (and
-        // the free-body solver velocities) so the contact starts "warm" each
-        // substep — mirrors rapier's per-substep `contact_constraints.warmstart`
-        // and matches what the rigid-body solver does for free contacts. On the
-        // first substep the impulse was just reset to 0, so this is a no-op.
-        self.warmstart_contact_constraints.call(
-            pass,
-            dispatch,
-            &mb.multibody_info,
-            &mb.contact_constraints,
-            &mb.contact_constraint_columns,
-            &mut mb.dof_state,
-            args.solver_vels,
-            args.batch_indices,
-        )?;
+        // Delassus blocks for the constraint-space contact sweep (consumes
+        // the columns finalized just above).
+        if let Some(delassus) = &mut mb.contact_delassus {
+            let mut pass =
+                encoder.begin_pass("[RBD] mbb/build-delassus", timestamps.as_deref_mut());
+            self.build_contact_delassus.call(
+                &mut pass,
+                args.mb_sweep_indirect,
+                &mb.multibody_info,
+                &mb.contact_constraints,
+                &mb.contact_constraint_jacs,
+                &mb.contact_constraint_columns,
+                delassus,
+                args.batch_indices,
+            )?;
+        }
 
         Ok(())
     }
 
-    /// P3: one PGS sweep WITH bias over the joint, contact, and multibody-
-    /// touching impulse-joint constraints.
-    pub fn substep_solve_with_bias(
+    /// One joint+contact PGS sweep: the dof-space fused kernel, or (when the
+    /// Delassus blocks are allocated) the joint-only kernel followed by the
+    /// constraint-space contact kernel. `use_bias_idx` indexes
+    /// `color_uniforms` (0 or 1, holding those constants).
+    fn dispatch_solve(
         &self,
         pass: &mut GpuPass,
         mb: &mut GpuMultibodySet,
         args: &mut MultibodySolverArgs<'_>,
+        solve_dispatch: [u32; 3],
+        use_bias_idx: usize,
     ) -> Result<(), GpuBackendError> {
-        if mb.is_empty() {
-            return Ok(());
-        }
-        let dispatch = [mb.multibodies_per_batch, mb.num_batches, 1];
-
-        if mb.has_joint_constraints {
-            self.solve_joint_with_bias.call(
+        let use_bias = &args.color_uniforms[use_bias_idx];
+        if let Some(delassus) = &mb.contact_delassus {
+            if mb.has_joint_constraints {
+                self.solve_joints.call(
+                    pass,
+                    solve_dispatch,
+                    &mb.multibody_info,
+                    &mut mb.joint_constraints,
+                    &mb.joint_constraint_columns,
+                    &mut mb.dof_state,
+                    use_bias,
+                    args.batch_indices,
+                )?;
+            }
+            // Contact-only work: indirect grid collapses to zero workgroups
+            // on contact-free steps.
+            self.solve_contacts_delassus.call(
+                pass,
+                args.mb_sweep_indirect,
+                &mb.multibody_info,
+                &mut mb.contact_constraints,
+                &mb.contact_constraint_jacs,
+                &mb.contact_constraint_columns,
+                delassus,
+                use_bias,
+                args.batch_indices,
+                &mut mb.dof_state,
+                args.solver_vels,
+            )?;
+        } else if mb.has_joint_constraints {
+            self.solve_constraints.call(
                 pass,
-                dispatch,
+                solve_dispatch,
                 &mb.multibody_info,
                 &mut mb.joint_constraints,
-                &mut mb.joint_constraint_columns,
+                &mb.joint_constraint_columns,
+                &mut mb.contact_constraints,
+                &mb.contact_constraint_jacs,
+                &mb.contact_constraint_columns,
+                use_bias,
+                args.batch_indices,
                 &mut mb.dof_state,
+                args.solver_vels,
+            )?;
+        } else {
+            // No joint limits/motors anywhere: the fused sweep is contact-only
+            // work, so the indirect grid (zero workgroups on contact-free
+            // steps) replaces the full per-(multibody, batch) launch.
+            self.solve_constraints.call(
+                pass,
+                args.mb_sweep_indirect,
+                &mb.multibody_info,
+                &mut mb.joint_constraints,
+                &mb.joint_constraint_columns,
+                &mut mb.contact_constraints,
+                &mb.contact_constraint_jacs,
+                &mb.contact_constraint_columns,
+                use_bias,
                 args.batch_indices,
+                &mut mb.dof_state,
+                args.solver_vels,
             )?;
         }
+        Ok(())
+    }
 
-        self.solve_contact_constraints.call(
-            pass,
-            dispatch,
-            &mb.multibody_info,
-            &mut mb.contact_constraints,
-            &mb.contact_constraint_jacs,
-            &mb.contact_constraint_columns,
-            &mut mb.dof_state,
-            args.solver_vels,
-            args.batch_indices,
-        )?;
+    /// P3: one PGS sweep with bias over the joint, contact, and multibody-
+    /// touching impulse-joint constraints.
+    pub fn substep_solve_with_bias(
+        &self,
+        pass: &mut GpuPass,
+        mb: &mut GpuMultibodySet,
+        args: &mut MultibodySolverArgs<'_>,
+    ) -> Result<(), GpuBackendError> {
+        if mb.is_empty() {
+            return Ok(());
+        }
+
+        // One 64-lane workgroup per multibody with the generalized velocities
+        // held in workgroup memory (`color_uniforms[1]` holds the constant
+        // 1 = use_bias). With the Delassus blocks allocated, the contact half
+        // runs in constraint space instead (joints first, same order).
+        let solve_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
+        self.dispatch_solve(pass, mb, args, solve_dispatch, 1)?;
 
         // Multibody-touching impulse joints — generic (rb-mb / mb-mb)
-        // constraints. Mirrors rapier's `JointGenericExternalConstraintBuilder::update`
-        // plus a PGS sweep WITH bias.
+        // constraints.
         if mb.mb_imp_joints_per_batch > 0 {
             let imp_dispatch = [mb.mb_imp_joints_per_batch, mb.num_batches, 1];
             self.update_impulse_joint_constraints.call(
@@ -357,9 +465,7 @@ impl GpuMultibodySolver {
             // Colored PGS sweep WITH bias: one dispatch per color, each
             // color's joints solved race-free in parallel (graph coloring
             // done at init in `set_impulse_joints`).
-            self.reset_imp_joint_color
-                .call(pass, 1u32, &mut mb.mb_imp_joint_curr_color)?;
-            for _ in 0..mb.mb_imp_joint_num_colors {
+            for c in 0..mb.mb_imp_joint_num_colors as usize {
                 self.solve_impulse_joint_constraints.call(
                     pass,
                     // One workgroup (MB_LU_LANES threads) per joint; thread
@@ -374,13 +480,11 @@ impl GpuMultibodySolver {
                     &mb.mb_imp_joint_jacobians,
                     &mb.mb_imp_joint_color_groups,
                     args.batch_indices,
-                    &mb.mb_imp_joint_curr_color,
+                    &args.color_uniforms[c],
                     &mb.multibody_info,
                     &mut mb.dof_state,
                     args.solver_vels,
                 )?;
-                self.inc_imp_joint_color
-                    .call(pass, 1u32, &mut mb.mb_imp_joint_curr_color)?;
             }
         }
 
@@ -400,7 +504,7 @@ impl GpuMultibodySolver {
         if mb.is_empty() {
             return Ok(());
         }
-        let dispatch = [mb.multibodies_per_batch, mb.num_batches, 1];
+        let dispatch = mb.flat_mb_dispatch();
 
         self.integrate.call(
             pass,
@@ -414,14 +518,9 @@ impl GpuMultibodySolver {
             args.batch_indices,
         )?;
 
-        // Recompute `a` for the next substep — orientations / positions just
-        // changed so M and τ are stale. Skipped on the last substep (rapier
-        // skips it too: `if !is_last_substep`).
-        // NOTE: we also only update the mass matrix a single time if running without
-        //       `implicit_coriolis`. This further improves performances as that’s the main
-        //       purpose of disabling the implicit handling of coriolis forces (and makes it
-        //       closer to Mujoco/Genesis).
-        if !is_last_substep && mb.implicit_coriolis {
+        // Recompute the dynamics (FK, mass matrices, LU, accelerations) for
+        // the next substep.
+        if !is_last_substep {
             self.compute_dynamics(pass, mb, args)?;
         }
 
@@ -441,26 +540,10 @@ impl GpuMultibodySolver {
         if mb.is_empty() {
             return Ok(());
         }
-        let dispatch = [mb.multibodies_per_batch, mb.num_batches, 1];
 
-        if mb.has_joint_constraints {
-            self.remove_solve_joint_no_bias.call(
-                pass,
-                dispatch,
-                &mb.multibody_info,
-                &mut mb.joint_constraints,
-                &mb.joint_constraint_columns,
-                &mut mb.dof_state,
-                args.batch_indices,
-            )?;
-        }
-        self.remove_contact_constraint_bias.call(
-            pass,
-            dispatch,
-            &mut mb.contact_constraints,
-            &mb.multibody_info,
-            args.batch_indices,
-        )?;
+        // Stabilization sweep: `use_bias = 0` (`color_uniforms[0] == 0`).
+        let solve_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
+        self.dispatch_solve(pass, mb, args, solve_dispatch, 0)?;
         if mb.mb_imp_joints_per_batch > 0 {
             let imp_dispatch = [mb.mb_imp_joints_per_batch, mb.num_batches, 1];
             self.remove_impulse_joint_constraint_bias.call(
@@ -471,26 +554,9 @@ impl GpuMultibodySolver {
                 &mb.mb_imp_joint_count,
                 args.batch_indices,
             )?;
-        }
-
-        // (joint sweep WITHOUT bias was fused into `remove_solve_joint_no_bias` above.)
-        self.solve_contact_constraints.call(
-            pass,
-            dispatch,
-            &mb.multibody_info,
-            &mut mb.contact_constraints,
-            &mb.contact_constraint_jacs,
-            &mb.contact_constraint_columns,
-            &mut mb.dof_state,
-            args.solver_vels,
-            args.batch_indices,
-        )?;
-        if mb.mb_imp_joints_per_batch > 0 {
             // Final stabilization sweep WITHOUT bias — colored, one
             // dispatch per color (see the with-bias sweep above).
-            self.reset_imp_joint_color
-                .call(pass, 1u32, &mut mb.mb_imp_joint_curr_color)?;
-            for _ in 0..mb.mb_imp_joint_num_colors {
+            for c in 0..mb.mb_imp_joint_num_colors as usize {
                 self.solve_impulse_joint_constraints.call(
                     pass,
                     // One workgroup (MB_LU_LANES threads) per joint; thread
@@ -505,13 +571,11 @@ impl GpuMultibodySolver {
                     &mb.mb_imp_joint_jacobians,
                     &mb.mb_imp_joint_color_groups,
                     args.batch_indices,
-                    &mb.mb_imp_joint_curr_color,
+                    &args.color_uniforms[c],
                     &mb.multibody_info,
                     &mut mb.dof_state,
                     args.solver_vels,
                 )?;
-                self.inc_imp_joint_color
-                    .call(pass, 1u32, &mut mb.mb_imp_joint_curr_color)?;
             }
         }
 
@@ -527,56 +591,72 @@ impl GpuMultibodySolver {
         mb: &mut GpuMultibodySet,
         args: &mut MultibodySolverArgs<'_>,
     ) -> Result<(), GpuBackendError> {
-        // Fused FK + body-jacobians + velocity propagation + Mass-matrix assembly
-        let pre_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
-        if mb.implicit_coriolis {
-            self.compute_dynamics_pre.call(
-                pass,
-                pre_dispatch,
-                &mb.multibody_info,
-                &mb.links_static,
-                &mut mb.links_workspace,
-                args.poses,
-                &mut mb.body_jacobians,
-                &mut mb.mass_matrices,
-                &mut mb.coriolis_packed,
-                &mb.dof_state,
-                &mb.dt,
-                args.batch_indices,
-            )?;
-        } else {
-            self.compute_dynamics_without_coriolis_pre.call(
-                pass,
-                pre_dispatch,
-                &mb.multibody_info,
-                &mb.links_static,
-                &mut mb.links_workspace,
-                args.poses,
-                &mut mb.body_jacobians,
-                &mut mb.mass_matrices,
-                &mb.dof_state,
-                &mb.dt,
-                args.batch_indices,
-            )?;
-        }
-
-        // Fused gravity + LU factor + LU solve.
-        let grav_lu_dispatch = [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
-        self.gravity_and_lu.call(
+        // Fused FK + body-jacobians + velocity propagation + Mass-matrix
+        // assembly. Packed: `64 / mb_pack_lanes` multibodies per workgroup,
+        // flattened (multibody, batch) grid.
+        let pre_dispatch = mb.packed_wg_dispatch();
+        self.compute_dynamics_pre.call(
             pass,
-            grav_lu_dispatch,
+            pre_dispatch,
             &mb.multibody_info,
             &mb.links_static,
             &mut mb.links_workspace,
-            &mb.body_jacobians,
-            &mut mb.gen_forces,
+            args.poses,
+            &mut mb.body_jacobians,
             &mut mb.mass_matrices,
-            &mut mb.lu_pivots,
+            &mut mb.coriolis_packed,
             &mb.dof_state,
-            &mb.gravity,
+            &mb.dt,
             args.batch_indices,
         )?;
 
+        // Fused gravity + LU factor + LU solve. Select an implementation based on how many
+        // environments we can pack on the same workgroup (depending on its dofs).
+        macro_rules! grav_lu {
+            ($kernel:ident) => {
+                self.$kernel.call(
+                    pass,
+                    mb.packed_wg_dispatch(),
+                    &mb.multibody_info,
+                    &mb.links_static,
+                    &mut mb.links_workspace,
+                    &mb.body_jacobians,
+                    &mut mb.gen_forces,
+                    &mut mb.mass_matrices,
+                    &mut mb.lu_pivots,
+                    &mb.dof_state,
+                    &mb.gravity,
+                    args.batch_indices,
+                    &mb.dt,
+                )?
+            };
+        }
+        match mb.pack_lanes() {
+            1 => grav_lu!(gravity_and_lu_t1),
+            8 => grav_lu!(gravity_and_lu_t8),
+            16 => grav_lu!(gravity_and_lu_t16),
+            32 => grav_lu!(gravity_and_lu_t32),
+            _ => {
+                let grav_lu_dispatch =
+                    [mb.multibodies_per_batch * MB_LU_LANES, mb.num_batches, 1];
+                self.gravity_and_lu.call(
+                    pass,
+                    grav_lu_dispatch,
+                    &mb.multibody_info,
+                    &mb.links_static,
+                    &mut mb.links_workspace,
+                    &mb.body_jacobians,
+                    &mut mb.gen_forces,
+                    &mut mb.mass_matrices,
+                    &mut mb.lu_pivots,
+                    &mb.dof_state,
+                    &mb.gravity,
+                    args.batch_indices,
+                    &mb.dt,
+                )?;
+            }
+        }
+
         Ok(())
     }
 }
diff --git a/src_rbd/dynamics/solver.rs b/src_rbd/dynamics/solver.rs
index 3b89840..05cac55 100644
--- a/src_rbd/dynamics/solver.rs
+++ b/src_rbd/dynamics/solver.rs
@@ -11,22 +11,22 @@ use crate::math::Pose;
 use crate::queries::GpuIndexedContact;
 use crate::shaders::dynamics::{
     GpuApplySolverVelsInc, GpuInitSolverBodies, GpuInitSolverVelsInc, GpuIntegrateLinearized,
-    GpuRemoveCfmAndBiasKernel, GpuSolverCleanup, GpuSolverCountConstraints, GpuSolverFinalize,
-    GpuSolverIncColor, GpuSolverInitConstraints, GpuSolverResetColor, GpuSolverSortConstraints,
-    GpuSolverUpdateConstraints, GpuStepGaussSeidel, GpuWarmstart, LocalMassProperties,
-    RbdSimParams, TwoBodyConstraint, TwoBodyConstraintBuilder, Velocity, WorldMassProperties,
+    GpuSolverCleanup, GpuSolverCountConstraints, GpuSolverFinalize,
+    GpuSolverInitConstraints, GpuSolverRefreshRhsWoBias, GpuSolverSortConstraints,
+    GpuSolverUpdateConstraints, GpuStepGaussSeidel, GpuStepGaussSeidelFused, GpuWarmstart,
+    GpuWarmstartFused, GpuWarmstartWithoutColors,
+    LocalMassProperties, RbdSimParams, TwoBodyConstraint, TwoBodyConstraintBuilder, Velocity,
+    WorldMassProperties,
 };
 use crate::utils::{GpuPrefixSum, PrefixSumWorkspace};
 use khal::Shader;
-use khal::backend::{GpuBackend, GpuBackendError, GpuPass};
+use khal::backend::{Encoder, GpuBackend, GpuBackendError, GpuEncoder, GpuPass, GpuTimestamps};
 use vortx::tensor::Tensor;
 
 /// GPU shader bundle for the constraint solver.
 #[derive(Shader)]
 pub struct GpuSolver {
     sort_constraints: GpuSolverSortConstraints,
-    reset_color: GpuSolverResetColor,
-    inc_color: GpuSolverIncColor,
     /// Initializes constraints from contact manifolds.
     init_constraints: GpuSolverInitConstraints,
     /// Companion counting pass to `init_constraints` (split out to keep each
@@ -34,12 +34,24 @@ pub struct GpuSolver {
     count_constraints: GpuSolverCountConstraints,
     /// Updates nonlinear constraint terms during substeps.
     update_constraints: GpuSolverUpdateConstraints,
+    /// Refreshes the unbiased normal rhs from the post-integration poses,
+    /// between the position integration and the no-bias sweeps of each substep.
+    refresh_rhs_wo_bias: GpuSolverRefreshRhsWoBias,
     /// Clears solver velocities and constraint counts.
     cleanup: GpuSolverCleanup,
     /// Applies warmstart impulses from previous frame.
+    #[allow(dead_code)]
     warmstart: GpuWarmstart,
+    /// Applies warmstart impulses from previous frame, without relying on graph coloring.
+    warmstart_without_colors: GpuWarmstartWithoutColors,
     /// Gauss-Seidel iteration step (sequential per color).
     step_gauss_seidel: GpuStepGaussSeidel,
+    /// Fused variant of the colored warmstart sweep: one workgroup per batch
+    /// loops every color internally (barrier between colors). Used when
+    /// per-batch constraint counts are small.
+    warmstart_fused: GpuWarmstartFused,
+    /// Fused variant of the colored Gauss-Seidel sweep (same rationale).
+    step_gauss_seidel_fused: GpuStepGaussSeidelFused,
     /// Initializes solver velocity increments.
     init_solver_vels_inc: GpuInitSolverVelsInc,
     /// Seeds the COM-centered solver poses from the body world poses
@@ -52,8 +64,6 @@ pub struct GpuSolver {
     /// Writes solver velocities and converts the COM-centered solver poses
     /// back to body-origin poses.
     finalize: GpuSolverFinalize,
-    /// Removes CFM and bias terms for velocity-only solving.
-    remove_cfm_and_bias_kernel: GpuRemoveCfmAndBiasKernel,
 }
 
 /// Arguments for constraint solver dispatch, used by [`GpuSolver::prepare`] and
@@ -95,8 +105,6 @@ pub struct SolverArgs<'a> {
     pub vels: &'a mut Tensor<Velocity>,
     /// Solver working velocities.
     pub solver_vels: &'a mut Tensor<Velocity>,
-    /// Solver output velocities (currently unused).
-    pub solver_vels_out: &'a Tensor<Velocity>,
     /// Accumulated velocity increments during substeps.
     pub solver_vels_inc: &'a mut Tensor<Velocity>,
     /// World-space mass properties.
@@ -113,21 +121,37 @@ pub struct SolverArgs<'a> {
     /// All constraints of all the bodies part of the same multibody are in the same list associated
     /// to the multibody’s root.
     pub body_constraint_ids: &'a mut Tensor<u32>,
-    /// Color assigned to each constraint by graph coloring.
-    pub constraints_colors: &'a Tensor<u32>,
-    /// Current color being processed.
-    pub curr_color: &'a mut Tensor<u32>,
+    /// Per-batch per-color exclusive prefix sums over the color-bucketed
+    /// constraint ids (stride `BatchIndices::solver_color_buckets_stride`).
+    pub color_bucket_starts: &'a Tensor<u32>,
+    /// Constraint ids bucket-sorted by color (contacts layout).
+    pub color_sorted_ids: &'a Tensor<u32>,
+    /// Per-color-index uniform tensors: `color_uniforms[c] == c`.
+    pub color_uniforms: &'a [Tensor<u32>],
     /// Prefix sum shader for building constraint ranges.
     pub prefix_sum: &'a GpuPrefixSum,
     /// Number of solver iterations (max across all environments).
     pub num_solver_iterations: u32,
     /// Per-body graph-coloring group id (multibody-aware).
     pub body_group: &'a Tensor<u32>,
-    /// Shared per-batch capacity / section-offset uniform — see
-    /// [`crate::shaders::utils::BatchIndices`]. Consumed by the (refactored)
-    /// multibody kernels via `MultibodySolverArgs::batch_indices`; the RBD
-    /// constraint-solver kernels will migrate to it next.
+    /// When `true` (no multibody in the scene), warmstart uses the single
+    /// gather-per-body dispatch instead of one scatter dispatch per color.
+    /// The gather variant looks bodies up by their own id, which is only
+    /// correct when `body_group` is the identity (multibody constraints are
+    /// counted on their root's slot with link-id constraint sides).
+    pub colorless_warmstart: bool,
+    /// Whether the fused colored kernels are used.
+    ///
+    /// This is generally used when the number of constraints is small wrt.
+    /// the number of environments.
+    pub fused_color_sweeps: bool,
+    /// `true` when every rigid-body contact constraint is provably a no-op.
+    pub rb_contacts_inert: bool,
+    /// Shared per-batch indices.
     pub batch_indices: &'a Tensor<crate::shaders::utils::BatchIndices>,
+    /// GPU-written workgroup grid for the per-multibody contact-constraint
+    /// dispatches (zero workgroups on contact-free steps).
+    pub mb_sweep_indirect: &'a Tensor<[u32; 3]>,
 }
 
 impl GpuSolver {
@@ -164,6 +188,10 @@ impl GpuSolver {
             args.batch_indices,
         )?;
 
+        if args.rb_contacts_inert {
+            return Ok(());
+        }
+
         self.init_constraints.call(
             pass,
             args.contacts_len_indirect,
@@ -187,6 +215,7 @@ impl GpuSolver {
             args.body_constraint_counts,
             args.body_group,
             args.mprops,
+            args.contacts_len,
             args.batch_indices,
         )?;
 
@@ -216,12 +245,11 @@ impl GpuSolver {
     /// Solves constraints using the TGS (Total Gauss-Seidel) algorithm.
     ///
     /// When `multibody` is `Some`, multibody substep work is interleaved with
-    /// the rigid-body substep work — one `multibody.apply_substep` call inside
-    /// each iteration of the substep loop, just like rapier's
-    /// `velocity_solver::solve_constraints`.
+    /// the rigid-body substep work.
     pub fn solve_tgs<'a>(
         &self,
-        pass: &mut GpuPass,
+        encoder: &mut GpuEncoder,
+        mut timestamps: Option<&mut GpuTimestamps>,
         joint_solver: &GpuJointSolver,
         args: SolverArgs<'a>,
         mut joint_args: JointSolverArgs<'a>,
@@ -234,41 +262,55 @@ impl GpuSolver {
             None => (None, None),
         };
 
+        let skip_rb = args.rb_contacts_inert;
+        let joints_empty = joint_args.joints.is_empty();
+
         /*
          * Init solver vel increments.
          */
-        self.init_solver_vels_inc.call(
-            pass,
-            [args.num_colliders, args.num_batches, 1],
-            args.solver_vels_inc,
-            args.mprops,
-            args.sim_params,
-            args.batch_indices,
-        )?;
+        {
+            let mut pass = encoder.begin_pass("[RBD] slv/init", timestamps.as_deref_mut());
+            if !skip_rb {
+                self.init_solver_vels_inc.call(
+                    &mut pass,
+                    [args.num_colliders, args.num_batches, 1],
+                    args.solver_vels_inc,
+                    args.mprops,
+                    args.sim_params,
+                    args.batch_indices,
+                )?;
+            }
 
-        joint_solver.init(pass, &mut joint_args)?;
+            joint_solver.init(&mut pass, &mut joint_args)?;
+
+            // Bound for the per-substep contact scans: runs after the narrow
+            // phase wrote `contacts_len`, before the first substep build.
+            #[cfg(feature = "dim3")]
+            if let (Some(solver), Some(state)) = (mb_solver, mb_state.as_deref_mut()) {
+                let mut mb_args = MultibodySolverArgs {
+                    poses: &mut *args.solver_body_poses,
+                    collider_world_poses: args.collider_world_poses,
+                    mprops: args.mprops,
+                    contacts: args.contacts,
+                    contacts_len: args.contacts_len,
+                    solver_vels: &mut *args.solver_vels,
+                    batch_indices: args.batch_indices,
+                    color_uniforms: args.color_uniforms,
+                    mb_sweep_indirect: args.mb_sweep_indirect,
+                };
+                solver.stash_contacts_len(&mut pass, state, &mut mb_args)?;
+            }
+        }
 
         // Per substep, the multibody work is split into five phases that are
-        // INTERLEAVED with the matching rigid-body phases, mirroring rapier's
-        // `velocity_solver::solve_constraints` order:
-        //   P1/F1  integrate all velocities
-        //   P2/F2  build + warmstart all constraints
-        //   P3/F3  one PGS sweep over ALL joints + contacts WITH bias
-        //   P4/F4  integrate ALL positions ONCE
-        //   P5/F5  one PGS sweep over ALL joints + contacts WITHOUT bias
-        // (Previously the entire multibody solve — including its own position
-        // integration — ran as a monolithic block BEFORE the rigid-body solve,
-        // which de-synchronised mb↔free contact coupling.)
-        //
+        // interleaved with the matching rigid-body phases.
         // Each `mb_phase!($method $(, $extra)*)` invocation runs one multibody
-        // phase. It is `#[cfg(feature = "dim3")]`-gated (the multibody solver
-        // only exists in 3D) and reconstructs `mb_args` each time — the borrow
-        // of `args.solver_vels` / `args.solver_body_poses` must be released
-        // before the interleaved rigid-body call that touches the same buffers.
+        // phase.
         macro_rules! mb_phase {
-            ($method:ident $(, $extra:expr)*) => {{
+            ($label:expr, $method:ident $(, $extra:expr)*) => {{
                 #[cfg(feature = "dim3")]
                 if let (Some(solver), Some(state)) = (mb_solver, mb_state.as_deref_mut()) {
+                    let mut pass = encoder.begin_pass($label, timestamps.as_deref_mut());
                     let mut mb_args = MultibodySolverArgs {
                         poses: &mut *args.solver_body_poses,
                         collider_world_poses: args.collider_world_poses,
@@ -277,8 +319,10 @@ impl GpuSolver {
                         contacts_len: args.contacts_len,
                         solver_vels: &mut *args.solver_vels,
                         batch_indices: args.batch_indices,
+                        color_uniforms: args.color_uniforms,
+                        mb_sweep_indirect: args.mb_sweep_indirect,
                     };
-                    solver.$method(pass, state, &mut mb_args $(, $extra)*)?;
+                    solver.$method(&mut pass, state, &mut mb_args $(, $extra)*)?;
                 }
             }};
         }
@@ -290,105 +334,223 @@ impl GpuSolver {
             let _ = is_last_substep;
 
             /*
-             * P1/F1 — integrate velocities (apply `a · dt'` / gravity increment).
-             */
-            mb_phase!(substep_integrate_velocities);
-            self.apply_solver_vels_inc.call(
-                pass,
-                [args.num_colliders, args.num_batches, 1],
-                args.solver_vels,
-                args.solver_vels_inc,
-                args.batch_indices,
-            )?;
-
-            /*
-             * P2/F2 — build + warmstart constraints.
+             * Integrate velocities (apply `a · dt'` / gravity increment).
              */
-            mb_phase!(substep_build_constraints);
-            self.update_constraints.call(
-                pass,
-                args.contacts_len_indirect,
-                args.constraints,
-                args.constraint_builders,
-                args.contacts_len,
-                args.solver_body_poses,
-                args.sim_params,
-                args.batch_indices,
-            )?;
-            joint_solver.update(pass, &mut joint_args, args.solver_body_poses)?;
-            self.reset_color.call(pass, 1u32, args.curr_color)?;
-            for _ in 0..args.num_colors {
-                self.warmstart.call(
-                    pass,
-                    args.contacts_len_indirect,
-                    args.constraints,
+            mb_phase!("[RBD] slv/mb-integrate-vels", substep_integrate_velocities);
+            if !skip_rb {
+                let mut pass =
+                    encoder.begin_pass("[RBD] slv/rb-apply-inc", timestamps.as_deref_mut());
+                self.apply_solver_vels_inc.call(
+                    &mut pass,
+                    [args.num_colliders, args.num_batches, 1],
                     args.solver_vels,
-                    args.constraints_colors,
-                    args.contacts_len,
-                    args.curr_color,
+                    args.solver_vels_inc,
                     args.batch_indices,
                 )?;
-                self.inc_color.call(pass, 1u32, args.curr_color)?
             }
 
             /*
-             * P3/F3 — solve ALL joints + contacts WITH bias.
+             * Build + warmstart constraints.
              */
-            mb_phase!(substep_solve_with_bias);
-            joint_solver.solve(pass, &mut joint_args, args.solver_vels, true)?;
-            self.reset_color.call(pass, 1u32, args.curr_color)?;
-            for _ in 0..args.num_colors {
-                self.step_gauss_seidel.call(
-                    pass,
-                    args.contacts_len_indirect,
-                    args.constraints,
-                    args.solver_vels,
-                    args.constraints_colors,
-                    args.contacts_len,
-                    args.curr_color,
-                    args.batch_indices,
-                )?;
-                self.inc_color.call(pass, 1u32, args.curr_color)?
+            {
+                #[cfg(feature = "dim3")]
+                if let (Some(solver), Some(state)) = (mb_solver, mb_state.as_deref_mut()) {
+                    let mut mb_args = MultibodySolverArgs {
+                        poses: &mut *args.solver_body_poses,
+                        collider_world_poses: args.collider_world_poses,
+                        mprops: args.mprops,
+                        contacts: args.contacts,
+                        contacts_len: args.contacts_len,
+                        solver_vels: &mut *args.solver_vels,
+                        batch_indices: args.batch_indices,
+                        color_uniforms: args.color_uniforms,
+                        mb_sweep_indirect: args.mb_sweep_indirect,
+                    };
+                    solver.substep_build_constraints(
+                        encoder,
+                        timestamps.as_deref_mut(),
+                        state,
+                        &mut mb_args,
+                        substep_id == 0,
+                    )?;
+                }
+            }
+            if !skip_rb || !joints_empty {
+                let mut pass =
+                    encoder.begin_pass("[RBD] slv/rb-build-warmstart", timestamps.as_deref_mut());
+                let pass = &mut pass;
+                if !skip_rb {
+                    self.update_constraints.call(
+                        pass,
+                        args.contacts_len_indirect,
+                        args.constraints,
+                        args.constraint_builders,
+                        args.contacts_len,
+                        args.solver_body_poses,
+                        args.sim_params,
+                        args.batch_indices,
+                    )?;
+                }
+                joint_solver.update(pass, &mut joint_args, args.solver_body_poses)?;
+                if skip_rb {
+                    // Contact warmstart skipped: no rigid-body contact
+                    // constraint can carry an impulse here.
+                } else if args.colorless_warmstart {
+                    self.warmstart_without_colors.call(
+                        pass,
+                        [args.num_colliders, args.num_batches, 1],
+                        args.body_constraint_counts,
+                        args.body_constraint_ids,
+                        args.constraints,
+                        args.solver_vels,
+                        args.batch_indices,
+                    )?;
+                } else if args.fused_color_sweeps {
+                    // One dispatch, one workgroup per batch, colors looped
+                    // internally. `color_uniforms[num_colors]` holds the
+                    // constant `num_colors`.
+                    self.warmstart_fused.call(
+                        pass,
+                        [64, args.num_batches, 1],
+                        args.constraints,
+                        args.solver_vels,
+                        args.color_bucket_starts,
+                        args.color_sorted_ids,
+                        &args.color_uniforms[args.num_colors as usize],
+                        args.batch_indices,
+                    )?;
+                } else {
+                    // NOTE: contact colors start at 1 (0 = unassigned).
+                    for c in 1..=args.num_colors {
+                        self.warmstart.call(
+                            pass,
+                            args.contacts_len_indirect,
+                            args.constraints,
+                            args.solver_vels,
+                            args.color_bucket_starts,
+                            args.color_sorted_ids,
+                            &args.color_uniforms[c as usize],
+                            args.batch_indices,
+                        )?;
+                    }
+                }
             }
 
             /*
-             * P4/F4 — integrate ALL positions once.
+             * Solve all joints + contacts with bias.
              */
-            mb_phase!(substep_integrate_positions, is_last_substep);
-            self.integrate_linearized.call(
-                pass,
-                [args.num_colliders, args.num_batches, 1],
-                args.solver_body_poses,
-                args.solver_vels,
-                args.sim_params,
-                args.batch_indices,
-            )?;
+            mb_phase!("[RBD] slv/mb-solve-bias", substep_solve_with_bias);
+            if !skip_rb || !joints_empty {
+                let mut pass =
+                    encoder.begin_pass("[RBD] slv/rb-solve-bias", timestamps.as_deref_mut());
+                let pass = &mut pass;
+                joint_solver.solve(pass, &mut joint_args, args.solver_vels, true)?;
+                if skip_rb {
+                    // Contact sweeps skipped (inert constraints).
+                } else if args.fused_color_sweeps {
+                    self.step_gauss_seidel_fused.call(
+                        pass,
+                        [64, args.num_batches, 1],
+                        args.constraints,
+                        args.solver_vels,
+                        args.color_bucket_starts,
+                        args.color_sorted_ids,
+                        &args.color_uniforms[args.num_colors as usize],
+                        args.batch_indices,
+                        // use_bias = 1 (the `color_uniform[1]` contains the value 1)
+                        &args.color_uniforms[1],
+                    )?;
+                } else {
+                    for c in 1..=args.num_colors {
+                        self.step_gauss_seidel.call(
+                            pass,
+                            args.contacts_len_indirect,
+                            args.constraints,
+                            args.solver_vels,
+                            args.color_bucket_starts,
+                            args.color_sorted_ids,
+                            &args.color_uniforms[c as usize],
+                            args.batch_indices,
+                            // use_bias = 1 (the `color_uniform[1]` contains the value 1)
+                            &args.color_uniforms[1],
+                        )?;
+                    }
+                }
+            }
 
             /*
-             * P5/F5 — solve ALL joints + contacts WITHOUT bias (stabilization).
+             * Integrate all positions once.
              */
-            mb_phase!(substep_solve_no_bias);
-            joint_solver.solve(pass, &mut joint_args, args.solver_vels, false)?;
-            self.remove_cfm_and_bias_kernel.call(
-                pass,
-                args.contacts_len_indirect,
-                args.constraints,
-                args.contacts_len,
-                args.batch_indices,
-            )?;
-            self.reset_color.call(pass, 1u32, args.curr_color)?;
-            for _ in 0..args.num_colors {
-                self.step_gauss_seidel.call(
-                    pass,
-                    args.contacts_len_indirect,
-                    args.constraints,
+            mb_phase!(
+                "[RBD] slv/mb-integrate-pos",
+                substep_integrate_positions,
+                is_last_substep
+            );
+            if !skip_rb {
+                let mut pass =
+                    encoder.begin_pass("[RBD] slv/rb-integrate", timestamps.as_deref_mut());
+                self.integrate_linearized.call(
+                    &mut pass,
+                    [args.num_colliders, args.num_batches, 1],
+                    args.solver_body_poses,
                     args.solver_vels,
-                    args.constraints_colors,
-                    args.contacts_len,
-                    args.curr_color,
+                    args.sim_params,
                     args.batch_indices,
                 )?;
-                self.inc_color.call(pass, 1u32, args.curr_color)?
+            }
+
+            /*
+             * Solve all joints + contacts without bias (stabilization).
+             */
+            mb_phase!("[RBD] slv/mb-solve-nobias", substep_solve_no_bias);
+            if !skip_rb || !joints_empty {
+                let mut pass =
+                    encoder.begin_pass("[RBD] slv/rb-solve-nobias", timestamps.as_deref_mut());
+                let pass = &mut pass;
+                if !skip_rb {
+                    self.refresh_rhs_wo_bias.call(
+                        pass,
+                        args.contacts_len_indirect,
+                        args.constraints,
+                        args.constraint_builders,
+                        args.contacts_len,
+                        args.solver_body_poses,
+                        args.sim_params,
+                        args.batch_indices,
+                    )?;
+                }
+                joint_solver.solve(pass, &mut joint_args, args.solver_vels, false)?;
+                if skip_rb {
+                    // Contact sweeps skipped (inert constraints).
+                } else if args.fused_color_sweeps {
+                    self.step_gauss_seidel_fused.call(
+                        pass,
+                        [64, args.num_batches, 1],
+                        args.constraints,
+                        args.solver_vels,
+                        args.color_bucket_starts,
+                        args.color_sorted_ids,
+                        &args.color_uniforms[args.num_colors as usize],
+                        args.batch_indices,
+                        // use_bias = 0 (the `color_uniform[0]` contains the value 0)
+                        &args.color_uniforms[0],
+                    )?;
+                } else {
+                    for c in 1..=args.num_colors {
+                        self.step_gauss_seidel.call(
+                            pass,
+                            args.contacts_len_indirect,
+                            args.constraints,
+                            args.solver_vels,
+                            args.color_bucket_starts,
+                            args.color_sorted_ids,
+                            &args.color_uniforms[c as usize],
+                            args.batch_indices,
+                            // use_bias = 0 (the `color_uniform[0]` contains the value 0)
+                            &args.color_uniforms[0],
+                        )?;
+                    }
+                }
             }
         }
 
@@ -396,16 +558,19 @@ impl GpuSolver {
          * Writeback body velocities and convert COM-centered solver poses
          * back to body-origin poses.
          */
-        self.finalize.call(
-            pass,
-            [args.num_colliders, args.num_batches, 1],
-            args.vels,
-            args.solver_vels,
-            args.body_poses,
-            args.solver_body_poses,
-            args.local_mprops,
-            args.batch_indices,
-        )?;
+        {
+            let mut pass = encoder.begin_pass("[RBD] slv/finalize", timestamps.as_deref_mut());
+            self.finalize.call(
+                &mut pass,
+                [args.num_colliders, args.num_batches, 1],
+                args.vels,
+                args.solver_vels,
+                args.body_poses,
+                args.solver_body_poses,
+                args.local_mprops,
+                args.batch_indices,
+            )?;
+        }
 
         Ok(())
     }
diff --git a/src_rbd/dynamics/warmstart.rs b/src_rbd/dynamics/warmstart.rs
index 00c1e2e..b0b07f3 100644
--- a/src_rbd/dynamics/warmstart.rs
+++ b/src_rbd/dynamics/warmstart.rs
@@ -1,7 +1,8 @@
 //! Warmstarting: reuses previous-frame impulses for faster solver convergence.
 
 use crate::shaders::dynamics::{
-    GpuTransferWarmstartImpulses, TwoBodyConstraint, TwoBodyConstraintBuilder,
+    GpuSeedColorsFromWarmstart, GpuTransferWarmstartImpulses, TwoBodyConstraint,
+    TwoBodyConstraintBuilder,
 };
 use crate::shaders::utils::BatchIndices;
 use khal::Shader;
@@ -16,6 +17,9 @@ use vortx::tensor::Tensor;
 pub struct GpuWarmstart {
     /// Compute pipeline that matches contacts and transfers impulses.
     transfer_warmstart_impulses_kernel: GpuTransferWarmstartImpulses,
+    /// Seeds the topo-gc coloring from the previous frame's colors (same
+    /// old/new body-pair matching as the impulse transfer).
+    seed_colors_kernel: GpuSeedColorsFromWarmstart,
 }
 
 /// Arguments for warmstart dispatch.
@@ -42,6 +46,30 @@ pub struct WarmstartArgs<'a> {
     pub batch_indices: &'a Tensor<BatchIndices>,
 }
 
+/// Arguments for the coloring seed dispatch.
+pub struct SeedColorsArgs<'a> {
+    /// Number of contacts in current frame.
+    pub contacts_len: &'a Tensor<u32>,
+    /// Constraint counts per body from previous frame.
+    pub old_body_constraint_counts: &'a Tensor<u32>,
+    /// Constraint IDs per body from previous frame.
+    pub old_body_constraint_ids: &'a Tensor<u32>,
+    /// Solver constraints from previous frame.
+    pub old_constraints: &'a Tensor<TwoBodyConstraint>,
+    /// Solver constraints for current frame.
+    pub new_constraints: &'a Tensor<TwoBodyConstraint>,
+    /// Colors assigned to the previous frame's constraints.
+    pub old_constraints_colors: &'a Tensor<u32>,
+    /// Output: colors for the current frame's constraints (seeded slots only).
+    pub constraints_colors: &'a mut Tensor<u32>,
+    /// Output: per-constraint colored flag consumed by topo-gc.
+    pub colored: &'a mut Tensor<u32>,
+    /// Indirect dispatch arguments based on contact count.
+    pub contacts_len_indirect: &'a Tensor<[u32; 3]>,
+    /// Shared per-batch index uniform.
+    pub batch_indices: &'a Tensor<BatchIndices>,
+}
+
 impl GpuWarmstart {
     /// Transfers warmstart impulses from old constraints to new constraints.
     pub fn transfer_warmstart_impulses<'a>(
@@ -62,4 +90,26 @@ impl GpuWarmstart {
             args.batch_indices,
         )
     }
+
+    /// Seeds the topo-gc coloring from the previous frame's colors. Must run
+    /// after the topo-gc reset and before its iterations.
+    pub fn seed_colors_from_warmstart(
+        &self,
+        pass: &mut GpuPass,
+        args: SeedColorsArgs<'_>,
+    ) -> Result<(), GpuBackendError> {
+        self.seed_colors_kernel.call(
+            pass,
+            args.contacts_len_indirect,
+            args.old_body_constraint_counts,
+            args.old_body_constraint_ids,
+            args.old_constraints,
+            args.new_constraints,
+            args.old_constraints_colors,
+            args.constraints_colors,
+            args.colored,
+            args.contacts_len,
+            args.batch_indices,
+        )
+    }
 }
diff --git a/src_rbd/pipeline/insertion_removal.rs b/src_rbd/pipeline/insertion_removal.rs
index b5e1332..2c9870b 100644
--- a/src_rbd/pipeline/insertion_removal.rs
+++ b/src_rbd/pipeline/insertion_removal.rs
@@ -77,9 +77,12 @@ impl RbdState {
         // collider_parent: identity within each batch initially (no body is
         // active yet). `append_bodies` overwrites the active prefix.
         let mut all_collider_parent = Vec::with_capacity(num_bodies_total);
+        let mut all_pair_filter = Vec::with_capacity(num_bodies_total);
         for _ in 0..num_batches {
             for c in 0..capacity_per_batch {
                 all_collider_parent.push(c);
+                // Identity parent, no multibody key.
+                all_pair_filter.push([c, 0u32]);
             }
         }
 
@@ -123,6 +126,7 @@ impl RbdState {
         let collider_local_poses = Tensor::vector(backend, &all_collider_local_poses, rw).unwrap();
         let collider_parent = Tensor::vector(backend, &all_collider_parent, rw).unwrap();
         let collision_groups = Tensor::vector(backend, &all_collision_groups, rw).unwrap();
+        let pair_filter = Tensor::vector(backend, &all_pair_filter, rw).unwrap();
         // Padding colliders are inert; default material keeps the buffer sized.
         let all_collider_materials = vec![GpuColliderMaterial::default(); num_bodies_total];
         let collider_materials = Tensor::vector(backend, &all_collider_materials, rw).unwrap();
@@ -158,6 +162,8 @@ impl RbdState {
         .unwrap();
         let contacts_indirect =
             Tensor::scalar_uninit(backend, BufferUsages::STORAGE | BufferUsages::INDIRECT).unwrap();
+        let mb_sweep_indirect =
+            Tensor::scalar_uninit(backend, BufferUsages::STORAGE | BufferUsages::INDIRECT).unwrap();
         let pfm_pairs_indirect =
             Tensor::scalar_uninit(backend, BufferUsages::STORAGE | BufferUsages::INDIRECT).unwrap();
         let pfm_pairs =
@@ -178,10 +184,25 @@ impl RbdState {
             Tensor::vector_uninit(backend, collisions_capacity * num_batches, storage).unwrap();
         let constraints_colors =
             Tensor::vector_uninit(backend, collisions_capacity * num_batches, storage).unwrap();
+        let old_constraints_colors = Tensor::vector(
+            backend,
+            &vec![0u32; (collisions_capacity * num_batches) as usize],
+            storage,
+        )
+        .unwrap();
         let colored =
             Tensor::vector_uninit(backend, collisions_capacity * num_batches, storage).unwrap();
         let constraints_rands =
             Tensor::vector_uninit(backend, collisions_capacity * num_batches, storage).unwrap();
+        let color_buckets_stride = capacities.solver_colors + 3;
+        let color_bucket_counts =
+            Tensor::vector_uninit(backend, color_buckets_stride * num_batches, storage).unwrap();
+        let color_bucket_starts =
+            Tensor::vector_uninit(backend, color_buckets_stride * num_batches, storage).unwrap();
+        let color_bucket_cursors =
+            Tensor::vector_uninit(backend, color_buckets_stride * num_batches, storage).unwrap();
+        let color_sorted_ids =
+            Tensor::vector_uninit(backend, collisions_capacity * num_batches, storage).unwrap();
         let old_constraints_counts =
             Tensor::vector_uninit(backend, num_colliders_per_batch * num_batches, storage).unwrap();
         let new_constraints_counts =
@@ -201,6 +222,7 @@ impl RbdState {
         let collision_pairs_per_batch_cpu = collisions_capacity;
         #[allow(unused_mut)] // Only mutated with the dim3 (multibody) feature.
         let mut bi = BatchIndices {
+            num_batches,
             colliders_batch_capacity: num_colliders_per_batch,
             // No body is active initially; bodies are added later via `append_bodies`.
             colliders_len: 0,
@@ -209,6 +231,7 @@ impl RbdState {
             contacts_batch_capacity: contacts_per_batch_cpu,
             impulse_joints_batch_capacity: joints.joints_per_batch(),
             impulse_joints_len: joints.num_active_joints(),
+            solver_color_buckets_stride: color_buckets_stride,
             ..Default::default()
         };
         #[cfg(feature = "dim3")]
@@ -228,7 +251,6 @@ impl RbdState {
             sim_params: Tensor::vector(backend, &all_sim_params, BufferUsages::STORAGE).unwrap(),
             vels: Tensor::vector(backend, &all_vels, rw).unwrap(),
             solver_vels: Tensor::vector(backend, &all_vels, storage).unwrap(),
-            solver_vels_out: Tensor::vector(backend, &all_vels, storage).unwrap(),
             solver_vels_inc: Tensor::vector(backend, &all_vels, storage).unwrap(),
             joints,
             #[cfg(feature = "dim3")]
@@ -245,6 +267,7 @@ impl RbdState {
             collider_local_poses,
             collider_parent,
             collision_groups,
+            pair_filter,
             collider_materials,
             collision_pairs,
             collision_pairs_len,
@@ -259,6 +282,7 @@ impl RbdState {
             contacts,
             contacts_len,
             contacts_indirect,
+            mb_sweep_indirect,
             pfm_pairs,
             pfm_pairs_len,
             pfm_pairs_indirect,
@@ -269,8 +293,13 @@ impl RbdState {
             new_constraint_builders,
             new_constraints_counts,
             constraints_colors,
+            old_constraints_colors,
             colored,
             constraints_rands,
+            color_bucket_counts,
+            color_bucket_starts,
+            color_bucket_cursors,
+            color_sorted_ids,
             curr_color: Tensor::scalar(
                 backend,
                 0u32,
@@ -294,9 +323,11 @@ impl RbdState {
             .unwrap(),
             old_body_constraint_ids,
             new_body_constraint_ids,
+            color_uniforms: Vec::new(),
             prefix_sum_workspace: PrefixSumWorkspace::default(),
             lbvh: LbvhState::with_usages(backend, lbvh_usages),
             max_colors: capacities.solver_colors,
+            rb_contacts_inert: false,
             num_active_colliders: 0,
             num_active_bodies: 0,
         }
@@ -342,6 +373,12 @@ impl RbdState {
             let body_pose = *rb.position();
             let collider_local_pose = co.position_wrt_parent().copied().unwrap_or(Pose::IDENTITY);
             let is_dynamic = rb.is_dynamic();
+            if is_dynamic {
+                // A free dynamic body: its contacts need the rigid-body
+                // constraint pipeline. Never set back on removal; a stale
+                // `false` only costs performance.
+                self.rb_contacts_inert = false;
+            }
             let (local, world) = if is_dynamic {
                 // A standalone rigid-body carries no collider mass: rapier only
                 // folds a collider's mass into the body once the collider is
@@ -404,6 +441,8 @@ impl RbdState {
         // body's collider slot equals its body slot: `collider_parent` is the
         // identity over the appended (env-local) range.
         let parents: Vec<u32> = (active as u32..(active + bodies.len()) as u32).collect();
+        // NOTE: appended bodies are free bodies (never multibody links).
+        let pair_filters: Vec<[u32; 2]> = parents.iter().map(|&p| [p, 0u32]).collect();
 
         // Write the same body data into every batch's slot range so all
         // environments share the same topology.
@@ -418,6 +457,7 @@ impl RbdState {
                 &collider_local_poses,
             )?;
             backend.write_buffer(self.collider_parent.buffer_mut(), base, &parents)?;
+            backend.write_buffer(self.pair_filter.buffer_mut(), base, &pair_filters)?;
             backend.write_buffer(self.local_mprops.buffer_mut(), base, &local_mprops)?;
             backend.write_buffer(self.mprops.buffer_mut(), base, &mprops)?;
             backend.write_buffer(self.shapes.buffer_mut(), base, &shapes)?;
@@ -460,6 +500,23 @@ impl RbdState {
             crate::rapier::geometry::InteractionTestMode::And,
         );
 
+        let staging_usages =
+            BufferUsages::STORAGE | BufferUsages::COPY_SRC | BufferUsages::COPY_DST;
+        let mut enc = backend.begin_encoding();
+        let mut any_copy = false;
+        let mut staging_pose = backend.uninit_buffer::<Pose>(1, staging_usages)?;
+        let mut staging_local_mprops =
+            backend.uninit_buffer::<GpuLocalMassProperties>(1, staging_usages)?;
+        let mut staging_mprops = backend.uninit_buffer::<GpuWorldMassProperties>(1, staging_usages)?;
+        let mut staging_vels = backend.uninit_buffer::<GpuVelocity>(1, staging_usages)?;
+        let mut staging_shapes = backend.uninit_buffer::<Shape>(1, staging_usages)?;
+        let mut staging_groups = backend
+            .uninit_buffer::<crate::rapier::geometry::InteractionGroups>(1, staging_usages)?;
+        let mut staging_materials =
+            backend.uninit_buffer::<GpuColliderMaterial>(1, staging_usages)?;
+        // Deferred `(buffer-kind, slot)` neutralisation writes.
+        let mut neutralize: Vec<usize> = Vec::new();
+
         for local in locals {
             let active = self.num_active_colliders as usize;
             if active == 0 || local >= active {
@@ -475,61 +532,37 @@ impl RbdState {
                     // Relocate the last active body into the freed slot. A staging
                     // buffer is used to avoid same-buffer overlapping copies.
                     macro_rules! relocate {
-                        ($t:expr) => {{
-                            let mut staging = backend.uninit_buffer(
-                                1,
-                                BufferUsages::STORAGE
-                                    | BufferUsages::COPY_SRC
-                                    | BufferUsages::COPY_DST,
-                            )?;
-                            let mut enc = backend.begin_encoding();
+                        ($t:expr, $staging:expr) => {{
                             enc.copy_buffer_to_buffer(
                                 $t.buffer(),
                                 last_global,
-                                &mut staging,
+                                &mut $staging,
                                 0,
                                 1,
                             )?;
                             enc.copy_buffer_to_buffer(
-                                &staging,
+                                &$staging,
                                 0,
                                 $t.buffer_mut(),
                                 hole_global,
                                 1,
                             )?;
-                            backend.submit(enc)?;
+                            any_copy = true;
                         }};
                     }
-                    relocate!(self.body_poses);
-                    relocate!(self.solver_body_poses);
-                    relocate!(self.collider_world_poses);
-                    relocate!(self.collider_local_poses);
-                    relocate!(self.local_mprops);
-                    relocate!(self.mprops);
-                    relocate!(self.vels);
-                    relocate!(self.shapes);
-                    relocate!(self.collision_groups);
-                    relocate!(self.collider_materials);
+                    relocate!(self.body_poses, staging_pose);
+                    relocate!(self.solver_body_poses, staging_pose);
+                    relocate!(self.collider_world_poses, staging_pose);
+                    relocate!(self.collider_local_poses, staging_pose);
+                    relocate!(self.local_mprops, staging_local_mprops);
+                    relocate!(self.mprops, staging_mprops);
+                    relocate!(self.vels, staging_vels);
+                    relocate!(self.shapes, staging_shapes);
+                    relocate!(self.collision_groups, staging_groups);
+                    relocate!(self.collider_materials, staging_materials);
                 }
 
-                // The now-topmost slot becomes inactive padding: neutralize it so
-                // it never participates in collisions even if a kernel scans up
-                // to the per-batch capacity.
-                backend.write_buffer(
-                    self.collision_groups.buffer_mut(),
-                    last_global as u64,
-                    &[none_groups],
-                )?;
-                backend.write_buffer(
-                    self.local_mprops.buffer_mut(),
-                    last_global as u64,
-                    &[GpuLocalMassProperties::default()],
-                )?;
-                backend.write_buffer(
-                    self.mprops.buffer_mut(),
-                    last_global as u64,
-                    &[GpuWorldMassProperties::default()],
-                )?;
+                neutralize.push(last_global);
             }
 
             if local != last {
@@ -538,9 +571,36 @@ impl RbdState {
             self.num_active_colliders = (active - 1) as u32;
         }
 
+        if any_copy {
+            backend.submit(enc)?;
+        }
+
+        // The now-topmost slots become inactive padding: neutralize them so
+        // they never participate in collisions even if a kernel scans up to
+        // the per-batch capacity. Done after the relocation submit so the
+        // copies read the pre-neutralisation data.
+        for last_global in neutralize {
+            backend.write_buffer(
+                self.collision_groups.buffer_mut(),
+                last_global as u64,
+                &[none_groups],
+            )?;
+            backend.write_buffer(
+                self.local_mprops.buffer_mut(),
+                last_global as u64,
+                &[GpuLocalMassProperties::default()],
+            )?;
+            backend.write_buffer(
+                self.mprops.buffer_mut(),
+                last_global as u64,
+                &[GpuWorldMassProperties::default()],
+            )?;
+        }
+
         // `collider_parent` is the identity mapping on the incremental (one
         // collider per body) path and stays identity under swap-remove, so it
         // needs no relocation; only the active body count tracks the colliders.
+        // The same holds for `pair_filter` (`[identity, 0]` on this path).
         self.num_active_bodies = self.num_active_colliders;
         self.rebuild_batch_indices(backend);
         Ok(remaps)
diff --git a/src_rbd/pipeline/rbd_state.rs b/src_rbd/pipeline/rbd_state.rs
index 73e0c92..001659d 100644
--- a/src_rbd/pipeline/rbd_state.rs
+++ b/src_rbd/pipeline/rbd_state.rs
@@ -123,21 +123,15 @@ pub struct RbdState {
     pub(super) num_colliders_per_batch: u32,
     pub(super) num_solver_iterations: u32,
     pub(super) sim_params: Tensor<RbdSimParams>,
-    /// Per-body world-origin pose (matches rapier's `RigidBody::position`). This
-    /// is the canonical pose stored between steps and the input to per-step
-    /// mass-properties update and multibody FK. The substep loop does NOT
-    /// touch this — see [`Self::solver_body_poses`].
+    /// Per-body world-origin pose (matches rapier's `RigidBody::position`).
     pub(super) body_poses: Tensor<Pose>,
-    /// Per-body COM-centered pose (rapier's `SolverPose`). Equals
-    /// `body_poses[i].prepend_translation(local_mprops[i].com)`. Seeded from
-    /// `body_poses` at step start, mutated by the solver substep loop, and
-    /// converted back to `body_poses` by `finalize` at step end.
+    /// Per-body COM-centered pose (rapier's `SolverPose`) used temporarily by
+    /// the solver (and then written back to `body_poses` after un-centering).
     pub(super) solver_body_poses: Tensor<Pose>,
     pub(super) local_mprops: Tensor<GpuLocalMassProperties>,
     pub(super) mprops: Tensor<GpuWorldMassProperties>,
     pub(super) vels: Tensor<GpuVelocity>,
     pub(super) solver_vels: Tensor<GpuVelocity>,
-    pub(super) solver_vels_out: Tensor<GpuVelocity>,
     pub(super) solver_vels_inc: Tensor<GpuVelocity>,
     pub(super) vertex_buffers: Tensor<PaddedVector>,
     pub(super) index_buffers: Tensor<u32>,
@@ -152,6 +146,11 @@ pub struct RbdState {
     pub(super) collider_world_poses: Tensor<Pose>,
     /// Per-collider [`crate::rapier::geometry::InteractionGroups`].
     pub(super) collision_groups: Tensor<crate::rapier::geometry::InteractionGroups>,
+    /// Per-collider broad-phase pair-filter key:
+    /// - `[0]`: to prevent colliders of the same body from colliding.
+    /// - `[1]`: to prevent colliders of adjacent links from a multibody from coliding.
+    /// Nonzero keys that are equal never collide.
+    pub(super) pair_filter: Tensor<[u32; 2]>,
     /// Per-collider friction / restitution coefficients (+ combine rules),
     pub(super) collider_materials: Tensor<GpuColliderMaterial>,
     pub(super) collision_pairs: Tensor<CollisionPair>,
@@ -187,6 +186,9 @@ pub struct RbdState {
     pub(super) contacts: Tensor<GpuIndexedContact>,
     pub(super) contacts_len: Tensor<u32>,
     pub(super) contacts_indirect: Tensor<[u32; 3]>,
+    /// Workgroup grid for the per-multibody contact-constraint dispatches:
+    /// `[multibodies_batch_capacity, num_batches, 1]`.
+    pub(super) mb_sweep_indirect: Tensor<[u32; 3]>,
     pub(super) new_constraints: Tensor<TwoBodyConstraint>,
     pub(super) new_constraint_builders: Tensor<TwoBodyConstraintBuilder>,
     pub(super) new_constraints_counts: Tensor<u32>,
@@ -196,9 +198,23 @@ pub struct RbdState {
     pub(super) old_constraints_counts: Tensor<u32>,
     pub(super) old_body_constraint_ids: Tensor<u32>,
     pub(super) constraints_colors: Tensor<u32>,
+    pub(super) old_constraints_colors: Tensor<u32>,
     pub(super) colored: Tensor<u32>,
     pub(super) constraints_rands: Tensor<u32>,
+    /// Per-batch per-color constraint counts (stride `max_colors + 3`), see
+    /// the `gpu_color_buckets_*` kernels.
+    pub(super) color_bucket_counts: Tensor<u32>,
+    /// Per-batch per-color exclusive prefix sums over the counts: color `c`
+    /// owns `color_sorted_ids[starts[c]..starts[c + 1]]`.
+    pub(super) color_bucket_starts: Tensor<u32>,
+    /// Scatter cursors (seeded from the starts each step).
+    pub(super) color_bucket_cursors: Tensor<u32>,
+    /// Constraint indices bucket-sorted by color (contacts layout).
+    pub(super) color_sorted_ids: Tensor<u32>,
     pub(super) curr_color: Tensor<u32>,
+    /// Pre-built per-color-index uniforms: `color_uniforms[c] == c`.
+    /// [`Self::ensure_color_uniforms`].
+    pub(super) color_uniforms: Vec<Tensor<u32>>,
     pub(super) uncolored: Tensor<u32>,
     pub(super) uncolored_staging: Tensor<u32>,
     pub(super) lbvh: LbvhState,
@@ -214,6 +230,9 @@ pub struct RbdState {
     pub(super) prefix_sum_workspace: PrefixSumWorkspace,
     /// Maximum number of constraint colors the solver will iterate.
     pub(super) max_colors: u32,
+    /// `true` when every body is either non-dynamic or multibody-controlled
+    /// (its rb-side `inv_mass` is zero),i.e., we can skip the contact pipelines.
+    pub(super) rb_contacts_inert: bool,
     /// CPU-side mirror of the number of *active* colliders per batch. Identical
     /// across all batches by the equal-topology invariant; slots in
     /// `[num_active_colliders .. num_colliders_per_batch)` are reserved padding.
@@ -235,6 +254,7 @@ impl RbdState {
     pub(super) fn rebuild_batch_indices(&mut self, backend: &GpuBackend) {
         #[allow(unused_mut)] // Only mutated with the dim3 (multibody) feature.
         let mut bi = BatchIndices {
+            num_batches: self.num_batches,
             colliders_batch_capacity: self.num_colliders_per_batch,
             colliders_len: self.num_active_colliders,
             bodies_len: self.num_active_bodies,
@@ -242,19 +262,17 @@ impl RbdState {
             contacts_batch_capacity: self.contacts_per_batch_cpu,
             impulse_joints_batch_capacity: self.joints.joints_per_batch(),
             impulse_joints_len: self.joints.num_active_joints(),
+            solver_color_buckets_stride: self.max_colors + 3,
             ..Default::default()
         };
         #[cfg(feature = "dim3")]
         self.multibodies.fill_batch_indices(&mut bi);
-        self.batch_indices = Tensor::scalar(
-            backend,
-            bi,
-            BufferUsages::STORAGE | BufferUsages::UNIFORM | BufferUsages::COPY_DST,
-        )
-        .unwrap();
+        backend
+            .write_buffer(self.batch_indices.buffer_mut(), 0, &[bi])
+            .unwrap();
     }
 
-    /// Shared per-batch index uniform — see `Self::rebuild_batch_indices`.
+    /// Shared per-batch index uniform.
     pub fn batch_indices(&self) -> &Tensor<BatchIndices> {
         &self.batch_indices
     }
@@ -266,10 +284,23 @@ impl RbdState {
         self.max_colors = max_colors.max(1);
     }
 
+    /// Grows [`Self::color_uniforms`] so indices `0..n` are available.
+    pub(super) fn ensure_color_uniforms(&mut self, backend: &GpuBackend, n: u32) {
+        for c in self.color_uniforms.len() as u32..n {
+            self.color_uniforms
+                .push(Tensor::scalar(backend, c, BufferUsages::UNIFORM).unwrap());
+        }
+    }
+
     /// Returns the configured max color count.
     pub fn max_colors(&self) -> u32 {
         self.max_colors
     }
+
+    /// `true` when every rigid-body contact constraint is provably a no-op.
+    pub fn rb_contacts_inert(&self) -> bool {
+        self.rb_contacts_inert
+    }
 }
 
 impl RbdState {
diff --git a/src_rbd/pipeline/rbd_state_from_rapier.rs b/src_rbd/pipeline/rbd_state_from_rapier.rs
index 92bdd1e..6dd8527 100644
--- a/src_rbd/pipeline/rbd_state_from_rapier.rs
+++ b/src_rbd/pipeline/rbd_state_from_rapier.rs
@@ -141,6 +141,7 @@ impl RbdState {
         // Bodies and colliders share the `max_colliders` per-batch stride but
         // form distinct index spaces — a body may own several colliders.
         let mut all_collider_parent: Vec<u32> = Vec::new();
+        let mut all_pair_filter: Vec<[u32; 2]> = Vec::new();
         // Per-environment count of *active* rigid bodies (distinct collider
         // parents + one synthetic body per parentless collider). Used to assert
         // the equal-topology invariant and to set `BatchIndices::bodies_len`.
@@ -229,6 +230,20 @@ impl RbdState {
                     }
                 };
 
+            // Handle bodies whose multibody disables self-contacts.
+            #[cfg(feature = "dim3")]
+            let no_self_collide: HashMap<crate::rapier::dynamics::RigidBodyHandle, u32> = {
+                let mut map = HashMap::new();
+                for (mb_ord, mb) in multibody_joints.multibodies().enumerate() {
+                    if !mb.self_contacts_enabled() {
+                        for link in mb.links() {
+                            map.insert(link.rigid_body_handle(), mb_ord as u32 + 1);
+                        }
+                    }
+                }
+                map
+            };
+
             for (_, co) in colliders.iter() {
                 // Resolve (allocating if needed) the parent body's env-local slot.
                 //
@@ -281,6 +296,16 @@ impl RbdState {
                 all_collider_materials.push(collider_material_from_rapier(co));
                 // Env-local body slot; the kernels apply the per-batch stride.
                 all_collider_parent.push(body_local);
+
+                // Broad-phase pair-filter key.
+                #[cfg(feature = "dim3")]
+                let mb_key = co
+                    .parent()
+                    .and_then(|h| no_self_collide.get(&h).copied())
+                    .unwrap_or(0);
+                #[cfg(feature = "dim2")]
+                let mb_key = 0u32;
+                all_pair_filter.push([body_local, mb_key]);
             }
 
             // Give every multibody link a body slot too, even collider-less ones
@@ -322,11 +347,9 @@ impl RbdState {
                     crate::rapier::geometry::Group::NONE,
                     crate::rapier::geometry::InteractionTestMode::And,
                 ));
-                // Padding colliders are inert; material is irrelevant but must
-                // exist to keep the per-collider buffers the same length.
                 all_collider_materials.push(GpuColliderMaterial::default());
-                // Padding colliders are inert; parent body 0 is a safe placeholder.
                 all_collider_parent.push(0);
+                all_pair_filter.push([u32::MAX, 0]);
             }
 
             // Pad bodies to the shared per-batch stride (`max_colliders` =
@@ -489,6 +512,16 @@ impl RbdState {
             }
         }
 
+        let rb_contacts_inert = all_env_body_counts
+            .iter()
+            .enumerate()
+            .all(|(batch, &count)| {
+                let start = batch * max_colliders;
+                all_local_mprops[start..start + count]
+                    .iter()
+                    .all(|m| m.inv_mass == Vector::ZERO)
+            });
+
         // Build the per-body "graph group" lookup. Free bodies map to themselves
         // (one body = one graph node). Bodies belonging to a multibody all map
         // to a single shared group id (= the body id of the multibody's root
@@ -539,6 +572,7 @@ impl RbdState {
             Tensor::vector(backend, &all_collider_local_poses, storage).unwrap();
         let collider_parent = Tensor::vector(backend, &all_collider_parent, storage).unwrap();
         let collision_groups = Tensor::vector(backend, &all_collision_groups, storage).unwrap();
+        let pair_filter = Tensor::vector(backend, &all_pair_filter, storage).unwrap();
         let collider_materials = Tensor::vector(backend, &all_collider_materials, storage).unwrap();
 
         let collision_pairs = Tensor::vector_uninit(
@@ -581,6 +615,8 @@ impl RbdState {
         .unwrap();
         let contacts_indirect =
             Tensor::scalar_uninit(backend, BufferUsages::STORAGE | BufferUsages::INDIRECT).unwrap();
+        let mb_sweep_indirect =
+            Tensor::scalar_uninit(backend, BufferUsages::STORAGE | BufferUsages::INDIRECT).unwrap();
         let pfm_pairs_indirect =
             Tensor::scalar_uninit(backend, BufferUsages::STORAGE | BufferUsages::INDIRECT).unwrap();
         let pfm_pairs = Tensor::vector_uninit(
@@ -625,6 +661,12 @@ impl RbdState {
             storage,
         )
         .unwrap();
+        let old_constraints_colors = Tensor::vector(
+            backend,
+            &vec![0u32; (capacities.collisions_capacity * num_batches) as usize],
+            storage,
+        )
+        .unwrap();
         let colored = Tensor::vector_uninit(
             backend,
             capacities.collisions_capacity * num_batches,
@@ -637,6 +679,19 @@ impl RbdState {
             storage,
         )
         .unwrap();
+        let color_buckets_stride = capacities.solver_colors + 3;
+        let color_bucket_counts =
+            Tensor::vector_uninit(backend, color_buckets_stride * num_batches, storage).unwrap();
+        let color_bucket_starts =
+            Tensor::vector_uninit(backend, color_buckets_stride * num_batches, storage).unwrap();
+        let color_bucket_cursors =
+            Tensor::vector_uninit(backend, color_buckets_stride * num_batches, storage).unwrap();
+        let color_sorted_ids = Tensor::vector_uninit(
+            backend,
+            capacities.collisions_capacity * num_batches,
+            storage,
+        )
+        .unwrap();
         let old_constraints_counts = Tensor::vector_uninit(
             backend,
             num_colliders_per_batch as u32 * num_batches,
@@ -672,6 +727,7 @@ impl RbdState {
         let collision_pairs_per_batch_cpu = capacities.collisions_capacity;
         #[allow(unused_mut)] // Only mutated with the dim3 (multibody) feature.
         let mut bi = BatchIndices {
+            num_batches,
             colliders_batch_capacity: num_colliders_per_batch as u32,
             colliders_len: num_colliders as u32,
             bodies_len: num_bodies as u32,
@@ -679,6 +735,7 @@ impl RbdState {
             contacts_batch_capacity: contacts_per_batch_cpu,
             impulse_joints_batch_capacity: joints.joints_per_batch(),
             impulse_joints_len: joints.num_active_joints(),
+            solver_color_buckets_stride: color_buckets_stride,
             ..Default::default()
         };
         #[cfg(feature = "dim3")]
@@ -698,7 +755,6 @@ impl RbdState {
             sim_params: Tensor::vector(backend, &all_sim_params, BufferUsages::STORAGE).unwrap(),
             vels: Tensor::vector(backend, &all_vels, storage).unwrap(),
             solver_vels: Tensor::vector(backend, &all_vels, storage).unwrap(),
-            solver_vels_out: Tensor::vector(backend, &all_vels, storage).unwrap(),
             solver_vels_inc: Tensor::vector(backend, &all_vels, storage).unwrap(),
             joints,
             #[cfg(feature = "dim3")]
@@ -736,6 +792,7 @@ impl RbdState {
             collider_local_poses,
             collider_parent,
             collision_groups,
+            pair_filter,
             collider_materials,
             collision_pairs,
             collision_pairs_len,
@@ -750,6 +807,7 @@ impl RbdState {
             contacts,
             contacts_len,
             contacts_indirect,
+            mb_sweep_indirect,
             pfm_pairs,
             pfm_pairs_len,
             pfm_pairs_indirect,
@@ -760,8 +818,13 @@ impl RbdState {
             new_constraint_builders,
             new_constraints_counts,
             constraints_colors,
+            old_constraints_colors,
             colored,
             constraints_rands,
+            color_bucket_counts,
+            color_bucket_starts,
+            color_bucket_cursors,
+            color_sorted_ids,
             curr_color: Tensor::scalar(
                 backend,
                 0u32,
@@ -771,6 +834,7 @@ impl RbdState {
                     | BufferUsages::COPY_SRC,
             )
             .unwrap(),
+            color_uniforms: Vec::new(),
             uncolored: Tensor::scalar(
                 backend,
                 0,
@@ -788,6 +852,7 @@ impl RbdState {
             prefix_sum_workspace: PrefixSumWorkspace::default(),
             lbvh: LbvhState::with_usages(backend, lbvh_usages),
             max_colors: capacities.solver_colors,
+            rb_contacts_inert,
             num_active_colliders: num_colliders as u32,
             num_active_bodies: num_bodies as u32,
         }
diff --git a/src_rbd/pipeline/rbd_step.rs b/src_rbd/pipeline/rbd_step.rs
index 64817d2..f2a83cf 100644
--- a/src_rbd/pipeline/rbd_step.rs
+++ b/src_rbd/pipeline/rbd_step.rs
@@ -1,6 +1,6 @@
 //! The [`RbdPipeline`] running one full simulation step on the GPU.
 
-use crate::broad_phase::{GpuNarrowPhase, Lbvh};
+use crate::broad_phase::{BRUTE_FORCE_MAX_COLLIDERS, GpuNarrowPhase, Lbvh};
 #[cfg(feature = "dim3")]
 use crate::dynamics::GpuMultibodySolver;
 use crate::dynamics::{
@@ -68,13 +68,24 @@ impl RbdPipeline {
     ) -> Result<RunStats, GpuBackendError> {
         let mut stats = RunStats::default();
 
+        // Make sure the color index uniforms are up-to-date.
+        // This is the maximum over the colors needed for contacts, joints, and multibodies.
+        {
+            let mut needed = state.max_colors + 2;
+            needed = needed.max(state.joints.num_colors() + 1);
+            #[cfg(feature = "dim3")]
+            {
+                needed = needed.max(state.multibodies.mb_imp_joint_num_colors() + 1);
+            }
+            state.ensure_color_uniforms(backend, needed);
+        }
+
+        let mut encoder = backend.begin_encoding();
+
         // Phase 0: Multibody once-per-visible-step setup (3D only for now).
         #[cfg(feature = "dim3")]
         {
             if !state.multibodies.is_empty() {
-                let mut encoder = backend.begin_encoding();
-                let mut pass =
-                    encoder.begin_pass("[RBD] multibody-init-step", timestamps.as_deref_mut());
                 let mut args = crate::dynamics::MultibodySolverArgs {
                     poses: &mut state.body_poses,
                     collider_world_poses: &state.collider_world_poses,
@@ -83,17 +94,20 @@ impl RbdPipeline {
                     contacts_len: &state.contacts_len,
                     solver_vels: &mut state.solver_vels,
                     batch_indices: &state.batch_indices,
+                    color_uniforms: &state.color_uniforms,
+                    mb_sweep_indirect: &state.mb_sweep_indirect,
                 };
-                self.multibody_solver
-                    .init_step(&mut pass, &mut state.multibodies, &mut args)?;
-                drop(pass);
-                backend.submit(encoder)?;
+                self.multibody_solver.init_step(
+                    &mut encoder,
+                    timestamps.as_deref_mut(),
+                    &mut state.multibodies,
+                    &mut args,
+                )?;
             }
         }
 
         // Phase 1: Update mass properties, build LBVH, and find collision pairs.
         {
-            let mut encoder = backend.begin_encoding();
             let mut pass = encoder.begin_pass("[RBD] update-mprops", timestamps.as_deref_mut());
 
             // Update mass properties — uses body world poses to compute the
@@ -122,60 +136,106 @@ impl RbdPipeline {
 
             drop(pass);
 
-            // Build LBVH and find collision pairs.
-            self.lbvh.update_tree(
-                backend,
-                &mut encoder,
-                &mut state.lbvh,
-                state.collider_local_poses.len() as u32,
-                state.num_active_colliders,
-                state.num_batches,
-                &state.collider_world_poses,
-                &state.vertex_buffers,
-                &state.shapes,
-                &state.batch_indices,
-                timestamps.as_deref_mut(),
-            )?;
-
-            // Debug: validate LBVH topology after tree construction
-            if crate::VALIDATE_LBVH_TOPOLOGY {
+            let use_bf = state.num_active_colliders <= BRUTE_FORCE_MAX_COLLIDERS
+                && std::env::var("NEXUS_DISABLE_BF").is_err();
+            if use_bf {
+                let mut pass =
+                    encoder.begin_pass("[RBD] bf-find-pairs", timestamps.as_deref_mut());
+                self.lbvh.brute_force_pairs(
+                    backend,
+                    &mut pass,
+                    &mut state.lbvh,
+                    state.collider_local_poses.len() as u32,
+                    state.num_active_colliders,
+                    state.num_batches,
+                    &state.collider_world_poses,
+                    &state.vertex_buffers,
+                    &state.shapes,
+                    &state.batch_indices,
+                    &mut state.collision_pairs,
+                    &mut state.collision_pairs_len,
+                    &mut state.collision_pairs_indirect,
+                    &state.collision_groups,
+                    &state.pair_filter,
+                )?;
+                drop(pass);
                 backend.submit(encoder)?;
+            } else {
+                // Build LBVH and find collision pairs.
+                self.lbvh.update_tree(
+                    backend,
+                    &mut encoder,
+                    &mut state.lbvh,
+                    state.collider_local_poses.len() as u32,
+                    state.num_active_colliders,
+                    state.num_batches,
+                    &state.collider_world_poses,
+                    &state.vertex_buffers,
+                    &state.shapes,
+                    &state.batch_indices,
+                    timestamps.as_deref_mut(),
+                )?;
 
-                let num_colliders = state.collider_world_poses.len() as u32;
-                let tree: Vec<LbvhNode> =
-                    futures::executor::block_on(backend.slow_read_vec(state.lbvh.tree().buffer()))?;
-                let sorted_colliders: Vec<u32> = futures::executor::block_on(
-                    backend.slow_read_vec(state.lbvh.sorted_colliders().buffer()),
+                // Debug: validate LBVH topology after tree construction
+                if crate::VALIDATE_LBVH_TOPOLOGY {
+                    backend.submit(encoder)?;
+
+                    let num_colliders = state.collider_world_poses.len() as u32;
+                    let tree: Vec<LbvhNode> = futures::executor::block_on(
+                        backend.slow_read_vec(state.lbvh.tree().buffer()),
+                    )?;
+                    let sorted_colliders: Vec<u32> = futures::executor::block_on(
+                        backend.slow_read_vec(state.lbvh.sorted_colliders().buffer()),
+                    )?;
+                    validate_lbvh_topology(&tree, &sorted_colliders, num_colliders);
+
+                    encoder = backend.begin_encoding();
+                    let _pass = encoder
+                        .begin_pass("[RBD] broad-phase-find-pairs", timestamps.as_deref_mut());
+                }
+
+                let mut pass =
+                    encoder.begin_pass("[RBD] lbvh-find-pairs", timestamps.as_deref_mut());
+                self.lbvh.find_pairs(
+                    &mut pass,
+                    &mut state.lbvh,
+                    state.num_active_colliders,
+                    state.num_batches,
+                    &state.batch_indices,
+                    &mut state.collision_pairs,
+                    &mut state.collision_pairs_len,
+                    &mut state.collision_pairs_indirect,
+                    &state.collision_groups,
+                    &state.pair_filter,
                 )?;
-                validate_lbvh_topology(&tree, &sorted_colliders, num_colliders);
 
-                encoder = backend.begin_encoding();
-                let _pass =
-                    encoder.begin_pass("[RBD] broad-phase-find-pairs", timestamps.as_deref_mut());
+                drop(pass);
+                backend.submit(encoder)?;
             }
+        }
 
-            let mut pass = encoder.begin_pass("[RBD] lbvh-find-pairs", timestamps.as_deref_mut());
-            self.lbvh.find_pairs(
-                &mut pass,
-                &mut state.lbvh,
-                state.num_active_colliders,
-                state.num_batches,
-                &state.batch_indices,
-                &mut state.collision_pairs,
-                &mut state.collision_pairs_len,
-                &mut state.collision_pairs_indirect,
-                &state.collision_groups,
-            )?;
+        let readback_enabled = state.capacities.solver_colors_resize_policy
+            != RbdResizePolicy::Fixed
+            || state.capacities.collisions_resize_policy != RbdResizePolicy::Fixed;
+        let est_pairs = if readback_enabled {
+            state.collision_pairs_len_cpu
+        } else {
+            state.collision_pairs_per_batch_cpu
+        };
 
-            drop(pass);
-            backend.submit(encoder)?;
-        }
+        // Choose the kernel depending on the expected pairs count.
+        // Small pairs with many environment benefit from the fused kernels.
+        let fused_color_sweeps = est_pairs <= 128;
+
+        // In small scenes, submit less frequently. In big scenes submit more
+        // to overlap compute and encoding.
+        let merge_submits = fused_color_sweeps && state.num_batches <= 64;
 
         // Phase 2a: Narrow phase. Split out from solver-prep + coloring
         // so its CPU encoding overlaps with Phase 1's GPU work and its
         // own GPU work overlaps with Phase 2b's CPU encoding.
+        let mut encoder = backend.begin_encoding();
         {
-            let mut encoder = backend.begin_encoding();
             let mut pass = encoder.begin_pass("[RBD] narrow-phase", timestamps.as_deref_mut());
 
             self.narrow_phase.dispatch(
@@ -191,6 +251,7 @@ impl RbdPipeline {
                 &mut state.contacts,
                 &mut state.contacts_len,
                 &mut state.contacts_indirect,
+                &mut state.mb_sweep_indirect,
                 &mut state.pfm_pairs,
                 &mut state.pfm_pairs_len,
                 &mut state.pfm_pairs_indirect,
@@ -200,14 +261,16 @@ impl RbdPipeline {
             )?;
 
             drop(pass);
-            backend.submit(encoder)?;
+            if !merge_submits {
+                backend.submit(encoder)?;
+                encoder = backend.begin_encoding();
+            }
         }
 
         // Phase 2b: solver-prep + warmstart + bounded coloring. Separate
         // submit from narrow-phase to enable CPU/GPU overlap with the
         // upcoming Phase 3 solver substep loop.
         {
-            let mut encoder = backend.begin_encoding();
             let mut pass = encoder.begin_pass("[RBD] solver-prep", timestamps.as_deref_mut());
 
             // Solver preparation - create args here to avoid borrow conflicts
@@ -224,14 +287,14 @@ impl RbdPipeline {
                 collider_world_poses: &state.collider_world_poses,
                 vels: &mut state.vels,
                 solver_vels: &mut state.solver_vels,
-                solver_vels_out: &state.solver_vels_out,
                 solver_vels_inc: &mut state.solver_vels_inc,
                 mprops: &state.mprops,
                 local_mprops: &state.local_mprops,
                 body_constraint_counts: &mut state.new_constraints_counts,
                 body_constraint_ids: &mut state.new_body_constraint_ids,
-                constraints_colors: &state.constraints_colors,
-                curr_color: &mut state.curr_color,
+                color_bucket_starts: &state.color_bucket_starts,
+                color_sorted_ids: &state.color_sorted_ids,
+                color_uniforms: &state.color_uniforms,
                 prefix_sum: &self.prefix_sum,
                 num_colors: 0,
                 num_batches: state.num_batches,
@@ -239,6 +302,10 @@ impl RbdPipeline {
                 num_solver_iterations: state.num_solver_iterations,
                 body_group: &state.body_group,
                 batch_indices: &state.batch_indices,
+                mb_sweep_indirect: &state.mb_sweep_indirect,
+                colorless_warmstart: false,
+                fused_color_sweeps,
+                rb_contacts_inert: state.rb_contacts_inert,
             };
             self.solver.prepare(
                 backend,
@@ -247,6 +314,11 @@ impl RbdPipeline {
                 &mut state.prefix_sum_workspace,
             )?;
 
+            if state.rb_contacts_inert {
+                stats.num_colors = state.max_colors + 1;
+                drop(pass);
+            } else {
+
             // Warmstart
             let warmstart_args = WarmstartArgs {
                 contacts_len: &state.contacts_len,
@@ -263,6 +335,40 @@ impl RbdPipeline {
             self.warmstart
                 .transfer_warmstart_impulses(&mut pass, warmstart_args)?;
 
+            let coloring_args = ColoringArgs {
+                contacts_len_indirect: &state.contacts_indirect,
+                body_constraint_counts: &state.new_constraints_counts,
+                body_constraint_ids: &state.new_body_constraint_ids,
+                constraints: &state.new_constraints,
+                constraints_colors: &mut state.constraints_colors,
+                constraints_rands: &mut state.constraints_rands,
+                curr_color: &mut state.curr_color,
+                uncolored: &mut state.uncolored,
+                uncolored_staging: &state.uncolored_staging,
+                contacts_len: &state.contacts_len,
+                colored: &mut state.colored,
+                batch_indices: &state.batch_indices,
+                body_group: &state.body_group,
+            };
+            self.coloring.dispatch_topo_gc_reset(&mut pass, coloring_args)?;
+
+            // Seed the coloring from the previous frame's colors (contacts
+            // persist, so most constraints can reuse their old color and the
+            // topo-gc iterations converge in 1-2 rounds instead of ~num_colors).
+            let seed_args = crate::dynamics::warmstart::SeedColorsArgs {
+                contacts_len: &state.contacts_len,
+                old_body_constraint_counts: &state.old_constraints_counts,
+                old_body_constraint_ids: &state.old_body_constraint_ids,
+                old_constraints: &state.old_constraints,
+                new_constraints: &state.new_constraints,
+                old_constraints_colors: &state.old_constraints_colors,
+                constraints_colors: &mut state.constraints_colors,
+                colored: &mut state.colored,
+                contacts_len_indirect: &state.contacts_indirect,
+                batch_indices: &state.batch_indices,
+            };
+            self.warmstart.seed_colors_from_warmstart(&mut pass, seed_args)?;
+
             let coloring_args = ColoringArgs {
                 contacts_len_indirect: &state.contacts_indirect,
                 body_constraint_counts: &state.new_constraints_counts,
@@ -279,14 +385,37 @@ impl RbdPipeline {
                 body_group: &state.body_group,
             };
             self.coloring
-                .dispatch_topo_gc_bounded(&mut pass, coloring_args, state.max_colors)?;
+                .dispatch_topo_gc_iterations(&mut pass, coloring_args, state.max_colors)?;
+
+            // Bucket-sort the constraint ids by color so each colored solver
+            // sweep only touches its own constraints.
+            let bucket_args = crate::dynamics::ColorBucketsArgs {
+                contacts_len_indirect: &state.contacts_indirect,
+                constraints_colors: &state.constraints_colors,
+                contacts_len: &state.contacts_len,
+                color_bucket_counts: &mut state.color_bucket_counts,
+                color_bucket_starts: &mut state.color_bucket_starts,
+                color_bucket_cursors: &mut state.color_bucket_cursors,
+                color_sorted_ids: &mut state.color_sorted_ids,
+                batch_indices: &state.batch_indices,
+            };
+            self.coloring.dispatch_build_color_buckets(
+                &mut pass,
+                bucket_args,
+                state.max_colors + 3,
+                state.num_batches,
+            )?;
 
             // `+1` because solver iterates 1..=max_colors (color 0 is unassigned).
             let num_colors = state.max_colors + 1;
             stats.num_colors = num_colors;
 
             drop(pass);
-            backend.submit(encoder)?;
+            }
+            if !merge_submits {
+                backend.submit(encoder)?;
+                encoder = backend.begin_encoding();
+            }
         }
 
         let num_colors = stats.num_colors;
@@ -305,14 +434,14 @@ impl RbdPipeline {
             collider_world_poses: &state.collider_world_poses,
             vels: &mut state.vels,
             solver_vels: &mut state.solver_vels,
-            solver_vels_out: &state.solver_vels_out,
             solver_vels_inc: &mut state.solver_vels_inc,
             mprops: &state.mprops,
             local_mprops: &state.local_mprops,
             body_constraint_counts: &mut state.new_constraints_counts,
             body_constraint_ids: &mut state.new_body_constraint_ids,
-            constraints_colors: &state.constraints_colors,
-            curr_color: &mut state.curr_color,
+            color_bucket_starts: &state.color_bucket_starts,
+            color_sorted_ids: &state.color_sorted_ids,
+            color_uniforms: &state.color_uniforms,
             prefix_sum: &self.prefix_sum,
             num_colors,
             num_batches: state.num_batches,
@@ -320,6 +449,15 @@ impl RbdPipeline {
             num_solver_iterations: state.num_solver_iterations,
             body_group: &state.body_group,
             batch_indices: &state.batch_indices,
+            mb_sweep_indirect: &state.mb_sweep_indirect,
+            // The gather warmstart is only valid without multibody grouping;
+            // see `SolverArgs::colorless_warmstart`.
+            #[cfg(feature = "dim3")]
+            colorless_warmstart: state.multibodies.is_empty(),
+            #[cfg(not(feature = "dim3"))]
+            colorless_warmstart: true,
+            fused_color_sweeps,
+            rb_contacts_inert: state.rb_contacts_inert,
         };
 
         // Phase 3: Solve constraints
@@ -330,11 +468,10 @@ impl RbdPipeline {
             local_mprops: &state.local_mprops,
             joints: &mut state.joints,
             batch_indices: &state.batch_indices,
+            color_uniforms: &state.color_uniforms,
         };
 
         {
-            let mut encoder = backend.begin_encoding();
-            let mut pass = encoder.begin_pass("[RBD] solver", timestamps.as_deref_mut());
             #[cfg(feature = "dim3")]
             let mb = if state.multibodies.is_empty() {
                 None
@@ -342,14 +479,14 @@ impl RbdPipeline {
                 Some((&self.multibody_solver, &mut state.multibodies))
             };
             self.solver.solve_tgs(
-                &mut pass,
+                &mut encoder,
+                timestamps.as_deref_mut(),
                 &self.joint_solver,
                 solver_args,
                 joint_solver_args,
                 #[cfg(feature = "dim3")]
                 mb,
             )?;
-            drop(pass);
 
             // Resolve all accumulated timestamps before the final submit.
             if let Some(ts) = &timestamps {
@@ -372,6 +509,10 @@ impl RbdPipeline {
             &mut state.old_constraints_counts,
             &mut state.new_constraints_counts,
         );
+        std::mem::swap(
+            &mut state.old_constraints_colors,
+            &mut state.constraints_colors,
+        );
 
         Ok(stats)
     }
@@ -405,8 +546,22 @@ impl RbdPipeline {
             //       count earlier, before it gets a chance to fail.
             if state.capacities.solver_colors_resize_policy != RbdResizePolicy::Fixed
                 && coloring_converged == 0
+                && !state.rb_contacts_inert
             {
                 state.max_colors += 5;
+
+                // The color-bucket buffers are strided by `max_colors + 3`:
+                // regrow them and update the stride in `BatchIndices`.
+                let storage: BufferUsages = BufferUsages::STORAGE | BufferUsages::COPY_SRC;
+                let stride = state.max_colors + 3;
+                let nb = state.num_batches;
+                state.color_bucket_counts =
+                    Tensor::vector_uninit(backend, stride * nb, storage)?;
+                state.color_bucket_starts =
+                    Tensor::vector_uninit(backend, stride * nb, storage)?;
+                state.color_bucket_cursors =
+                    Tensor::vector_uninit(backend, stride * nb, storage)?;
+                state.rebuild_batch_indices(backend);
             }
 
             // Lazy resize based on the *previous* frame's max pair count.
@@ -449,9 +604,15 @@ impl RbdPipeline {
                     Tensor::vector_uninit(backend, new_capacity * 2 * nb, storage)?;
                 state.constraints_colors =
                     Tensor::vector_uninit(backend, new_capacity * nb, storage)?;
+                // Zeroed (not uninit): 0 = "uncolored" disables color seeding
+                // for the frame right after the resize.
+                state.old_constraints_colors =
+                    Tensor::vector(backend, &vec![0u32; (new_capacity * nb) as usize], storage)?;
                 state.colored = Tensor::vector_uninit(backend, new_capacity * nb, storage)?;
                 state.constraints_rands =
                     Tensor::vector_uninit(backend, new_capacity * nb, storage)?;
+                state.color_sorted_ids =
+                    Tensor::vector_uninit(backend, new_capacity * nb, storage)?;
 
                 state.collision_pairs_per_batch_cpu = new_capacity;
                 state.contacts_per_batch_cpu = new_capacity;
diff --git a/src_rbd/utils/radix_sort/mod.rs b/src_rbd/utils/radix_sort/mod.rs
index 6111942..667520e 100644
--- a/src_rbd/utils/radix_sort/mod.rs
+++ b/src_rbd/utils/radix_sort/mod.rs
@@ -38,6 +38,9 @@ pub struct RadixSort {
 /// intermediate buffers as needed.
 pub struct RadixSortWorkspace {
     pass_uniforms: Vec<Tensor<SortUniforms>>,
+    /// Configuration `(mode, sorting_bits, max_keys, num_batches)` the cached
+    /// `pass_uniforms` / `n_sort_flat` were built for.
+    uniforms_key: (u32, u32, u32, u32),
     reduced_buf: Tensor<u32>, // Tensor of size BLOCK_SIZE
     count_buf: Tensor<u32>,
     num_wgs: Tensor<[u32; 3]>,
@@ -57,6 +60,7 @@ impl RadixSortWorkspace {
         let zeros = vec![0u32; BLOCK_SIZE as usize];
         Self {
             pass_uniforms: vec![],
+            uniforms_key: (u32::MAX, 0, 0, 0),
             reduced_buf: Tensor::vector(backend, &zeros, BufferUsages::STORAGE).unwrap(),
             count_buf: Tensor::vector_uninit(backend, 0, BufferUsages::STORAGE).unwrap(),
             num_wgs: Tensor::scalar(
@@ -254,18 +258,22 @@ impl RadixSort {
 
         let num_passes = sorting_bits.div_ceil(4);
 
-        // Create uniforms (has_aux=0 for single batch).
-        workspace.pass_uniforms.clear();
-        for pass_id in 0..num_passes {
-            workspace.pass_uniforms.push(Tensor::scalar(
-                backend,
-                SortUniforms {
-                    shift: pass_id * 4,
-                    max_keys_per_batch: per_batch_max,
-                    has_aux: 0,
-                },
-                BufferUsages::STORAGE | BufferUsages::UNIFORM,
-            )?);
+        // Create uniforms (has_aux=0 for single batch), cached across calls.
+        let uniforms_key = (0, sorting_bits, per_batch_max, 1);
+        if workspace.uniforms_key != uniforms_key {
+            workspace.pass_uniforms.clear();
+            for pass_id in 0..num_passes {
+                workspace.pass_uniforms.push(Tensor::scalar(
+                    backend,
+                    SortUniforms {
+                        shift: pass_id * 4,
+                        max_keys_per_batch: per_batch_max,
+                        has_aux: 0,
+                    },
+                    BufferUsages::STORAGE | BufferUsages::UNIFORM,
+                )?);
+            }
+            workspace.uniforms_key = uniforms_key;
         }
 
         let mut output_keys = output_keys;
@@ -402,41 +410,47 @@ impl RadixSort {
                 Tensor::vector_uninit(backend, total_n, BufferUsages::STORAGE)?;
         }
 
-        // n_sort_flat = [total_n] for the flattened single-batch view.
-        workspace.n_sort_flat = Tensor::scalar(backend, total_n, BufferUsages::STORAGE)?;
+        // The pass uniforms and the flattened count only depend on
+        // `(sorting_bits, total_n, num_batches)`; cache them across calls.
+        let init_uniform_idx = total_passes as usize;
+        let uniforms_key = (1, sorting_bits, total_n, num_batches);
+        if workspace.uniforms_key != uniforms_key {
+            // n_sort_flat = [total_n] for the flattened single-batch view.
+            workspace.n_sort_flat = Tensor::scalar(backend, total_n, BufferUsages::STORAGE)?;
+
+            // Create uniforms for all passes.
+            workspace.pass_uniforms.clear();
+            for pass_id in 0..total_passes {
+                let shift = if pass_id < key_passes {
+                    pass_id * 4
+                } else {
+                    (pass_id - key_passes) * 4
+                };
+                workspace.pass_uniforms.push(Tensor::scalar(
+                    backend,
+                    SortUniforms {
+                        shift,
+                        max_keys_per_batch: total_n,
+                        has_aux: 1,
+                    },
+                    BufferUsages::STORAGE | BufferUsages::UNIFORM,
+                )?);
+            }
 
-        // Create uniforms for all passes.
-        workspace.pass_uniforms.clear();
-        for pass_id in 0..total_passes {
-            let shift = if pass_id < key_passes {
-                pass_id * 4
-            } else {
-                (pass_id - key_passes) * 4
-            };
+            // Extra uniform for init_batched (max_keys_per_batch = per_batch, not total_n).
+            // shift is repurposed to carry num_batches for this kernel.
             workspace.pass_uniforms.push(Tensor::scalar(
                 backend,
                 SortUniforms {
-                    shift,
-                    max_keys_per_batch: total_n,
-                    has_aux: 1,
+                    shift: num_batches,
+                    max_keys_per_batch: per_batch,
+                    has_aux: 0,
                 },
                 BufferUsages::STORAGE | BufferUsages::UNIFORM,
             )?);
+            workspace.uniforms_key = uniforms_key;
         }
 
-        // Extra uniform for init_batched (max_keys_per_batch = per_batch, not total_n).
-        // shift is repurposed to carry num_batches for this kernel.
-        let init_uniform_idx = total_passes as usize;
-        workspace.pass_uniforms.push(Tensor::scalar(
-            backend,
-            SortUniforms {
-                shift: num_batches,
-                max_keys_per_batch: per_batch,
-                has_aux: 0,
-            },
-            BufferUsages::STORAGE | BufferUsages::UNIFORM,
-        )?);
-
         // Init writes to output buffers. After even total_passes, data stays in output.
         // NOTE: call() takes a thread count, not workgroup count (khal resolves internally).
         self.init_batched.call(
diff --git a/src_rbd_shaders/broad_phase/brute_force.rs b/src_rbd_shaders/broad_phase/brute_force.rs
new file mode 100644
index 0000000..fada07e
--- /dev/null
+++ b/src_rbd_shaders/broad_phase/brute_force.rs
@@ -0,0 +1,108 @@
+//! Brute-force O(n²) broad phase for tiny environments.
+//!
+//! When each batch holds only a handful of colliders (which happens often
+//! in single-robot many-batches simulations), the BVH broad-phase is significantly
+//! slower than a naive brute-force approach.
+use khal_std::glamx::UVec3;
+use khal_std::index::MaybeIndexUnchecked;
+use khal_std::macros::{spirv, spirv_bindgen};
+use khal_std::sync::atomic_add_u32;
+
+use crate::bounding_volumes::Aabb;
+use crate::broad_phase::CollisionPair;
+use crate::shapes::Shape;
+use crate::utils::BatchIndices;
+use crate::{PaddedVector, Pose, Vector};
+use glamx::UVec2;
+use rapier::geometry::InteractionGroups;
+
+use super::narrow_phase::PREDICTION;
+
+/// Computes every active collider's world AABB.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_bf_compute_aabbs(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] poses: &[Pose],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] shapes: &[Shape],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] aabbs: &mut [Aabb],
+    #[spirv(uniform, descriptor_set = 0, binding = 3)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 0)] vertices: &[PaddedVector],
+) {
+    let n = batch_ids.colliders_len;
+    if invocation_id.x >= n * batch_ids.num_batches {
+        return;
+    }
+    let batch_id = invocation_id.x / n;
+    let i = invocation_id.x % n;
+
+    let poses = batch_ids.coll_batch(batch_id, poses);
+    let shapes = batch_ids.coll_batch(batch_id, shapes);
+    let out = batch_ids.coll_start(batch_id) + i as usize;
+    aabbs.write(out, shapes[i as usize].compute_aabb(poses[i as usize], vertices));
+}
+
+/// Tests every collider pair of every batch and appends the intersecting,
+/// unfiltered ones to `collision_pairs`.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_bf_find_pairs(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] aabbs: &[Aabb],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    collision_pairs: &mut [CollisionPair],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] collision_pairs_len: &mut [u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)]
+    collision_groups: &[InteractionGroups],
+    #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] pair_filter: &[[u32; 2]],
+) {
+    let n = batch_ids.colliders_len;
+    let nn = n * n;
+    if invocation_id.x >= nn * batch_ids.num_batches {
+        return;
+    }
+    let batch_id = invocation_id.x / nn;
+    let r = invocation_id.x % nn;
+    let i = r / n;
+    let j = r % n;
+    if i >= j {
+        return;
+    }
+
+    let collision_groups = batch_ids.coll_batch(batch_id, collision_groups);
+    let pair_filter = batch_ids.coll_batch(batch_id, pair_filter);
+
+    // Skip pairs whose collision groups don't authorize an interaction.
+    if !collision_groups[i as usize].test(collision_groups[j as usize]) {
+        return;
+    }
+
+    // Built-in filters (same-body and adjacent-multibody-links).
+    let filter_i = pair_filter[i as usize];
+    let filter_j = pair_filter[j as usize];
+    if filter_i[0] == filter_j[0] || (filter_i[1] != 0 && filter_i[1] == filter_j[1]) {
+        return;
+    }
+
+    let coll_start = batch_ids.coll_start(batch_id);
+    // Dilate one side by the contact prediction distance.
+    let mut aabb_i = aabbs.read(coll_start + i as usize);
+    let dilation = Vector::splat(PREDICTION);
+    aabb_i.mins -= dilation;
+    aabb_i.maxs += dilation;
+    let aabb_j = aabbs.read(coll_start + j as usize);
+    if !aabb_i.intersects(&aabb_j) {
+        return;
+    }
+
+    let target_pair_index = atomic_add_u32(collision_pairs_len.at_mut(batch_id as usize), 1);
+
+    // If we exceed capacity, keep counting the pairs but don’t store any more to avoid overflow.
+    if target_pair_index < batch_ids.collision_pairs_batch_capacity {
+        let mut collision_pairs = batch_ids.collision_pairs_batch_mut(batch_id, collision_pairs);
+        collision_pairs[target_pair_index as usize] = CollisionPair {
+            colliders: UVec2::new(i, j),
+        };
+    }
+}
diff --git a/src_rbd_shaders/broad_phase/lbvh.rs b/src_rbd_shaders/broad_phase/lbvh.rs
index 350a49d..368e8d7 100644
--- a/src_rbd_shaders/broad_phase/lbvh.rs
+++ b/src_rbd_shaders/broad_phase/lbvh.rs
@@ -41,53 +41,82 @@ pub struct LbvhNode {
     pub right: u32,
     /// Parent node index.
     pub parent: u32,
-    /// Counter for bottom-up refitting (0, 1, or 2).
-    pub refit_count: u32,
+    /// During refit: bottom-up arrival counter; each thread atomically
+    /// increments it and only the second one continues upward (both children
+    /// ready). After refit, `refit_leaves` sets it to the maximum sorted leaf
+    /// index in this node’s subtree, which the pair traversal uses to prune
+    /// subtrees that can only produce duplicate pairs.
+    pub refit_count_or_max_subtree_index: u32,
 }
 
-/// Resets the collision pairs counter.
+/// Resets the collision pairs counter. One thread per batch.
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_lbvh_reset_collision_pairs(
-    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] collision_pairs_len: &mut [u32],
 ) {
-    let batch_id = workgroup_id.y as usize;
-
-    // NOTE: this `for` loop is silly. It doesn’t do anything
-    //       more than a `*collision_pairs_len = 0` in a convoluted
-    //       way because otherwise rustgpu apparently does not generate
-    //       the spirv for this kernel (seems to happen if the kernel is
-    //       too trivial.
-    for k in 0..1 {
-        collision_pairs_len.write(batch_id, k);
+    let batch_id = invocation_id.x as usize;
+    if batch_id < collision_pairs_len.len() {
+        collision_pairs_len.write(batch_id, 0);
+    }
+}
+
+/// Number of lanes used by the per-batch-count max reductions below. Their
+/// host dispatch is a single workgroup: `.call(pass, MAX_REDUCE_LANES, ...)`.
+pub const MAX_REDUCE_LANES: u32 = 256;
+
+/// Workgroup-parallel `max` over the per-batch counts, then writes the
+/// `[ceil(max/64), num_batches, 1]` indirect grid.
+///
+/// NOTE: `lens` is mutable even though we don't modify it: the loads must be
+/// atomic or they occasionally read stale data (breaks Windows+Nvidia+wgpu, see
+/// https://github.com/gfx-rs/wgpu/issues/9221).
+#[inline(always)]
+pub(crate) fn max_len_indirect_args(
+    lane: u32,
+    lens: &mut [u32],
+    indirect_args: &mut [u32; 3],
+    partial: &mut [u32; MAX_REDUCE_LANES as usize],
+) {
+    let num_batches = lens.len();
+
+    let mut m = 0u32;
+    for i in StepRng::new(lane..num_batches as u32, MAX_REDUCE_LANES) {
+        m = m.max(atomic_load_u32(lens.at_mut(i as usize)));
+    }
+    partial.write(lane as usize, m);
+    workgroup_memory_barrier_with_group_sync();
+
+    // Tree reduction over the 256 lanes (8 halving steps).
+    for step in 0..8u32 {
+        let stride = MAX_REDUCE_LANES >> (step + 1);
+        if lane < stride {
+            let v = partial
+                .read(lane as usize)
+                .max(partial.read((lane + stride) as usize));
+            partial.write(lane as usize, v);
+        }
+        workgroup_memory_barrier_with_group_sync();
+    }
+
+    if lane == 0 {
+        *indirect_args.at_mut(0) = partial.read(0).div_ceil(WORKGROUP_SIZE);
+        *indirect_args.at_mut(1) = num_batches as u32;
+        *indirect_args.at_mut(2) = 1;
     }
 }
 
 /// Initializes indirect dispatch arguments for narrow phase.
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(256)))]
 pub fn gpu_lbvh_init_dispatch(
-    // TODO: take the batch dimension as argument (instead of relying on the len of `collision_pairs_len`)?
-    // NOTE: the `collision_pairs_len` is mutable here even though we don’t modify it. That’s
-    //       because we access it with an atomic load otherwise it would occasionally read
-    //       stale data (on Windows+Nvidia+wgpu backend). This might be caused by:
-    //       https://github.com/gfx-rs/wgpu/issues/9221
+    #[spirv(local_invocation_id)] lid: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] collision_pairs_len: &mut [u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] indirect_args: &mut [u32; 3],
+    #[spirv(workgroup)] partial: &mut [u32; MAX_REDUCE_LANES as usize],
 ) {
-    // For indirect dispatch, get the largest length along all batch dimensions.
-    let num_batches = collision_pairs_len.len();
-    let mut highest_pairs_len = 0;
-    for batch_id in 0..num_batches {
-        // NOTE: atomic_load is needed for correctness on some platforms (see comment above `collision_pairs_len`).
-        highest_pairs_len =
-            highest_pairs_len.max(atomic_load_u32(collision_pairs_len.at_mut(batch_id)));
-    }
-
-    *indirect_args.at_mut(0) = highest_pairs_len.div_ceil(WORKGROUP_SIZE);
-    *indirect_args.at_mut(1) = num_batches as u32;
-    *indirect_args.at_mut(2) = 1;
+    max_len_indirect_args(lid.x, collision_pairs_len, indirect_args, partial);
 }
 
 /// Runs a reduction to compute the AABB of the collider positions.
@@ -278,7 +307,7 @@ pub fn gpu_lbvh_build(
 
         tree.at_mut(node_id as usize).left = left as u32;
         tree.at_mut(node_id as usize).right = right as u32;
-        tree.at_mut(node_id as usize).refit_count = 0; // Might as well reset the refit count here.
+        tree.at_mut(node_id as usize).refit_count_or_max_subtree_index = 0; // Might as well reset the refit count here.
         tree.at_mut(left as usize).parent = node_id;
         tree.at_mut(right as usize).parent = node_id;
     }
@@ -319,6 +348,9 @@ pub fn gpu_lbvh_refit_leaves(
 
         tree.at_mut(curr_leaf_id as usize).aabb = leaf_shape.compute_aabb(leaf_pose, vertices);
         tree.at_mut(curr_leaf_id as usize).left = leaf_collider;
+        // For leaves, we can set their index here. They don’t use the `refit_count`
+        // mechanism which is for internal nodes only.
+        tree.at_mut(curr_leaf_id as usize).refit_count_or_max_subtree_index = i;
     }
 }
 
@@ -342,12 +374,7 @@ pub fn gpu_lbvh_refit_internal(
     let first_leaf_id = num_bodies - 1;
 
     let mut tree = SliceMut(tree, root_id(colliders_start) as usize);
-
-    // All threads must execute the same number of outer loop iterations for uniform control flow.
-    // NOTE: we calculate the interation count based on `colliders_batch_capacity` instead of
-    //       `num_bodies` since the latter is non-uniform because it originates from a storage
-    //       buffer.
-    let num_iterations = batch_ids.colliders_batch_capacity.div_ceil(num_threads);
+    let num_iterations = num_bodies.div_ceil(num_threads);
 
     // NOTE: using unchecked indexing (via MaybeIndexUnchecked) because otherwise the bounds
     //        checking inserted by rustgpu breaks the shader when targeting some NVidia graphics
@@ -366,7 +393,7 @@ pub fn gpu_lbvh_refit_internal(
         // Maximum tree depth is log2(num_colliders), but we use 32 as a safe upper bound.
         for _level in 0..32u32 {
             if thread_is_active {
-                let refit_count = atomic_add_u32(&mut tree.at_mut(curr_id as usize).refit_count, 1);
+                let refit_count = atomic_add_u32(&mut tree.at_mut(curr_id as usize).refit_count_or_max_subtree_index, 1);
 
                 if refit_count == 0 {
                     // If `refit_count` was 0 then the other thread hasn't reached this node
@@ -385,6 +412,11 @@ pub fn gpu_lbvh_refit_internal(
                     let right = tree.at(right_idx as usize).aabb;
                     tree.at_mut(curr_id as usize).aabb = left.merged(&right);
 
+                    // Set `refit_count_or_max_subtree_index` to the max subtree leaf index.
+                    let max_l = tree.at(left_idx as usize).refit_count_or_max_subtree_index;
+                    let max_r = tree.at(right_idx as usize).refit_count_or_max_subtree_index;
+                    tree.at_mut(curr_id as usize).refit_count_or_max_subtree_index = max_l.max(max_r);
+
                     if curr_id == 0 {
                         // We reached the root, can't go higher.
                         thread_is_active = false;
@@ -449,8 +481,10 @@ pub fn gpu_lbvh_refit(
         // Propagate to ancestors.
         let mut curr_id = tree.at(curr_leaf_id as usize).parent;
 
-        loop {
-            let refit_count = atomic_add_u32(&mut tree.at_mut(curr_id as usize).refit_count, 1);
+        // NOTE: bounded `for` (tree depth <= 32 in practice) instead of `loop`
+        //       to avoid the MacOS miscompilation bug.
+        for _ in 0..32u32 {
+            let refit_count = atomic_add_u32(&mut tree.at_mut(curr_id as usize).refit_count_or_max_subtree_index, 1);
 
             if refit_count == 0 {
                 // If `refit_count` was 0 then the other thread hasn't reached this node
@@ -493,6 +527,7 @@ pub fn gpu_lbvh_find_collision_pairs(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 3)]
     collision_groups: &[InteractionGroups],
     #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] pair_filter: &[[u32; 2]],
 ) {
     let num_threads = num_workgroups.x * WORKGROUP_SIZE;
     let batch_id = invocation_id.y;
@@ -503,10 +538,12 @@ pub fn gpu_lbvh_find_collision_pairs(
     let mut collision_pairs = batch_ids.collision_pairs_batch_mut(batch_id, collision_pairs);
     let tree = Slice(tree, root_id(colliders_start) as usize);
     let collision_groups = batch_ids.coll_batch(batch_id, collision_groups);
+    let pair_filter = batch_ids.coll_batch(batch_id, pair_filter);
 
     for leaf_i in StepRng::new(invocation_id.x..num_bodies, num_threads) {
         let i = tree.at((first_leaf_id + leaf_i) as usize).left;
         let groups_i = collision_groups[i as usize];
+        let filter_i = pair_filter[i as usize];
         let mut aabb1 = tree.at((first_leaf_id + leaf_i) as usize).aabb;
         let prediction = 2.0e-3; // TODO: should be configurable.
         let dilation = Vector::splat(prediction);
@@ -518,7 +555,13 @@ pub fn gpu_lbvh_find_collision_pairs(
         let mut stack_len = 1u32;
         stack.write(0, 0);
 
-        while stack_len != 0 {
+        // NOTE: we use a fixed-size for loop to avoid miscompilation issues of
+        //       while loops on MacOs. Each tree node is pushed at most once per
+        //       traversal, so `2 * num_bodies` (≥ node count) bounds the loop.
+        for _ in 0..2 * num_bodies {
+            if stack_len == 0 {
+                break;
+            }
             stack_len -= 1;
             let curr_id = stack.read(stack_len as usize);
             let node = tree.at(curr_id as usize);
@@ -529,15 +572,16 @@ pub fn gpu_lbvh_find_collision_pairs(
                 let groups_j = collision_groups[j as usize];
 
                 // Skip pairs whose collision groups don't authorize an interaction.
-                // NOTE: same-body collider pairs are *not* filtered here — that
-                //       skip is deferred to the narrow-phase so the broad phase
-                //       never has to touch `collider_parent`.
                 if !groups_i.test(groups_j) {
                     continue;
                 }
 
-                // NOTE: we don't have to compare i < j to avoid duplicates since that comparison already happened
-                //       alongside the AABB check.
+                // Apply computed filters (same-body and self-contact).
+                let filter_j = pair_filter[j as usize];
+                if filter_i[0] == filter_j[0] || (filter_i[1] != 0 && filter_i[1] == filter_j[1]) {
+                    continue;
+                }
+
                 let target_pair_index =
                     atomic_add_u32(collision_pairs_len.at_mut(batch_id as usize), 1);
 
@@ -551,17 +595,19 @@ pub fn gpu_lbvh_find_collision_pairs(
                     //       by the solver — keeping this hot buffer (and the
                     //       intermediate pfm-pair buffer) narrow, and keeping
                     //       `collider_parent` out of the broad phase entirely.
+                    let (ci, cj) = if i < j { (i, j) } else { (j, i) };
                     collision_pairs[target_pair_index as usize] = CollisionPair {
-                        colliders: UVec2::new(i, j),
+                        colliders: UVec2::new(ci, cj),
                     };
                 }
             } else {
                 let left = node.left;
                 let right = node.right;
 
-                // Go on the child only if the AABB intersects and either the child isn't a leaf, or it is a leaf with associated collider
-                // smaller than `i` (to avoid duplicate pairs).
-                if (left < first_leaf_id || i < tree.at(left as usize).left)
+                // Descend only if the subtree contains leaf id smaller than
+                // `leaf_i`. That way we prune the part of the tree that’s
+                // on the "left" of `leaf_i`, avoiding duplicate pairs/traversals.
+                if leaf_i < tree.at(left as usize).refit_count_or_max_subtree_index
                     && aabb1.intersects(&tree.at(left as usize).aabb)
                     && stack_len < 64
                 {
@@ -569,8 +615,7 @@ pub fn gpu_lbvh_find_collision_pairs(
                     stack_len += 1;
                 }
 
-                // NOTE: on leaves (including tree[right]), the collider id is stored as the left child index.
-                if (right < first_leaf_id || i < tree.at(right as usize).left)
+                if leaf_i < tree.at(right as usize).refit_count_or_max_subtree_index
                     && aabb1.intersects(&tree.at(right as usize).aabb)
                     && stack_len < 64
                 {
diff --git a/src_rbd_shaders/broad_phase/mod.rs b/src_rbd_shaders/broad_phase/mod.rs
index 36d279f..43e94b7 100644
--- a/src_rbd_shaders/broad_phase/mod.rs
+++ b/src_rbd_shaders/broad_phase/mod.rs
@@ -5,6 +5,7 @@
 //! - LBVH (Linear Bounding Volume Hierarchy, for large scenes)
 
 // Data structures and algorithms
+mod brute_force;
 mod lbvh;
 
 // GPU compute shader kernels
@@ -14,12 +15,13 @@ use glamx::UVec2;
 // Re-export non-spirv items explicitly to avoid ambiguous glob re-exports.
 // The div_ceil functions have different signatures (u32 vs i32) so we pick one.
 // Spirv-only items (functions and generated structs) are re-exported via glob.
+pub use brute_force::*;
 pub use lbvh::*;
 #[cfg(feature = "dim2")]
 pub use lbvh::{expand_bits_2d, morton_2d};
 pub use narrow_phase::*;
 
-#[derive(Copy, Clone, PartialEq, Eq)]
+#[derive(Copy, Clone, PartialEq, Eq, Default)]
 #[cfg_attr(not(target_arch_is_gpu), derive(bytemuck::Pod, bytemuck::Zeroable))]
 #[repr(C)]
 pub struct CollisionPair {
diff --git a/src_rbd_shaders/broad_phase/narrow_phase.rs b/src_rbd_shaders/broad_phase/narrow_phase.rs
index 20a4ead..d910bb2 100644
--- a/src_rbd_shaders/broad_phase/narrow_phase.rs
+++ b/src_rbd_shaders/broad_phase/narrow_phase.rs
@@ -16,61 +16,61 @@ use khal_std::index::MaybeIndexUnchecked;
 use khal_std::macros::{spirv, spirv_bindgen};
 use khal_std::{
     iter::StepRng,
-    sync::{atomic_add_u32, atomic_load_u32},
+    sync::atomic_add_u32,
 };
 
+use super::lbvh::{MAX_REDUCE_LANES, max_len_indirect_args};
 use crate::broad_phase::CollisionPair;
 use crate::utils::{BatchIndices, SliceMut};
 use glamx::UVec2;
 
 const WORKGROUP_SIZE: u32 = 64;
 
-/// Resets the contacts counter.
+/// Resets the contacts counter. One thread per batch.
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_reset_narrow_phase(
-    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] contacts_len: &mut [u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] pfm_pairs_len: &mut [u32],
 ) {
-    let batch_id = workgroup_id.y as usize;
-
-    // NOTE: this `for` loop is silly. It doesn’t do anything
-    //       more than a `*contacts_len = 0` in a convoluted
-    //       way because otherwise rustgpu apparently does not generate
-    //       the spirv for this kernel (seems to happen if the kernel is
-    //       too trivial.
-    for k in 0..1 {
-        contacts_len.write(batch_id, k);
-        pfm_pairs_len.write(batch_id, k);
+    let batch_id = invocation_id.x as usize;
+    if batch_id < contacts_len.len() {
+        contacts_len.write(batch_id, 0);
+        pfm_pairs_len.write(batch_id, 0);
     }
 }
 
 /// Initializes indirect dispatch arguments for constraint solver.
+///
+/// Also inits `mb_sweep_indirect`, the workgroup grid for the per-multibody
+/// contact-constraint dispatches (`[multibodies_batch_capacity, num_batches,
+/// 1]`).
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(256)))]
 pub fn gpu_narrow_phase_init_contacts_dispatch(
-    // NOTE: the `contacts_len` is mutable here even though we don’t modify it. That’s
-    //       because we access it with an atomic load otherwise it would occasionally read
-    //       stale data (on Windows+Nvidia+wgpu backend). This might be caused by:
-    //       https://github.com/gfx-rs/wgpu/issues/9221
+    #[spirv(local_invocation_id)] lid: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] contacts_len: &mut [u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] indirect_args: &mut [u32; 3],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] mb_sweep_indirect: &mut [u32; 3],
+    #[spirv(uniform, descriptor_set = 0, binding = 3)] batch_ids: &BatchIndices,
+    #[spirv(workgroup)] partial: &mut [u32; MAX_REDUCE_LANES as usize],
 ) {
-    // For indirect dispatch, get the largest length along all batch dimensions.
-    let num_batches = contacts_len.len();
-    let mut highest_contacts_len = 0;
-    for i in 0..num_batches {
-        // NOTE: atomic_load is needed for correctness on some platforms (see comment above `contacts_len`).
-        highest_contacts_len = highest_contacts_len.max(atomic_load_u32(contacts_len.at_mut(i)));
+    max_len_indirect_args(lid.x, contacts_len, indirect_args, partial);
+    // `partial[0]` holds the max after the reduction (all lanes synced).
+    if lid.x == 0 {
+        let any_contacts = partial.read(0) > 0;
+        *mb_sweep_indirect.at_mut(0) = if any_contacts {
+            batch_ids.multibodies_batch_capacity
+        } else {
+            0
+        };
+        *mb_sweep_indirect.at_mut(1) = batch_ids.num_batches;
+        *mb_sweep_indirect.at_mut(2) = 1;
     }
-
-    *indirect_args.at_mut(0) = highest_contacts_len.div_ceil(WORKGROUP_SIZE);
-    *indirect_args.at_mut(1) = num_batches as u32;
-    *indirect_args.at_mut(2) = 1;
 }
 
-const PREDICTION: f32 = 2.0e-3; // TODO: make the prediction configurable.
+pub(crate) const PREDICTION: f32 = 2.0e-2; // TODO: make the prediction configurable.
 
 /// Narrow phase, pass 1 of 2: analytic shape-shape contacts for ball / cuboid
 /// pairs, written straight into the `contacts` buffer.
@@ -171,8 +171,7 @@ pub fn gpu_narrow_phase_shape_shape(
             let target_contact_index = atomic_add_u32(contacts_len, 1) as usize;
 
             // NOTE: if we exceed the contacts allocation size, just skip
-            //       the contact. It’s up to the caller to resize the buffer
-            //       and re-run the narrow-phase.
+            //       the contact.
             if target_contact_index < contacts_batch_capacity {
                 let mat1 = collider_materials[pair.colliders.x as usize];
                 let mat2 = collider_materials[pair.colliders.y as usize];
@@ -234,13 +233,10 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
     //       contact is written.
     for i in StepRng::new(invocation_id.x..len, num_threads) {
         let pair = collision_pairs[i as usize];
-        let pose1 = poses[pair.colliders.x as usize];
-        let pose2 = poses[pair.colliders.y as usize];
         let shape1 = &shapes[pair.colliders.x as usize];
         let shape2 = &shapes[pair.colliders.y as usize];
         let shape_ty1 = shape1.shape_type();
         let shape_ty2 = shape2.shape_type();
-        let pose12 = pose1.inverse() * pose2;
 
         // Mirror pass 1's analytic-pair predicate (ball/cuboid) so those pairs
         // are skipped here — they were already turned into contacts. Only the
@@ -267,6 +263,13 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
         if !checked && shape_ty1 == SHAPE_TYPE_CUBOID && shape_ty2 == SHAPE_TYPE_CUBOID {
             checked = true;
         }
+        if checked {
+            continue;
+        }
+
+        let pose1 = poses[pair.colliders.x as usize];
+        let pose2 = poses[pair.colliders.y as usize];
+        let pose12 = pose1.inverse() * pose2;
 
         // PFM - PFM (generic convex shapes via GJK/EPA)
         if !checked {
@@ -283,7 +286,10 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
                     colliders: pair.colliders,
                 };
                 let pfm_index = atomic_add_u32(pfm_pairs_len, 1);
-                pfm_pairs.write(pfm_index as usize, pfm_pair);
+                // NOTE: if we exceed capacity, just skip the pair.
+                if (pfm_index as usize) < contacts_batch_capacity {
+                    pfm_pairs.write(pfm_index as usize, pfm_pair);
+                }
 
                 // The actual calculations are deferred to another kernel.
                 continue;
@@ -302,6 +308,7 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
                 pair.colliders,
                 &mut pfm_pairs,
                 pfm_pairs_len,
+                contacts_batch_capacity,
                 vertices,
                 indices,
             );
@@ -319,6 +326,7 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
                 UVec2::new(pair.colliders.y, pair.colliders.x),
                 &mut pfm_pairs,
                 pfm_pairs_len,
+                contacts_batch_capacity,
                 vertices,
                 indices,
             );
@@ -337,6 +345,7 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
                 pair.colliders,
                 &mut pfm_pairs,
                 pfm_pairs_len,
+                contacts_batch_capacity,
                 vertices,
                 indices,
             );
@@ -354,6 +363,7 @@ pub fn gpu_narrow_phase_shape_shape_deferred(
                 UVec2::new(pair.colliders.y, pair.colliders.x),
                 &mut pfm_pairs,
                 pfm_pairs_len,
+                contacts_batch_capacity,
                 vertices,
                 indices,
             );
@@ -370,6 +380,7 @@ fn trimesh_convex(
     colliders: UVec2,
     pfm_pairs: &mut SliceMut<NarrowPhasePfmPair>,
     pfm_pairs_len: &mut u32,
+    pfm_pairs_capacity: usize,
     vertices: &[PaddedVector],
     indices: &[u32],
 ) {
@@ -413,7 +424,10 @@ fn trimesh_convex(
                 colliders,
             };
             let pfm_index = atomic_add_u32(pfm_pairs_len, 1);
-            pfm_pairs.write(pfm_index as usize, pfm_pair);
+            // Skip (don’t write) on overflow; the caller resizes and re-runs.
+            if (pfm_index as usize) < pfm_pairs_capacity {
+                pfm_pairs.write(pfm_index as usize, pfm_pair);
+            }
 
             // Continue traversal.
             curr = idx.exit_index;
@@ -436,6 +450,7 @@ fn polyline_convex(
     colliders: UVec2,
     pfm_pairs: &mut SliceMut<NarrowPhasePfmPair>,
     pfm_pairs_len: &mut u32,
+    pfm_pairs_capacity: usize,
     vertices: &[PaddedVector],
     indices: &[u32],
 ) {
@@ -482,7 +497,10 @@ fn polyline_convex(
                 colliders,
             };
             let pfm_index = atomic_add_u32(pfm_pairs_len, 1);
-            pfm_pairs.write(pfm_index as usize, pfm_pair);
+            // Skip (don’t write) on overflow; the caller resizes and re-runs.
+            if (pfm_index as usize) < pfm_pairs_capacity {
+                pfm_pairs.write(pfm_index as usize, pfm_pair);
+            }
 
             // Continue traversal.
             curr = idx.exit_index;
@@ -509,28 +527,17 @@ pub struct NarrowPhasePfmPair {
     colliders: UVec2,
 }
 
-/// Initializes PFM-PFM dispatch arguments for constraint solver.
+/// Initializes PFM-PFM dispatch arguments for constraint solver. Dispatch one
+/// [`MAX_REDUCE_LANES`]-thread workgroup.
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(256)))]
 pub fn gpu_init_pfm_pfm_dispatch(
-    // NOTE: the `pfm_pairs_len` is mutable here even though we don’t modify it. That’s
-    //       because we access it with an atomic load otherwise it would occasionally read
-    //       stale data (on Windows+Nvidia+wgpu backend). This might be caused by:
-    //       https://github.com/gfx-rs/wgpu/issues/9221
+    #[spirv(local_invocation_id)] lid: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] pfm_pairs_len: &mut [u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] indirect_args: &mut [u32; 3],
+    #[spirv(workgroup)] partial: &mut [u32; MAX_REDUCE_LANES as usize],
 ) {
-    let num_batches = pfm_pairs_len.len();
-    let mut highest_pfm_pairs_len = 0;
-    for batch_id in 0..num_batches {
-        // NOTE: atomic_load is needed for correctness on some platforms (see comment above `pfm_pairs_len`).
-        highest_pfm_pairs_len =
-            highest_pfm_pairs_len.max(atomic_load_u32(pfm_pairs_len.at_mut(batch_id)));
-    }
-    // TODO PERF: pfm_pfm is very divergent. Use a smaller workgroup size?
-    *indirect_args.at_mut(0) = highest_pfm_pairs_len.div_ceil(WORKGROUP_SIZE);
-    *indirect_args.at_mut(1) = num_batches as u32;
-    *indirect_args.at_mut(2) = 1;
+    max_len_indirect_args(lid.x, pfm_pairs_len, indirect_args, partial);
 }
 
 #[spirv_bindgen]
@@ -564,7 +571,11 @@ pub fn gpu_narrow_phase_pfm_pfm(
     let collider_materials = batch_ids.coll_batch(batch_id, collider_materials);
     let pfm_pairs = batch_ids.contact_batch(batch_id, pfm_pairs);
     let contacts_len = contacts_len.at_mut(batch_id as usize);
-    let pfm_pairs_len = pfm_pairs_len.read(batch_id as usize);
+    // The producer counter can exceed the allocation on overflow (writes are
+    // skipped past capacity); clamp so we never read uninitialized slots.
+    let pfm_pairs_len = pfm_pairs_len
+        .read(batch_id as usize)
+        .min(contacts_batch_capacity as u32);
 
     for i in StepRng::new(invocation_id.x..pfm_pairs_len, num_threads) {
         let pair = pfm_pairs[i as usize];
@@ -592,9 +603,7 @@ pub fn gpu_narrow_phase_pfm_pfm(
         if manifold.len > 0 && manifold.points_a.at(0).dist < PREDICTION {
             let target_contact_index = atomic_add_u32(contacts_len, 1) as usize;
 
-            // NOTE: if we exceed the contacts allocation size, just skip
-            //       the contact. It’s up to the caller to resize the buffer
-            //       and re-run the narrow-phase.
+            // NOTE: if we exceed capacity, just skip the pair.
             if target_contact_index < contacts_batch_capacity {
                 let mat1 = collider_materials[pair.colliders.x as usize];
                 let mat2 = collider_materials[pair.colliders.y as usize];
diff --git a/src_rbd_shaders/dynamics/color_buckets.rs b/src_rbd_shaders/dynamics/color_buckets.rs
new file mode 100644
index 0000000..0ee2fe2
--- /dev/null
+++ b/src_rbd_shaders/dynamics/color_buckets.rs
@@ -0,0 +1,120 @@
+//! Bucket-sort of contact constraints by graph-coloring color.
+//!
+//! After the per-step coloring converges, the constraint indices are
+//! bucket-sorted by color (`color_sorted_ids`, contacts layout) with
+//! per-batch per-color exclusive prefix sums (`color_starts`), so each
+//! colored solver sweep iterates only its own bucket instead of scanning the
+//! whole constraint buffer. The count/start/cursor buffers are flat
+//! `[num_batches × stride]` arrays with `stride =
+//! BatchIndices::solver_color_buckets_stride` (= `max_colors + 3`, keeping
+//! `starts[c + 1]` in bounds for every swept color).
+
+use khal_std::glamx::UVec3;
+use khal_std::macros::{spirv, spirv_bindgen};
+use khal_std::{index::MaybeIndexUnchecked, iter::StepRng, sync::atomic_add_u32};
+
+use crate::utils::BatchIndices;
+
+const WORKGROUP_SIZE: u32 = 64;
+
+/// Zeroes the per-batch per-color constraint counts.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_color_buckets_reset(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] color_counts: &mut [u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 1)] batch_ids: &BatchIndices,
+) {
+    let stride = batch_ids.solver_color_buckets_stride;
+    let batch_id = invocation_id.y;
+    let i = invocation_id.x;
+
+    if i < stride {
+        color_counts.write((batch_id * stride + i) as usize, 0);
+    }
+}
+
+/// Counts, per batch, how many constraints hold each color.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_color_buckets_count(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(num_workgroups)] num_workgroups: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] constraints_colors: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] contacts_len: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_counts: &mut [u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 3)] batch_ids: &BatchIndices,
+) {
+    let num_threads = num_workgroups.x * WORKGROUP_SIZE;
+    let batch_id = invocation_id.y;
+    let stride = batch_ids.solver_color_buckets_stride;
+
+    let constraints_colors = batch_ids.contact_batch(batch_id, constraints_colors);
+    let len = contacts_len
+        .read(batch_id as usize)
+        .min(batch_ids.contacts_batch_capacity);
+
+    for i in StepRng::new(invocation_id.x..len, num_threads) {
+        let color = constraints_colors[i as usize];
+        // Colors past the swept range (can happen if the bounded coloring
+        // didn't converge) are dropped; they were never solved before either.
+        if color < stride - 1 {
+            atomic_add_u32(color_counts.at_mut((batch_id * stride + color) as usize), 1);
+        }
+    }
+}
+
+/// Per-batch serial exclusive prefix sum over the (hopefully very small) per-color counts,
+/// producing bucket start offsets.
+#[spirv_bindgen]
+#[spirv(compute(threads(1)))]
+pub fn gpu_color_buckets_scan(
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] color_counts: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] color_starts: &mut [u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_cursors: &mut [u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 3)] batch_ids: &BatchIndices,
+) {
+    let stride = batch_ids.solver_color_buckets_stride;
+    let batch_id = workgroup_id.y;
+    let base = (batch_id * stride) as usize;
+
+    let mut acc = 0u32;
+    for c in 0..stride as usize {
+        color_starts.write(base + c, acc);
+        color_cursors.write(base + c, acc);
+        acc += color_counts.read(base + c);
+    }
+}
+
+/// Scatters each constraint index into its color's bucket.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_color_buckets_scatter(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(num_workgroups)] num_workgroups: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] constraints_colors: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] contacts_len: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_cursors: &mut [u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] color_sorted_ids: &mut [u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
+) {
+    let num_threads = num_workgroups.x * WORKGROUP_SIZE;
+    let batch_id = invocation_id.y;
+    let stride = batch_ids.solver_color_buckets_stride;
+
+    let constraints_colors = batch_ids.contact_batch(batch_id, constraints_colors);
+    let mut color_sorted_ids = batch_ids.contact_batch_mut(batch_id, color_sorted_ids);
+    let len = contacts_len
+        .read(batch_id as usize)
+        .min(batch_ids.contacts_batch_capacity);
+
+    for i in StepRng::new(invocation_id.x..len, num_threads) {
+        let color = constraints_colors[i as usize];
+        if color < stride - 1 {
+            let dst =
+                atomic_add_u32(color_cursors.at_mut((batch_id * stride + color) as usize), 1);
+            color_sorted_ids[dst as usize] = i;
+        }
+    }
+}
diff --git a/src_rbd_shaders/dynamics/constraint.rs b/src_rbd_shaders/dynamics/constraint.rs
index 415238e..559dfb8 100644
--- a/src_rbd_shaders/dynamics/constraint.rs
+++ b/src_rbd_shaders/dynamics/constraint.rs
@@ -193,8 +193,12 @@ pub struct TwoBodyConstraintNormalPart {
     /// Inverse effective mass: 1 / (m_eff)
     /// where m_eff = projected mass along constraint direction
     pub r: f32,
-    #[cfg(feature = "dim3")]
-    pub _padding3: f32,
+
+    /// Per-point softness: separated (speculative, `dist > 0`) points are solved
+    /// rigidly (`cfm_factor = 1`): perfectly inelastic touchdowns damp stack
+    /// rocking; a constraint-wide cfm under-stops each touchdown and pumps tall
+    /// stacks. Refreshed by `update_constraint` each substep.
+    pub cfm_factor: f32,
 }
 
 /// Tangent constraint data (friction).
diff --git a/src_rbd_shaders/dynamics/joint_constraint.rs b/src_rbd_shaders/dynamics/joint_constraint.rs
index 63774ff..04ba1e2 100644
--- a/src_rbd_shaders/dynamics/joint_constraint.rs
+++ b/src_rbd_shaders/dynamics/joint_constraint.rs
@@ -160,39 +160,6 @@ pub struct JointConstraintElement {
     pub impulse_bounds: Vec2,
 }
 
-/// Resets the joint color to 0.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_reset_joint_color(
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] curr_color: &mut u32,
-) {
-    // NOTE: this `for` loop is silly. It doesn’t do anything
-    //       more than a `*curr_color = 0` in a convoluted
-    //       way because otherwise rustgpu apparently does not generate
-    //       the spirv for this kernel (seems to happen if the kernel is
-    //       too trivial.
-    for k in 0..1 {
-        // NOTE: for joints, our first colors start at 0.
-        *curr_color = k;
-    }
-}
-
-/// Increments the joint color.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_inc_joint_color(
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] curr_color: &mut u32,
-) {
-    // NOTE: this `for` loop is silly. It doesn’t do anything
-    //       more than a `*curr_color += 1` in a convoluted
-    //       way because otherwise rustgpu apparently does not generate
-    //       the spirv for this kernel (seems to happen if the kernel is
-    //       too trivial.
-    for k in 0..1 {
-        *curr_color += 1 + k;
-    }
-}
-
 /// Initializes joint constraint builders and constraints.
 #[spirv_bindgen]
 #[spirv(compute(threads(64)))]
@@ -276,29 +243,6 @@ pub fn gpu_update_joint_constraints(
     }
 }
 
-/// Removes bias from joint constraints for the final substep.
-#[spirv_bindgen]
-#[spirv(compute(threads(64)))]
-pub fn gpu_remove_joint_bias(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(num_workgroups)] num_workgroups: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] constraints: &mut [JointConstraint],
-    #[spirv(uniform, descriptor_set = 0, binding = 1)] batch_ids: &BatchIndices,
-) {
-    let num_threads = num_workgroups.x * WORKGROUP_SIZE;
-    let batch_id = invocation_id.y;
-    let mut constraints = batch_ids.impulse_joints_batch_mut(batch_id, constraints);
-
-    let len = batch_ids.impulse_joints_len;
-
-    for i in StepRng::new(invocation_id.x..len, num_threads) {
-        let idx = i as usize;
-        for j in 0..(constraints[idx].len as usize) {
-            constraints[idx].elements.at_mut(j).rhs = constraints[idx].elements.at(j).rhs_wo_bias;
-        }
-    }
-}
-
 /// Solves joint constraints.
 #[spirv_bindgen]
 #[spirv(compute(threads(64)))]
@@ -310,12 +254,14 @@ pub fn gpu_solve_joint_constraints(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] all_color_groups: &[u32],
     #[spirv(uniform, descriptor_set = 0, binding = 3)] curr_color: &u32,
     #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] use_bias: &u32,
 ) {
     let num_threads = num_workgroups.x * WORKGROUP_SIZE;
     let batch_id = invocation_id.y;
 
     let mut constraints = batch_ids.impulse_joints_batch_mut(batch_id, constraints);
     let mut solver_vels = batch_ids.coll_batch_mut(batch_id, solver_vels);
+    let use_bias = *use_bias != 0;
 
     let color = *curr_color as usize;
     // Coloring is identical across batches (enforced on the host), so the
@@ -330,6 +276,6 @@ pub fn gpu_solve_joint_constraints(
     let end = color_groups[color];
 
     for i in StepRng::new(start + invocation_id.x..end, num_threads) {
-        constraints[i as usize].solve_joint_constraint(&mut solver_vels);
+        constraints[i as usize].solve_joint_constraint(&mut solver_vels, use_bias);
     }
 }
diff --git a/src_rbd_shaders/dynamics/joint_constraint_builder.rs b/src_rbd_shaders/dynamics/joint_constraint_builder.rs
index 7ce3cdb..a1e5707 100644
--- a/src_rbd_shaders/dynamics/joint_constraint_builder.rs
+++ b/src_rbd_shaders/dynamics/joint_constraint_builder.rs
@@ -330,7 +330,7 @@ impl JointConstraintHelper {
 
 impl JointConstraint {
     /// Solves a joint constraint.
-    pub fn solve_joint_constraint(&mut self, solver_vels: &mut SliceMut<Velocity>) {
+    pub fn solve_joint_constraint(&mut self, solver_vels: &mut SliceMut<Velocity>, use_bias: bool) {
         let mut solver_vel1 = solver_vels[self.solver_vel_a as usize];
         let mut solver_vel2 = solver_vels[self.solver_vel_b as usize];
 
@@ -340,7 +340,12 @@ impl JointConstraint {
             let dangvel = gdot(element.ang_jac_b, solver_vel2.angular)
                 - gdot(element.ang_jac_a, solver_vel1.angular);
 
-            let rhs = dlinvel + dangvel + element.rhs;
+            let el_rhs = if use_bias {
+                element.rhs
+            } else {
+                element.rhs_wo_bias
+            };
+            let rhs = dlinvel + dangvel + el_rhs;
             let total_impulse = (element.impulse
                 + element.inv_lhs * (rhs - element.cfm_gain * element.impulse))
                 .clamp(element.impulse_bounds.x, element.impulse_bounds.y);
diff --git a/src_rbd_shaders/dynamics/mod.rs b/src_rbd_shaders/dynamics/mod.rs
index eca83ab..d1303a9 100644
--- a/src_rbd_shaders/dynamics/mod.rs
+++ b/src_rbd_shaders/dynamics/mod.rs
@@ -19,6 +19,7 @@ mod solver_utils;
 mod warmstart;
 
 // GPU compute shader kernels
+mod color_buckets;
 mod coloring;
 mod mprops_update;
 mod prep_render;
@@ -35,6 +36,7 @@ pub use joint_constraint_builder::{JointConstraintBuilder, JointConstraintHelper
 pub use multibody::*;
 pub use sim_params::*;
 // Re-export solver items; update_constraint comes from joint_constraint_builder for joints
+pub use color_buckets::*;
 pub use coloring::*;
 pub use mprops_update::*;
 pub use prep_render::*;
diff --git a/src_rbd_shaders/dynamics/multibody/compute_dynamics_pre.rs b/src_rbd_shaders/dynamics/multibody/compute_dynamics_pre.rs
index a15512f..880ee02 100644
--- a/src_rbd_shaders/dynamics/multibody/compute_dynamics_pre.rs
+++ b/src_rbd_shaders/dynamics/multibody/compute_dynamics_pre.rs
@@ -9,25 +9,54 @@
 //! (gravity rhs + LU factor + LU solve).
 
 use khal_std::glamx::UVec3;
+use glamx::Vec4;
 use khal_std::index::MaybeIndexUnchecked;
 use khal_std::macros::{spirv, spirv_bindgen};
 use khal_std::sync::workgroup_memory_barrier_with_group_sync;
 
-use super::types::{MultibodyInfo, MultibodyLinkStatic, MultibodyLinkWorkspace};
+use super::types::{MultibodyInfo, MultibodyLinkStatic};
+use super::ws_soa::{
+    WS_JOINT_ROT, WS_JOINT_VEL, WS_LTP, WS_LTW, WS_RB_VELS, WS_SHIFT02, WS_SHIFT23, WsAddr,
+    ws_coords, ws_pose, ws_rot, ws_set_pose, ws_set_vec, ws_set_vel, ws_vec, ws_vel, ws_vel_ang,
+    ws_world_inertia,
+};
 use crate::dynamics::body::Velocity;
 use crate::dynamics::joint::SPATIAL_DIM;
 #[cfg(feature = "dim3")]
-use crate::utils::linalg::gemm_skew_lhs_cross_buf_par;
+use crate::utils::linalg::{gemm_inertia_lhs_cross_buf_par, gemm_skew_lhs_cross_buf_par};
 use crate::utils::linalg::{
-    MAX_MB_DOFS, MatSlice, copy_from_par, fill_par, gemm_inertia_lhs_par,
+    axpy_mat_par, copy_from_par, fill_par, gemm_inertia_lhs_par,
     gemm_omega_skew_tr_cross_buf_par, gemm_skew_tr_lhs_cross_buf_par, gemm_skew_tr_lhs_par,
     gemm_tr_par, quadform_spatial_par,
 };
-use crate::utils::{BatchIndices, Slice, SliceMut};
+use crate::utils::{BatchIndices, ISlice, SliceMut};
 use crate::{ANG_DIM, AngVector, DIM, Pose, Vector, gcross_av};
 use parry::math::VectorExt;
 
-const LANES: u32 = 64;
+#[inline(always)]
+fn sync_slots(t: u32) {
+    if t > 1 {
+        workgroup_memory_barrier_with_group_sync();
+    }
+}
+
+/// Returns `(t, lane, batch_id, mb_idx, active_slot)`.
+#[inline(always)]
+fn packed_decode(wg_id: UVec3, lid: UVec3, batch_ids: &BatchIndices) -> (u32, u32, u32, u32, bool) {
+    let t = batch_ids.mb_pack_lanes;
+    let slot = lid.x / t;
+    let lane = lid.x % t;
+    let slots = 64 / t;
+
+    let num_mb = batch_ids.multibodies_len;
+    let total_mb = num_mb * batch_ids.num_batches;
+    let global_mb = wg_id.x * slots + slot;
+    let active_slot = global_mb < total_mb;
+    let clamped_mb = if active_slot { global_mb } else { total_mb - 1 };
+    let batch_id = clamped_mb / num_mb;
+    let mb_idx = clamped_mb % num_mb;
+    (t, lane, batch_id, mb_idx, active_slot)
+}
 
 // TODO: refactor into multiple functions (but single kernel) to share between the coriolis and non-coriolis versions.
 /// Fused FK + body-jacobians + velocity propagation + CRBA-with-Coriolis.
@@ -40,7 +69,7 @@ pub fn gpu_mb_compute_dynamics_pre(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
     links_static: &[MultibodyLinkStatic],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
-    links_workspace: &mut [MultibodyLinkWorkspace],
+    links_workspace: &mut [Vec4],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] poses: &mut [Pose],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] body_jacobians: &mut [f32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] mass_matrices: &mut [f32],
@@ -49,88 +78,99 @@ pub fn gpu_mb_compute_dynamics_pre(
     #[spirv(uniform, descriptor_set = 0, binding = 8)] dt_uniform: &f32,
     #[spirv(uniform, descriptor_set = 0, binding = 9)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = wg_id.y;
-    let mb_idx = wg_id.x;
-    let lane = lid.x;
-    // Padding multibody slots have `num_links == 0` and `ndofs == 0` so all
-    // per-link / per-DOF loops below iterate zero times. No early-return —
-    // WGSL's naga frontend can't prove a storage-loaded comparison is
-    // uniform across the workgroup, so any subsequent `workgroupBarrier()`
-    // would be flagged "called from non-uniform control flow". See
-    // `gpu_mb_lu_decompose` for the rationale.
+    let (t, lane, batch_id, mb_idx, active_slot) = packed_decode(wg_id, lid, batch_ids);
 
     let dt = *dt_uniform;
 
-    let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
-        .read(mb_idx as usize);
+    let mb = if active_slot {
+        batch_ids
+            .ib(batch_id, multibody_info)
+            .read(mb_idx as usize)
+    } else {
+        MultibodyInfo::default()
+    };
     let num_links = mb.num_links;
     let ndofs = mb.ndofs;
-    let mb_jac_base = batch_ids.jac_start(batch_id) + mb.jacobian_offset as usize;
-    let mb_mm_base = batch_ids.mm_start(batch_id) + mb.mass_matrix_offset as usize;
-    let mb_cor_base = batch_ids.cor_start(batch_id) + mb.coriolis_offset as usize;
-    let mb_cor_w_base = batch_ids.coriolis_w_section_offset as usize + mb_cor_base;
-    let mb_icdt_base = batch_ids.i_coriolis_dt_section_offset as usize
-        + batch_ids.icdt_start(batch_id)
-        + mb.i_coriolis_dt_offset as usize;
-    let vel_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
+    let mb_jac_base = mb.jacobian_offset as usize;
+    let mb_mm_base = mb.mass_matrix_offset as usize;
+    let mb_cor_base = mb.coriolis_offset as usize;
+    let mb_cor_w_base = batch_ids.coriolis_batch_capacity as usize + mb_cor_base;
+    let mb_icdt_base =
+        2 * batch_ids.coriolis_batch_capacity as usize + mb.i_coriolis_dt_offset as usize;
+    let vel_base = mb.first_dof as usize;
 
     let stat_slice = batch_ids
-        .mb_links_batch(batch_id, links_static)
-        .offset(mb.first_link as usize);
-    let mut ws_slice = batch_ids
-        .mb_links_batch_mut(batch_id, links_workspace)
+        .ib(batch_id, links_static)
         .offset(mb.first_link as usize);
+    let wa = WsAddr::new(mb.first_link as usize, batch_ids.num_batches, batch_id);
     let mut poses_slice = batch_ids.coll_batch_mut(batch_id, poses);
-    let damping_slice = Slice(
-        dof_state,
-        vel_base + batch_ids.dof_damping_section_offset as usize,
-    );
+    let damping_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(batch_ids.dof_batch_capacity as usize + vel_base);
     // Armature (reflected rotor inertia) section sits right after damping, at
     // `2 · dof_damping_section_offset` (= 2·N). Added to the mass-matrix diagonal
     // alongside `damping·dt`, matching rapier's `update_mass_matrix`.
-    let armature_slice = Slice(
-        dof_state,
-        vel_base + 2 * batch_ids.dof_damping_section_offset as usize,
-    );
-    let vel_slice = Slice(dof_state, vel_base);
+    let armature_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(2 * batch_ids.dof_batch_capacity as usize + vel_base);
+    // Per-DoF spring stiffness, and kinematic-DoF mask.
+    let stiffness_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(3 * batch_ids.dof_batch_capacity as usize + vel_base);
+    let kin_mask_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(5 * batch_ids.dof_batch_capacity as usize + vel_base);
+    let vel_slice = batch_ids.ib(batch_id, dof_state).offset(vel_base);
 
     // 1) Forward Kinematics (single-threaded)
-    if lane == 0 {
-        forward_kinematics(&mb, &stat_slice, &mut poses_slice, &mut ws_slice, num_links);
+    if active_slot && num_links > 0 && lane == 0 {
+        forward_kinematics(&mb, &stat_slice, &mut poses_slice, links_workspace, wa, num_links);
     }
-    workgroup_memory_barrier_with_group_sync();
+    sync_slots(t);
 
     // 2) Update body jacobians
     update_body_jacobians(
         lane,
+        t,
         mb_jac_base,
         ndofs,
         num_links,
+        batch_ids.mb_max_links,
         &stat_slice,
-        &ws_slice.as_ref(),
+        links_workspace,
+        wa,
         body_jacobians,
+        batch_ids,
+        batch_id,
     );
 
     // 3) Propagate velocities (single-threaded)
-    if lane == 0 {
-        propagate_velocities(num_links, &stat_slice, &vel_slice, &mut ws_slice);
+    if active_slot && num_links > 0 && lane == 0 {
+        propagate_velocities(num_links, &stat_slice, &vel_slice, links_workspace, wa);
     }
-    workgroup_memory_barrier_with_group_sync();
+    sync_slots(t);
 
-    // 3) Mass matrix (with semi-implicit coriolis handling).
-    let acc_augmented_mass = MatSlice::dense(mb_mm_base, ndofs, ndofs);
-    fill_par(mass_matrices, acc_augmented_mass, 0.0, lane, LANES);
+    // 3) Mass matrices.
+    let acc_section = batch_ids.mass_matrix_acc_section_offset as usize;
+    let split = acc_section != 0;
+    let acc_augmented_mass = if split {
+        batch_ids.imat(batch_id, acc_section + mb_mm_base, ndofs, ndofs)
+    } else {
+        batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs)
+    };
+    let plain_mass = batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs);
+    fill_par(mass_matrices, acc_augmented_mass, 0.0, lane, t);
+    if split {
+        fill_par(mass_matrices, plain_mass, 0.0, lane, t);
+    }
 
-    let i_coriolis_dt_view = MatSlice::dense(mb_icdt_base, SPATIAL_DIM as u32, ndofs);
+    let i_coriolis_dt_view = batch_ids.imat(batch_id, mb_icdt_base, SPATIAL_DIM as u32, ndofs);
     let i_coriolis_dt_v = i_coriolis_dt_view.fixed_rows(0, DIM);
     let i_coriolis_dt_w = i_coriolis_dt_view.fixed_rows(DIM, ANG_DIM);
 
-    workgroup_memory_barrier_with_group_sync();
+    sync_slots(t);
 
-    // NOTE: fixed number of iterations for uniform control flow.
-    // TODO(PERF): on non-web platforms we could just use `mb.num_links` as the upper bound.
-    for k in 0..MAX_MB_DOFS as u32 {
+    for k in 0..batch_ids.mb_max_links {
         let loop_is_active = k < num_links;
         let mut inv_mass_x = 0.0;
         let mut mass = 0.0;
@@ -141,41 +181,41 @@ pub fn gpu_mb_compute_dynamics_pre(
             inv_mass_x = lmp.inv_mass.x;
 
             if inv_mass_x == 0.0 {
-                let coriolis_block = MatSlice::dense(
+                let coriolis_block = batch_ids.imat(batch_id, 
                     mb_cor_base + (k as usize) * (DIM as usize) * (ndofs as usize),
                     DIM,
                     ndofs,
                 );
-                fill_par(coriolis_packed, coriolis_block, 0.0, lane, LANES);
+                fill_par(coriolis_packed, coriolis_block, 0.0, lane, t);
                 fill_par(
                     coriolis_packed,
-                    MatSlice::dense(
+                    batch_ids.imat(batch_id, 
                         mb_cor_w_base + (k as usize) * (DIM as usize) * (ndofs as usize),
                         DIM,
                         ndofs,
                     ),
                     0.0,
                     lane,
-                    LANES,
+                    t,
                 );
             }
         }
         // Uniform barrier so subsequent parent-coriolis reads see consistent
         // state — WebGPU forbids a barrier inside divergent control flow.
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(t);
 
         let loop_is_active = k < num_links && inv_mass_x != 0.0;
-        let coriolis_v_i = MatSlice::dense(
+        let coriolis_v_i = batch_ids.imat(batch_id, 
             mb_cor_base + (k as usize) * (DIM as usize) * (ndofs as usize),
             DIM,
             ndofs,
         );
-        let coriolis_w_i = MatSlice::dense(
+        let coriolis_w_i = batch_ids.imat(batch_id, 
             mb_cor_w_base + (k as usize) * (DIM as usize) * (ndofs as usize),
             ANG_DIM,
             ndofs,
         );
-        let body_jacobian = MatSlice::dense(
+        let body_jacobian = batch_ids.imat(batch_id, 
             mb_jac_base + (k as usize) * SPATIAL_DIM * (ndofs as usize),
             SPATIAL_DIM as u32,
             ndofs,
@@ -184,86 +224,75 @@ pub fn gpu_mb_compute_dynamics_pre(
         let mut rb_inertia = Default::default();
 
         if loop_is_active {
-            let ws = &ws_slice[k as usize];
             let lmp = stat_slice[k as usize].local_mprops;
             mass = 1.0 / inv_mass_x;
-            rb_inertia = ws.link_world_inertia(&lmp);
-
-            #[cfg(feature = "dim3")]
-            let augmented_inertia = {
-                let angvel = ws.rb_vels.angular;
-                let w_skew = crate::utils::linalg::skew(angvel);
-                let i_omega = rb_inertia * angvel;
-                let i_omega_skew = crate::utils::linalg::skew(i_omega);
-                let gyro_mat = w_skew * rb_inertia - i_omega_skew;
-                rb_inertia + gyro_mat * dt
-            };
-            #[cfg(feature = "dim2")]
-            let augmented_inertia = rb_inertia;
+            rb_inertia = ws_world_inertia(links_workspace, wa, k, &lmp);
 
+            let quad_target = if split { plain_mass } else { acc_augmented_mass };
             quadform_spatial_par(
                 mass_matrices,
-                acc_augmented_mass,
+                quad_target,
                 1.0,
                 mass,
-                augmented_inertia,
+                rb_inertia,
                 body_jacobians,
                 body_jacobian,
                 1.0,
                 lane,
-                LANES,
+                t,
             );
 
             if k != 0 {
                 let stat = stat_slice[k as usize];
                 let parent_id = stat.parent_link_id;
-                let parent_link = &ws_slice[parent_id as usize];
-                let parent_j = MatSlice::dense(
+                let parent_j = batch_ids.imat(batch_id,
                     mb_jac_base + (parent_id as usize) * SPATIAL_DIM * (ndofs as usize),
                     SPATIAL_DIM as u32,
                     ndofs,
                 );
                 let parent_j_w = parent_j.fixed_rows(DIM, ANG_DIM);
-                let parent_coriolis_v = MatSlice::dense(
+                let parent_coriolis_v = batch_ids.imat(batch_id, 
                     mb_cor_base + (parent_id as usize) * (DIM as usize) * (ndofs as usize),
                     DIM,
                     ndofs,
                 );
-                let parent_coriolis_w = MatSlice::dense(
+                let parent_coriolis_w = batch_ids.imat(batch_id, 
                     mb_cor_w_base + (parent_id as usize) * (DIM as usize) * (ndofs as usize),
                     ANG_DIM,
                     ndofs,
                 );
-                let parent_w = parent_link.rb_vels.angular;
+                let parent_w = ws_vel_ang(links_workspace, wa, parent_id, WS_RB_VELS);
+                let ws_shift02 = ws_vec(links_workspace, wa, k, WS_SHIFT02);
+                let ws_joint_vel = ws_vel(links_workspace, wa, k, WS_JOINT_VEL);
 
                 copy_from_par(
                     coriolis_packed,
                     coriolis_v_i,
                     parent_coriolis_v,
                     lane,
-                    LANES,
+                    t,
                 );
                 copy_from_par(
                     coriolis_packed,
                     coriolis_w_i,
                     parent_coriolis_w,
                     lane,
-                    LANES,
+                    t,
                 );
 
                 gemm_skew_tr_lhs_par(
                     coriolis_packed,
                     coriolis_v_i,
                     1.0,
-                    ws.shift02,
+                    ws_shift02,
                     parent_coriolis_w,
                     1.0,
                     lane,
-                    LANES,
+                    t,
                 );
 
-                let dvel = crate::gcross_av(ws.rb_vels.angular, ws.shift02)
-                    + ws.joint_velocity.linear * 2.0;
+                let ws_rb_ang = ws_vel_ang(links_workspace, wa, k, WS_RB_VELS);
+                let dvel = crate::gcross_av(ws_rb_ang, ws_shift02) + ws_joint_vel.linear * 2.0;
                 gemm_skew_tr_lhs_cross_buf_par(
                     coriolis_packed,
                     coriolis_v_i,
@@ -273,19 +302,19 @@ pub fn gpu_mb_compute_dynamics_pre(
                     parent_j_w,
                     1.0,
                     lane,
-                    LANES,
+                    t,
                 );
 
                 gemm_skew_tr_lhs_cross_buf_par(
                     coriolis_packed,
                     coriolis_v_i,
                     1.0,
-                    ws.joint_velocity.linear,
+                    ws_joint_vel.linear,
                     body_jacobians,
                     parent_j_w,
                     1.0,
                     lane,
-                    LANES,
+                    t,
                 );
 
                 gemm_omega_skew_tr_cross_buf_par(
@@ -293,12 +322,12 @@ pub fn gpu_mb_compute_dynamics_pre(
                     coriolis_v_i,
                     1.0,
                     parent_w,
-                    ws.shift02,
+                    ws_shift02,
                     body_jacobians,
                     parent_j_w,
                     1.0,
                     lane,
-                    LANES,
+                    t,
                 );
 
                 #[cfg(feature = "dim3")]
@@ -307,34 +336,35 @@ pub fn gpu_mb_compute_dynamics_pre(
                         coriolis_packed,
                         coriolis_w_i,
                         -1.0,
-                        ws.joint_velocity.angular,
+                        ws_joint_vel.angular,
                         body_jacobians,
                         parent_j_w,
                         1.0,
                         lane,
-                        LANES,
+                        t,
                     );
                 }
             }
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(t);
 
         if loop_is_active {
             if k != 0 {
                 let stat = stat_slice[k as usize];
                 let parent_id = stat.parent_link_id;
-                let parent_link = &ws_slice[parent_id as usize];
 
                 if stat.kinematic == 0 {
-                    let transform_rot =
-                        parent_link.local_to_world.rotation * stat.data.local_frame_a.rotation;
+                    let transform_rot = ws_rot(links_workspace, wa, parent_id, WS_LTW)
+                        * stat.data.local_frame_a.rotation;
                     let coriolis_v_part = coriolis_v_i.columns(stat.assembly_id, stat.ndofs);
                     let coriolis_w_part = coriolis_w_i.columns(stat.assembly_id, stat.ndofs);
 
                     #[cfg(feature = "dim3")]
                     {
-                        let parent_w_skew = crate::utils::linalg::skew(parent_link.rb_vels.angular);
+                        let parent_w_skew = crate::utils::linalg::skew(
+                            ws_vel_ang(links_workspace, wa, parent_id, WS_RB_VELS),
+                        );
                         let c = lane;
                         if c < stat.ndofs {
                             let (jv, jw) = stat.joint_jacobian_column(transform_rot, c);
@@ -356,7 +386,7 @@ pub fn gpu_mb_compute_dynamics_pre(
                     }
                     #[cfg(feature = "dim2")]
                     {
-                        let parent_w = parent_link.rb_vels.angular;
+                        let parent_w = ws_vel_ang(links_workspace, wa, parent_id, WS_RB_VELS);
                         let c = lane;
                         if c < stat.ndofs {
                             let (jv, _) = stat.joint_jacobian_column(transform_rot, c);
@@ -371,27 +401,28 @@ pub fn gpu_mb_compute_dynamics_pre(
                     }
                 }
             } else {
-                fill_par(coriolis_packed, coriolis_v_i, 0.0, lane, LANES);
-                fill_par(coriolis_packed, coriolis_w_i, 0.0, lane, LANES);
+                fill_par(coriolis_packed, coriolis_v_i, 0.0, lane, t);
+                fill_par(coriolis_packed, coriolis_w_i, 0.0, lane, t);
             }
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(t);
 
         if loop_is_active {
-            let ws = &ws_slice[k as usize];
+            let ws_shift23 = ws_vec(links_workspace, wa, k, WS_SHIFT23);
+            let ws_rb_ang = ws_vel_ang(links_workspace, wa, k, WS_RB_VELS);
             gemm_skew_tr_lhs_par(
                 coriolis_packed,
                 coriolis_v_i,
                 1.0,
-                ws.shift23,
+                ws_shift23,
                 coriolis_w_i,
                 1.0,
                 lane,
-                LANES,
+                t,
             );
 
-            let dvel_23 = crate::gcross_av(ws.rb_vels.angular, ws.shift23);
+            let dvel_23 = crate::gcross_av(ws_rb_ang, ws_shift23);
             gemm_skew_tr_lhs_cross_buf_par(
                 coriolis_packed,
                 coriolis_v_i,
@@ -401,24 +432,24 @@ pub fn gpu_mb_compute_dynamics_pre(
                 rb_j_w,
                 1.0,
                 lane,
-                LANES,
+                t,
             );
 
             gemm_omega_skew_tr_cross_buf_par(
                 coriolis_packed,
                 coriolis_v_i,
                 1.0,
-                ws.rb_vels.angular,
-                ws.shift23,
+                ws_rb_ang,
+                ws_shift23,
                 body_jacobians,
                 rb_j_w,
                 1.0,
                 lane,
-                LANES,
+                t,
             );
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(t);
 
         if loop_is_active {
             // i_coriolis_dt assembly: dt · (mass·coriolis_v, I·coriolis_w).
@@ -440,11 +471,30 @@ pub fn gpu_mb_compute_dynamics_pre(
                 coriolis_w_i,
                 0.0,
                 lane,
-                LANES,
+                t,
             );
+
+            #[cfg(feature = "dim3")]
+            {
+                let angvel = ws_vel_ang(links_workspace, wa, k, WS_RB_VELS);
+                let w_skew = crate::utils::linalg::skew(angvel);
+                let i_omega_skew = crate::utils::linalg::skew(rb_inertia * angvel);
+                let gyro_mat = w_skew * rb_inertia - i_omega_skew;
+                gemm_inertia_lhs_cross_buf_par(
+                    coriolis_packed,
+                    i_coriolis_dt_w,
+                    dt,
+                    gyro_mat,
+                    body_jacobians,
+                    rb_j_w,
+                    1.0,
+                    lane,
+                    t,
+                );
+            }
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(t);
 
         if loop_is_active {
             gemm_tr_par(
@@ -457,160 +507,41 @@ pub fn gpu_mb_compute_dynamics_pre(
                 i_coriolis_dt_view,
                 1.0,
                 lane,
-                LANES,
+                t,
             );
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(t);
     }
 
-    // Diagonal: M[i, i] += damping[i] * dt + armature[i] — parallel.
-    // Matches rapier's `update_mass_matrix`: `diag = damping·dt + armature`.
+    // Diagonal: M[i, i] += damping[i]·dt + armature[i] + stiffness[i]·dt²
     let d = lane;
     if d < ndofs {
-        let diag_idx = acc_augmented_mass.idx(d, d);
+        let diag = damping_slice[d as usize] * dt
+            + armature_slice[d as usize]
+            + stiffness_slice[d as usize] * dt * dt;
+        let diag_target = if split { plain_mass } else { acc_augmented_mass };
+        let diag_idx = diag_target.idx(d, d);
         let cur = mass_matrices.read(diag_idx);
-        mass_matrices.write(
-            diag_idx,
-            cur + damping_slice[d as usize] * dt + armature_slice[d as usize],
-        );
-    }
-}
-
-/// Fused FK + body-jacobians + velocity propagation + CRBA-with-Coriolis.
-#[spirv_bindgen(force_cpu_coroutines)]
-#[spirv(compute(threads(64, 1, 1)))]
-pub fn gpu_mb_compute_dynamics_without_coriolis_pre(
-    #[spirv(workgroup_id)] wg_id: UVec3,
-    #[spirv(local_invocation_id)] lid: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
-    links_static: &[MultibodyLinkStatic],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
-    links_workspace: &mut [MultibodyLinkWorkspace],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] poses: &mut [Pose],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] body_jacobians: &mut [f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] mass_matrices: &mut [f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 6)] dof_state: &[f32],
-    #[spirv(uniform, descriptor_set = 0, binding = 7)] dt_uniform: &f32,
-    #[spirv(uniform, descriptor_set = 0, binding = 8)] batch_ids: &BatchIndices,
-) {
-    let batch_id = wg_id.y;
-    let mb_idx = wg_id.x;
-    let lane = lid.x;
-    // No early-return on out-of-range `mb_idx` — see `gpu_mb_lu_decompose`
-    // for the WGSL uniformity rationale. Dummy multibody slots have zero
-    // links / DOFs, so all per-link loops below iterate zero times.
-
-    let dt = *dt_uniform;
-
-    let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
-        .read(mb_idx as usize);
-    let num_links = mb.num_links;
-    let ndofs = mb.ndofs;
-    let mb_jac_base = batch_ids.jac_start(batch_id) + mb.jacobian_offset as usize;
-    let mb_mm_base = batch_ids.mm_start(batch_id) + mb.mass_matrix_offset as usize;
-    let vel_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
-
-    let stat_slice = batch_ids
-        .mb_links_batch(batch_id, links_static)
-        .offset(mb.first_link as usize);
-    let mut ws_slice = batch_ids
-        .mb_links_batch_mut(batch_id, links_workspace)
-        .offset(mb.first_link as usize);
-    let mut poses_slice = batch_ids.coll_batch_mut(batch_id, poses);
-    let damping_slice = Slice(
-        dof_state,
-        vel_base + batch_ids.dof_damping_section_offset as usize,
-    );
-    // Armature (reflected rotor inertia) section sits right after damping, at
-    // `2 · dof_damping_section_offset` (= 2·N). Added to the mass-matrix diagonal
-    // alongside `damping·dt`, matching rapier's `update_mass_matrix`.
-    let armature_slice = Slice(
-        dof_state,
-        vel_base + 2 * batch_ids.dof_damping_section_offset as usize,
-    );
-    let vel_slice = Slice(dof_state, vel_base);
-
-    // 1) Forward Kinematics (single-threaded)
-    if lane == 0 {
-        forward_kinematics(&mb, &stat_slice, &mut poses_slice, &mut ws_slice, num_links);
+        mass_matrices.write(diag_idx, cur + diag);
     }
-    workgroup_memory_barrier_with_group_sync();
-
-    // 2) Update body jacobians
-    update_body_jacobians(
-        lane,
-        mb_jac_base,
-        ndofs,
-        num_links,
-        &stat_slice,
-        &ws_slice.as_ref(),
-        body_jacobians,
-    );
+    sync_slots(t);
 
-    // 3) Velocities propagation (single-threaded)
-    if lane == 0 {
-        propagate_velocities(num_links, &stat_slice, &vel_slice, &mut ws_slice);
+    if split {
+        axpy_mat_par(mass_matrices, acc_augmented_mass, 1.0, plain_mass, lane, t);
     }
-    workgroup_memory_barrier_with_group_sync();
-
-    // 4) Mass matrix (without coriolis).
-    let acc_augmented_mass = MatSlice::dense(mb_mm_base, ndofs, ndofs);
-    fill_par(mass_matrices, acc_augmented_mass, 0.0, lane, LANES);
-    workgroup_memory_barrier_with_group_sync();
-
-    // NOTE: fixed number of iterations for uniform control flow.
-    // TODO(PERF): on non-web platforms we could just use `num_links` as the upper bound.
-    for k in 0..MAX_MB_DOFS as u32 {
-        let mut active = k < num_links;
-        if active {
-            let lmp = stat_slice[k as usize].local_mprops;
-            if lmp.inv_mass.x == 0.0 {
-                active = false;
+    sync_slots(t);
+
+    if d < ndofs && kin_mask_slice[d as usize] != 0.0 {
+        for j in 0..ndofs {
+            let v = if j == d { 1.0 } else { 0.0 };
+            mass_matrices.write(acc_augmented_mass.idx(d, j), v);
+            mass_matrices.write(acc_augmented_mass.idx(j, d), v);
+            if split {
+                mass_matrices.write(plain_mass.idx(d, j), v);
+                mass_matrices.write(plain_mass.idx(j, d), v);
             }
         }
-
-        if active {
-            let ws = &ws_slice[k as usize];
-            let lmp = stat_slice[k as usize].local_mprops;
-            let mass = 1.0 / lmp.inv_mass.x;
-            let inertia = ws.link_world_inertia(&lmp);
-
-            let body_jacobian = MatSlice::dense(
-                mb_jac_base + (k as usize) * SPATIAL_DIM * (ndofs as usize),
-                SPATIAL_DIM as u32,
-                ndofs,
-            );
-
-            quadform_spatial_par(
-                mass_matrices,
-                acc_augmented_mass,
-                1.0,
-                mass,
-                inertia,
-                body_jacobians,
-                body_jacobian,
-                1.0,
-                lane,
-                LANES,
-            );
-        }
-
-        workgroup_memory_barrier_with_group_sync();
-    }
-
-    // Diagonal: M[i, i] += damping[i] * dt + armature[i] — parallel.
-    // Matches rapier's `update_mass_matrix`: `diag = damping·dt + armature`.
-    let d = lane;
-    if d < ndofs {
-        let diag_idx = acc_augmented_mass.idx(d, d);
-        let cur = mass_matrices.read(diag_idx);
-        mass_matrices.write(
-            diag_idx,
-            cur + damping_slice[d as usize] * dt + armature_slice[d as usize],
-        );
     }
 }
 
@@ -620,7 +551,7 @@ pub fn gpu_mb_compute_dynamics_without_coriolis_pre(
 fn jacobian_mul_coordinates(
     locked_axes: u32,
     assembly_id: u32,
-    vel_slice: &Slice<f32>,
+    vel_slice: &ISlice<f32>,
 ) -> (Vector, AngVector) {
     let mut lin = Vector::ZERO;
     #[cfg(feature = "dim3")]
@@ -667,9 +598,10 @@ fn jacobian_mul_coordinates(
 // sequentially on a single thread.
 fn forward_kinematics(
     mb: &MultibodyInfo,
-    stat_slice: &Slice<MultibodyLinkStatic>,
+    stat_slice: &ISlice<MultibodyLinkStatic>,
     poses_slice: &mut SliceMut<Pose>,
-    ws_slice: &mut SliceMut<MultibodyLinkWorkspace>,
+    ws: &mut [Vec4],
+    wa: WsAddr,
     num_links: u32,
 ) {
     // Root pose.
@@ -677,26 +609,22 @@ fn forward_kinematics(
     let root_pose = if mb.root_is_dynamic == 0 {
         poses_slice[root_config.rb_id as usize]
     } else {
-        let ws_ref = &ws_slice[0];
-        let pose = root_config.body_to_parent(ws_ref.joint_rot, &ws_ref.coords);
+        let jr = ws_rot(ws, wa, 0, WS_JOINT_ROT);
+        let coords = ws_coords(ws, wa, 0);
+        let pose = root_config.body_to_parent(jr, &coords);
         poses_slice[root_config.rb_id as usize] = pose;
         pose
     };
-    let link0 = &mut ws_slice[0];
-    link0.local_to_parent = root_pose;
-    link0.local_to_world = root_pose;
+    ws_set_pose(ws, wa, 0, WS_LTP, root_pose);
+    ws_set_pose(ws, wa, 0, WS_LTW, root_pose);
 
     for k in 1..num_links {
         let k_usize = k as usize;
         let stat = &stat_slice[k_usize];
-        let local_to_parent;
-        let parent_to_world;
-        {
-            let ws_ref = &ws_slice[k_usize];
-            let parent_ref = &ws_slice[stat.parent_link_id as usize];
-            parent_to_world = parent_ref.local_to_world;
-            local_to_parent = stat.body_to_parent(ws_ref.joint_rot, &ws_ref.coords);
-        }
+        let parent_to_world = ws_pose(ws, wa, stat.parent_link_id, WS_LTW);
+        let jr = ws_rot(ws, wa, k, WS_JOINT_ROT);
+        let coords = ws_coords(ws, wa, k);
+        let local_to_parent = stat.body_to_parent(jr, &coords);
         let local_to_world = parent_to_world * local_to_parent;
 
         let parent_lmp = stat_slice[stat.parent_link_id as usize].local_mprops;
@@ -707,32 +635,36 @@ fn forward_kinematics(
         let shift02 = child_anchor_world - parent_com_world;
         let shift23 = world_com - child_anchor_world;
 
-        let link_mut = &mut ws_slice[k_usize];
-        link_mut.local_to_parent = local_to_parent;
-        link_mut.local_to_world = local_to_world;
-        link_mut.shift02 = shift02;
-        link_mut.shift23 = shift23;
+        ws_set_pose(ws, wa, k, WS_LTP, local_to_parent);
+        ws_set_pose(ws, wa, k, WS_LTW, local_to_world);
+        ws_set_vec(ws, wa, k, WS_SHIFT02, shift02);
+        ws_set_vec(ws, wa, k, WS_SHIFT23, shift23);
         poses_slice[stat.rb_id as usize] = local_to_world;
     }
 }
 
 fn update_body_jacobians(
     lane: u32,
+    // Lanes owned by this multibody's slot (`BatchIndices::mb_pack_lanes`).
+    lanes: u32,
     mb_jac_base: usize,
     ndofs: u32,
     num_links: u32,
-    stat_slice: &Slice<MultibodyLinkStatic>,
-    ws_slice: &Slice<MultibodyLinkWorkspace>,
+    max_links: u32,
+    stat_slice: &ISlice<MultibodyLinkStatic>,
+    ws: &[Vec4],
+    wa: WsAddr,
     body_jacobians: &mut [f32],
+    batch_ids: &BatchIndices,
+    batch_id: u32,
 ) {
-    // TODO(PERF): on non-web platforms we could just use `mb.num_links` as the upper bound.
     // TODO(PERF): instead of copying the body jacobian over and over for each body, we should
     //             precompute a bit set that indicates which dofs are part of the kinematic tree
     //             of each node. For a max number of DOFs set to 32, this means a single addition 32-bits
     //             value per node.
-    for k in 0..MAX_MB_DOFS as u32 {
+    for k in 0..max_links {
         let mut parent_to_world = Pose::default();
-        let link_j = MatSlice::dense(
+        let link_j = batch_ids.imat(batch_id, 
             mb_jac_base + (k as usize) * SPATIAL_DIM * (ndofs as usize),
             SPATIAL_DIM as u32,
             ndofs,
@@ -740,37 +672,35 @@ fn update_body_jacobians(
 
         if k < num_links {
             let link_infos = &stat_slice[k as usize];
-            let link = &ws_slice[k as usize];
 
             if k != 0 {
-                let parent_j = MatSlice::dense(
+                let parent_j = batch_ids.imat(batch_id, 
                     mb_jac_base
                         + (link_infos.parent_link_id as usize) * SPATIAL_DIM * (ndofs as usize),
                     SPATIAL_DIM as u32,
                     ndofs,
                 );
-                let parent_link = &ws_slice[link_infos.parent_link_id as usize];
-                parent_to_world = parent_link.local_to_world;
+                parent_to_world = ws_pose(ws, wa, link_infos.parent_link_id, WS_LTW);
 
-                copy_from_par(body_jacobians, link_j, parent_j, lane, LANES);
+                copy_from_par(body_jacobians, link_j, parent_j, lane, lanes);
                 let link_j_v = link_j.fixed_rows(0, DIM);
                 let parent_j_w = parent_j.fixed_rows(DIM, ANG_DIM);
                 gemm_skew_tr_lhs_par(
                     body_jacobians,
                     link_j_v,
                     1.0,
-                    link.shift02,
+                    ws_vec(ws, wa, k, WS_SHIFT02),
                     parent_j_w,
                     1.0,
                     lane,
-                    LANES,
+                    lanes,
                 );
             } else {
-                fill_par(body_jacobians, link_j, 0.0, lane, LANES);
+                fill_par(body_jacobians, link_j, 0.0, lane, lanes);
             }
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(lanes);
 
         if k < num_links {
             let link_infos = &stat_slice[k as usize];
@@ -780,36 +710,36 @@ fn update_body_jacobians(
                 body_jacobians,
                 link_j_part,
                 lane,
-                LANES,
+                lanes,
             );
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(lanes);
 
         if k < num_links {
-            let link = &ws_slice[k as usize];
             let (link_j_v, link_j_w) = link_j.rows_range_pair(0, DIM, DIM, ANG_DIM);
             gemm_skew_tr_lhs_par(
                 body_jacobians,
                 link_j_v,
                 1.0,
-                link.shift23,
+                ws_vec(ws, wa, k, WS_SHIFT23),
                 link_j_w,
                 1.0,
                 lane,
-                LANES,
+                lanes,
             );
         }
 
-        workgroup_memory_barrier_with_group_sync();
+        sync_slots(lanes);
     }
 }
 
 fn propagate_velocities(
     num_links: u32,
-    stat_slice: &Slice<MultibodyLinkStatic>,
-    vel_slice: &Slice<f32>,
-    ws_slice: &mut SliceMut<MultibodyLinkWorkspace>,
+    stat_slice: &ISlice<MultibodyLinkStatic>,
+    vel_slice: &ISlice<f32>,
+    ws: &mut [Vec4],
+    wa: WsAddr,
 ) {
     for k in 0..num_links {
         let k_usize = k as usize;
@@ -822,14 +752,14 @@ fn propagate_velocities(
             let jv = Velocity::new(jv_local_lin, jv_local_ang);
             (jv, jv)
         } else {
-            let parent_id = stat.parent_link_id as usize;
-            let parent_ws = &ws_slice[parent_id];
-            let parent_to_world_rot = parent_ws.local_to_world.rotation;
-            let parent_world_com_pose = parent_ws.local_to_world;
-            let parent_rb_lin = parent_ws.rb_vels.linear;
-            let parent_rb_ang = parent_ws.rb_vels.angular;
-
-            let parent_lmp = stat_slice[parent_id].local_mprops;
+            let parent_id = stat.parent_link_id;
+            let parent_world_com_pose = ws_pose(ws, wa, parent_id, WS_LTW);
+            let parent_to_world_rot = parent_world_com_pose.rotation;
+            let parent_rb = ws_vel(ws, wa, parent_id, WS_RB_VELS);
+            let parent_rb_lin = parent_rb.linear;
+            let parent_rb_ang = parent_rb.angular;
+
+            let parent_lmp = stat_slice[parent_id as usize].local_mprops;
             let transform_rot = parent_to_world_rot * stat.data.local_frame_a.rotation;
 
             #[cfg(feature = "dim3")]
@@ -838,10 +768,8 @@ fn propagate_velocities(
             #[cfg(feature = "dim2")]
             let joint_velocity = Velocity::new(transform_rot * jv_local_lin, jv_local_ang);
 
-            let (self_local_to_world, self_shift23) = {
-                let ws_ref = &ws_slice[k_usize];
-                (ws_ref.local_to_world, ws_ref.shift23)
-            };
+            let self_local_to_world = ws_pose(ws, wa, k, WS_LTW);
+            let self_shift23 = ws_vec(ws, wa, k, WS_SHIFT23);
 
             let lmp = stat.local_mprops;
             let world_com = self_local_to_world * lmp.com;
@@ -856,8 +784,7 @@ fn propagate_velocities(
             (joint_velocity, Velocity::new(new_lin, new_ang))
         };
 
-        let link_mut = &mut ws_slice[k_usize];
-        link_mut.joint_velocity = joint_velocity;
-        link_mut.rb_vels = rb_vels;
+        ws_set_vel(ws, wa, k, WS_JOINT_VEL, joint_velocity);
+        ws_set_vel(ws, wa, k, WS_RB_VELS, rb_vels);
     }
 }
diff --git a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs
index 922015a..b23d6b2 100644
--- a/src_rbd_shaders/dynamics/multibody/contact_constraints.rs
+++ b/src_rbd_shaders/dynamics/multibody/contact_constraints.rs
@@ -15,6 +15,7 @@
 
 use khal_std::glamx::UVec3;
 use khal_std::index::MaybeIndexUnchecked;
+use khal_std::iter::StepRng;
 use khal_std::macros::{spirv, spirv_bindgen};
 
 use crate::dynamics::ConstraintSoftness;
@@ -22,7 +23,7 @@ use crate::dynamics::body::{Velocity, WorldMassProperties};
 use crate::dynamics::joint::SPATIAL_DIM;
 use crate::queries::IndexedManifold;
 use crate::utils::BatchIndices;
-use crate::utils::linalg::{MatSlice, lu_solve_in_place};
+use crate::utils::linalg::{MatSlice, VSlice, lu_solve_in_place};
 use crate::{ANG_DIM, AngVector, DIM, Pose, Vector, gcross, gdot};
 
 use super::types::{
@@ -59,10 +60,15 @@ fn orthonormal_vector(v: Vec2) -> Vec2 {
 /// **adding** the resulting `Jᵀ` row to `out_jacs[col_offset ..]` (so two
 /// calls accumulate — used by self-collisions, which combine the two
 /// touched links into a single net `Jᵀ` row).
+///
+/// One DOF per lane.
 #[inline]
 fn fill_contact_jac_row(
     body_jacobians: &[f32],
     mb_jac_base: usize,
+    // Interleave parameters of `body_jacobians` (`num_batches`, `batch_id`).
+    jac_stride: u32,
+    jac_shift: u32,
     ndofs: u32,
     link_id: u32,
     unit_force: Vector,
@@ -70,13 +76,16 @@ fn fill_contact_jac_row(
     out_jacs: &mut [f32],
     col_offset: usize,
     accumulate: bool,
+    lane: u32,
 ) {
     // Per-link SPATIAL_DIM × ndofs jacobian (rows 0..DIM = J_v, rows
     // DIM..SPATIAL_DIM = J_w).
     let link_jac_base = mb_jac_base + (link_id as usize) * SPATIAL_DIM * (ndofs as usize);
-    let link_j = MatSlice::dense(link_jac_base, SPATIAL_DIM as u32, ndofs);
+    let link_j =
+        MatSlice::interleaved(link_jac_base, SPATIAL_DIM as u32, ndofs, jac_stride, jac_shift);
     let (link_j_v, link_j_w) = link_j.rows_range_pair(0, DIM, DIM, ANG_DIM);
-    for j in 0..ndofs {
+    let j = lane;
+    if j < ndofs {
         // Linear contribution: `unit_force · J_v[:, j]`.
         let dot;
         #[cfg(feature = "dim3")]
@@ -117,10 +126,12 @@ fn fill_contact_jac_row(
 /// `MultibodyContactConstraint` and writes the multibody-side `Jᵀ` row into
 /// `contact_constraint_jacs`. Multibody-multibody contacts (each side a
 /// different multibody) are not handled — such contacts are skipped.
+/// One 64-lane workgroup per (multibody, batch).
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_init_contact_constraints(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
     multibody_info: &mut [MultibodyInfo],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] body_jacobians: &[f32],
@@ -134,18 +145,20 @@ pub fn gpu_mb_init_contact_constraints(
     #[spirv(storage_buffer, descriptor_set = 1, binding = 2)] contacts: &[IndexedManifold],
     #[spirv(uniform, descriptor_set = 0, binding = 6)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
-    // Soft-constraint coefficients (rapier TGS-soft), precomputed on the host.
-    // The old path used a rigid `erp = 1/dt` with zero CFM, which overshoots
-    // penetration recovery (~14× too stiff for the defaults) and jitters.
+    // Only active multibody slots are visited now; the `ndofs == 0` sentinel
+    // below is kept for all-locked (zero-dof) multibodies. Padding slots past
+    // `multibodies_len` are never read (every consumer guards on it).
+    let num_mb = batch_ids.multibodies_len;
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
+    if mb_idx >= num_mb {
+        return;
+    }
+
     let inv_dt = softness.inv_dt;
-    let erp_inv_dt = softness.erp_inv_dt;
-    let allowed_lin_err = softness.allowed_lin_err;
     let max_corr_velocity = softness.max_corr_velocity;
-    let cfm_factor = softness.cfm_factor;
 
-    let mb_start = batch_ids.mb_start(batch_id);
     let cons_start = batch_ids.mb_contact_constraints_start(batch_id);
     let col_start = batch_ids.mb_contact_constraint_columns_start(batch_id);
     let colliders_start = batch_ids.coll_start(batch_id);
@@ -155,14 +168,17 @@ pub fn gpu_mb_init_contact_constraints(
     // Per-multibody early-out: padding multibody slots have `ndofs == 0`,
     // which we use here as the sentinel (replaces the `num_multibodies`
     // storage binding the kernel used to read).
-    let mut mb = multibody_info.read(mb_start + mb_idx as usize);
+    let mut mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
     let ndofs = mb.ndofs;
     if ndofs == 0 {
-        mb.contact_constraint_count = 0;
-        multibody_info.write(mb_start + mb_idx as usize, mb);
+        // Uniform per workgroup: every lane returns together.
+        if lane == 0 {
+            mb.contact_constraint_count = 0;
+            multibody_info.write(batch_ids.mbi(batch_id, mb_idx as usize), mb);
+        }
         return;
     }
-    let mb_jac_base = batch_ids.jac_start(batch_id) + mb.jacobian_offset as usize;
+    let mb_jac_base = mb.jacobian_offset as usize;
     let cons_base = cons_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
     // Each constraint slot reserves `dof_batch_capacity` floats in the
     // column buffer (matches the allocation in `from_rapier` and avoids any
@@ -172,10 +188,7 @@ pub fn gpu_mb_init_contact_constraints(
         col_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize) * dofs_stride;
 
     let contacts_slice = batch_ids.contact_batch(batch_id, contacts);
-    // Iterate to capacity (instead of reading a `contacts_len` storage buffer):
-    // empty slots have `contact.len == 0` and are skipped. Drops one binding to
-    // fit the 8-storage-buffer WebGPU limit; matches `gpu_solver_init_constraints`.
-    let n_contacts = batch_ids.contacts_batch_capacity;
+    let n_contacts = mb.batch_contacts_len.min(batch_ids.contacts_batch_capacity);
     let mut count = 0u32;
 
     for ci in 0..n_contacts {
@@ -243,18 +256,20 @@ pub fn gpu_mb_init_contact_constraints(
         };
         let free_im = if is_self { 0.0 } else { free_mp.inv_mass.x };
 
+        let contact_is_static = !is_self && free_mp.inv_mass == Vector::ZERO;
+        let erp_inv_dt = if contact_is_static {
+            softness.static_erp_inv_dt
+        } else {
+            softness.erp_inv_dt
+        };
+        let cfm_factor = if contact_is_static {
+            softness.static_cfm_factor
+        } else {
+            softness.cfm_factor
+        };
+
         // Multibody-link origins come from the collider poses buffer instead
-        // of `links_workspace` (which we no longer bind in this kernel — see
-        // binding-count cap discussion). This uses the contacting collider's
-        // world translation as a proxy for its link's world origin. It is exact
-        // when the collider sits at the link origin (the historical one-collider
-        // case); for a link carrying a collider offset from its origin the
-        // angular lever arm `pt_world - link_origin_a` is off by that offset.
-        // Binding `body_poses` here to use the true link origin would exceed the
-        // 10-storage-buffer cap — left as a follow-up. `mb_link_id_a` always
-        // corresponds to side `id1`/`b1` when `mb_on_1 || is_self`, and to
-        // `id2`/`b2` otherwise. `mb_link_id_b` is only used in the self-contact
-        // case where it corresponds to `id2`.
+        // of `links_workspace`.
         let link_origin_a = if is_self || mb_on_1 {
             pose1.translation
         } else {
@@ -288,22 +303,24 @@ pub fn gpu_mb_init_contact_constraints(
             #[cfg(feature = "dim3")]
             let torque_a_t1 = gcross(shift_a, mb_tangent1);
 
-            let rhs_bias = (erp_inv_dt * (dist + allowed_lin_err)).clamp(-max_corr_velocity, 0.0);
+            let rhs_bias = (erp_inv_dt * dist).clamp(-max_corr_velocity, 0.0);
             let rhs_wo_bias = if dist > 0.0 { dist * inv_dt } else { 0.0 };
 
             let normal_slot = count;
             let normal_col_offset = col_base + (normal_slot as usize) * dofs_stride;
-            // Warmstart: preserve the accumulated impulse from the previous
-            // substep (same contact slot — within a frame the manifolds are
-            // fixed). `gpu_mb_reset_contact_warmstart` zeroes these once per
-            // frame so the first substep starts cold.
-            let warmstart_normal_impulse = contact_constraints
-                .read(cons_base + normal_slot as usize)
-                .impulse;
+            let warmstart_normal_impulse = if lane == 0 {
+                contact_constraints
+                    .read(cons_base + normal_slot as usize)
+                    .impulse
+            } else {
+                0.0
+            };
 
             fill_contact_jac_row(
                 body_jacobians,
                 mb_jac_base,
+                batch_ids.num_batches,
+                batch_id,
                 ndofs,
                 mb_link_id_a,
                 mb_normal,
@@ -311,6 +328,7 @@ pub fn gpu_mb_init_contact_constraints(
                 contact_constraint_jacs,
                 normal_col_offset,
                 false,
+                lane,
             );
 
             // B-side fold-in for self-contacts, free body for the rest. The
@@ -328,6 +346,8 @@ pub fn gpu_mb_init_contact_constraints(
                 fill_contact_jac_row(
                     body_jacobians,
                     mb_jac_base,
+                    batch_ids.num_batches,
+                    batch_id,
                     ndofs,
                     mb_link_id_b,
                     lin_jac,
@@ -335,6 +355,7 @@ pub fn gpu_mb_init_contact_constraints(
                     contact_constraint_jacs,
                     normal_col_offset,
                     true,
+                    lane,
                 );
                 #[cfg(feature = "dim3")]
                 {
@@ -397,7 +418,9 @@ pub fn gpu_mb_init_contact_constraints(
                 cfm_factor,
                 _unused_cfm: 0.0,
             };
-            contact_constraints.write(cons_base + normal_slot as usize, normal_cons);
+            if lane == 0 {
+                contact_constraints.write(cons_base + normal_slot as usize, normal_cons);
+            }
             count += 1;
 
             // Friction tangent constraints — same contact point, tangent
@@ -435,14 +458,20 @@ pub fn gpu_mb_init_contact_constraints(
                 let tang_slot = count;
                 let tang_col_offset = col_base + (tang_slot as usize) * dofs_stride;
                 // Warmstart: preserve the accumulated tangent impulse (see the
-                // normal slot above).
-                let warmstart_tang_impulse = contact_constraints
-                    .read(cons_base + tang_slot as usize)
-                    .impulse;
+                // normal slot above; lane 0 only).
+                let warmstart_tang_impulse = if lane == 0 {
+                    contact_constraints
+                        .read(cons_base + tang_slot as usize)
+                        .impulse
+                } else {
+                    0.0
+                };
 
                 fill_contact_jac_row(
                     body_jacobians,
                     mb_jac_base,
+                    batch_ids.num_batches,
+                    batch_id,
                     ndofs,
                     mb_link_id_a,
                     mb_tangent,
@@ -450,6 +479,7 @@ pub fn gpu_mb_init_contact_constraints(
                     contact_constraint_jacs,
                     tang_col_offset,
                     false,
+                    lane,
                 );
 
                 let (ang_jac_tang, ii_ang_jac_tang) = if is_self {
@@ -459,6 +489,8 @@ pub fn gpu_mb_init_contact_constraints(
                     fill_contact_jac_row(
                         body_jacobians,
                         mb_jac_base,
+                        batch_ids.num_batches,
+                        batch_id,
                         ndofs,
                         mb_link_id_b,
                         free_tangent,
@@ -466,6 +498,7 @@ pub fn gpu_mb_init_contact_constraints(
                         contact_constraint_jacs,
                         tang_col_offset,
                         true,
+                        lane,
                     );
                     #[cfg(feature = "dim3")]
                     {
@@ -527,7 +560,9 @@ pub fn gpu_mb_init_contact_constraints(
                     cfm_factor,
                     _unused_cfm: 0.0,
                 };
-                contact_constraints.write(cons_base + tang_slot as usize, tang_cons);
+                if lane == 0 {
+                    contact_constraints.write(cons_base + tang_slot as usize, tang_cons);
+                }
                 count += 1;
             }
         }
@@ -535,8 +570,34 @@ pub fn gpu_mb_init_contact_constraints(
 
     // The solve / finalize / remove-bias kernels iterate `0..count` so we
     // don't need to mark surplus slots inactive — they're never read.
-    mb.contact_constraint_count = count;
-    multibody_info.write(mb_start + mb_idx as usize, mb);
+    if lane == 0 {
+        mb.contact_constraint_count = count;
+        multibody_info.write(batch_ids.mbi(batch_id, mb_idx as usize), mb);
+    }
+}
+
+/// HACK: stash `contacts_len[batch]` into each multibody's `batch_contacts_len`.
+///
+/// This exists only to work around the web 8-storage-bindings limit for kernels
+/// that bind multibodies but don’t have any room left to bind `contacts_len`.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_mb_stash_contacts_len(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
+    multibody_info: &mut [MultibodyInfo],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] contacts_len: &[u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 2)] batch_ids: &BatchIndices,
+) {
+    let num_mb = batch_ids.multibodies_len;
+    if invocation_id.x >= num_mb * batch_ids.num_batches {
+        return;
+    }
+    let batch_id = invocation_id.x / num_mb;
+    let mb_idx = invocation_id.x % num_mb;
+    let mut mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
+    mb.batch_contacts_len = contacts_len.read(batch_id as usize);
+    multibody_info.write(batch_ids.mbi(batch_id, mb_idx as usize), mb);
 }
 
 /// Zero the accumulated impulse of every contact-constraint slot for each
@@ -544,45 +605,43 @@ pub fn gpu_mb_init_contact_constraints(
 /// substep loop) so the first substep's warmstart starts cold; within a frame
 /// `gpu_mb_init_contact_constraints` preserves the impulse across substeps and
 /// `gpu_mb_warmstart_contact_constraints` re-applies it each substep.
+/// One 64-lane workgroup per (multibody, batch).
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_reset_contact_warmstart(
     #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
     contact_constraints: &mut [MultibodyContactConstraint],
-    #[spirv(uniform, descriptor_set = 0, binding = 2)] batch_ids: &BatchIndices,
+    #[spirv(uniform, descriptor_set = 0, binding = 1)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
+    // One thread per (slot, multibody, batch), flattened.
+    const MAXC: u32 = MAX_MB_CONTACT_CONSTRAINTS_PER_MB;
     let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
+    let per_batch = num_mb * MAXC;
+    if invocation_id.x >= per_batch * batch_ids.num_batches {
         return;
     }
-    let mb_start = batch_ids.mb_start(batch_id);
+    let batch_id = invocation_id.x / per_batch;
+    let r = invocation_id.x % per_batch;
+    let mb_idx = r / MAXC;
+    let s = r % MAXC;
+
     let cons_start = batch_ids.mb_contact_constraints_start(batch_id);
-    let mb = multibody_info.read(mb_start + mb_idx as usize);
-    if mb.ndofs == 0 {
-        return;
-    }
-    let cons_base = cons_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
-    // Zero to capacity (the per-frame contact count isn't known here, and last
-    // frame's count may be smaller than this frame's).
-    for s in 0..MAX_MB_CONTACT_CONSTRAINTS_PER_MB {
-        let mut cons = contact_constraints.read(cons_base + s as usize);
-        cons.impulse = 0.0;
-        contact_constraints.write(cons_base + s as usize, cons);
-    }
+    let idx = cons_start + (mb_idx * MAXC + s) as usize;
+    contact_constraints.at_mut(idx).impulse = 0.0;
 }
 
 /// Warmstart: re-apply each active contact constraint's accumulated `impulse`
 /// to the multibody generalized velocities (`dof_state`) and the free-body
 /// solver velocities. Applies the FULL accumulated impulse (no `rhs` term, no
 /// clamping).
+///
+/// One 64-lane workgroup per (multibody, batch).
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_warmstart_contact_constraints(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
     contact_constraints: &[MultibodyContactConstraint],
@@ -591,45 +650,55 @@ pub fn gpu_mb_warmstart_contact_constraints(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] solver_vels: &mut [Velocity],
     #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
     let num_mb = batch_ids.multibodies_len;
     if mb_idx >= num_mb {
         return;
     }
 
-    let mb_start = batch_ids.mb_start(batch_id);
     let cons_start = batch_ids.mb_contact_constraints_start(batch_id);
     let col_start = batch_ids.mb_contact_constraint_columns_start(batch_id);
     let colliders_start = batch_ids.coll_start(batch_id);
 
-    let mb = multibody_info.read(mb_start + mb_idx as usize);
+    let mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
     let ndofs = mb.ndofs;
     if ndofs == 0 {
         return;
     }
-    let v_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
+    let v_base = mb.first_dof as usize;
     let cons_base = cons_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
     let dofs_stride = batch_ids.dof_batch_capacity as usize;
     let col_base =
         col_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize) * dofs_stride;
 
     let count = mb.contact_constraint_count;
+    // No accumulated impulses to re-apply: skip the dof round-trip.
+    if count == 0 {
+        return;
+    }
+
+    // This lane's DOF velocity, accumulated in a register across every
+    // constraint.
+    let mut v_lane = if lane < ndofs {
+        dof_state.read(batch_ids.mbi(batch_id, v_base + lane as usize))
+    } else {
+        0.0
+    };
     for s in 0..count {
         let cons = contact_constraints.read(cons_base + s as usize);
         let imp = cons.impulse;
         if imp != 0.0 {
             let col_offset = col_base + (s as usize) * dofs_stride;
             // Multibody side: v += impulse · column (column = M⁻¹ Jᵀ).
-            for i in 0..ndofs {
-                let v_idx = v_base + i as usize;
-                let cur = dof_state.read(v_idx);
-                let col = contact_constraint_columns.read(col_offset + i as usize);
-                dof_state.write(v_idx, cur + imp * col);
+            if lane < ndofs {
+                let col = contact_constraint_columns.read(col_offset + lane as usize);
+                v_lane += imp * col;
             }
             // Free body side (skipped for self-contacts).
             let is_self = cons.free_body_id == u32::MAX;
-            if !is_self {
+            if lane == 0 && !is_self {
                 let free = solver_vels.read(colliders_start + cons.free_body_id as usize);
                 let mut new_free = free;
                 new_free.linear += cons.lin_jac * (cons.free_body_im * imp);
@@ -638,15 +707,20 @@ pub fn gpu_mb_warmstart_contact_constraints(
             }
         }
     }
+
+    if lane < ndofs {
+        dof_state.write(batch_ids.mbi(batch_id, v_base + lane as usize), v_lane);
+    }
 }
 
 /// Pass 2: for each emitted constraint, LU back-solve `M · column = Jᵀ`
 /// (the row produced by the init kernel) and set `inv_lhs = 1 / (Jᵀ ·
 /// column + free_body_inv_r)`.
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_finalize_contact_constraints(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] mass_matrices: &[f32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] lu_pivots: &[u32],
@@ -657,33 +731,34 @@ pub fn gpu_mb_finalize_contact_constraints(
     contact_constraint_columns: &mut [f32],
     #[spirv(uniform, descriptor_set = 0, binding = 6)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
+    const LANES: u32 = 64;
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
     let num_mb = batch_ids.multibodies_len;
     if mb_idx >= num_mb {
         return;
     }
 
-    let mb_start = batch_ids.mb_start(batch_id);
     let cons_start = batch_ids.mb_contact_constraints_start(batch_id);
     let col_start = batch_ids.mb_contact_constraint_columns_start(batch_id);
 
-    let mb = multibody_info.read(mb_start + mb_idx as usize);
+    let mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
     let ndofs = mb.ndofs;
     if ndofs == 0 {
         return;
     }
-    let mb_mm_base = batch_ids.mm_start(batch_id) + mb.mass_matrix_offset as usize;
-    let piv_offset = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
+    let mb_mm_base = mb.mass_matrix_offset as usize;
+    let piv = batch_ids.ivec(batch_id, mb.first_dof as usize);
     let cons_base = cons_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
     let dofs_stride = batch_ids.dof_batch_capacity as usize;
     let col_base =
         col_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize) * dofs_stride;
 
-    let m = MatSlice::dense(mb_mm_base, ndofs, ndofs);
+    let m = batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs);
     let count = mb.contact_constraint_count;
 
-    for s in 0..count {
+    for s in StepRng::new(lane..count, LANES) {
         let col_offset = col_base + (s as usize) * dofs_stride;
         // 1) Copy J^T row into the column buffer (it'll be overwritten by the
         //    LU solve with the M⁻¹·Jᵀ result).
@@ -696,9 +771,9 @@ pub fn gpu_mb_finalize_contact_constraints(
             mass_matrices,
             m,
             lu_pivots,
-            piv_offset,
+            piv,
             contact_constraint_columns,
-            col_offset,
+            VSlice::dense(col_offset),
         );
         // 3) inv_r_mb = J · column.
         let mut inv_r_mb = 0.0f32;
@@ -722,146 +797,3 @@ pub fn gpu_mb_finalize_contact_constraints(
         contact_constraints.write(cons_base + s as usize, cons);
     }
 }
-
-/// One PGS sweep over the multibody's active contact constraints. Updates
-/// the multibody's `dof_velocities` and the free body's `solver_vels`.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_mb_solve_contact_constraints(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
-    contact_constraints: &mut [MultibodyContactConstraint],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] contact_constraint_jacs: &[f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] contact_constraint_columns: &[f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] dof_state: &mut [f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] solver_vels: &mut [Velocity],
-    #[spirv(uniform, descriptor_set = 0, binding = 6)] batch_ids: &BatchIndices,
-) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
-    let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
-        return;
-    }
-
-    let mb_start = batch_ids.mb_start(batch_id);
-    let cons_start = batch_ids.mb_contact_constraints_start(batch_id);
-    let col_start = batch_ids.mb_contact_constraint_columns_start(batch_id);
-    let colliders_start = batch_ids.coll_start(batch_id);
-
-    let mb = multibody_info.read(mb_start + mb_idx as usize);
-    let ndofs = mb.ndofs;
-    if ndofs == 0 {
-        return;
-    }
-    let v_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
-    let cons_base = cons_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
-    let dofs_stride = batch_ids.dof_batch_capacity as usize;
-    let col_base =
-        col_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize) * dofs_stride;
-
-    let count = mb.contact_constraint_count;
-    for s in 0..count {
-        let mut cons = contact_constraints.read(cons_base + s as usize);
-        let col_offset = col_base + (s as usize) * dofs_stride;
-
-        // J · u = J_mb · v_mb_dofs + J_free · v_free.
-        let is_self = cons.free_body_id == u32::MAX;
-        let mut j_dot_v = 0.0f32;
-        for i in 0..ndofs {
-            let j = contact_constraint_jacs.read(col_offset + i as usize);
-            let v = dof_state.read(v_base + i as usize);
-            j_dot_v += j * v;
-        }
-        let free = if is_self {
-            Velocity::default()
-        } else {
-            solver_vels.read(colliders_start + cons.free_body_id as usize)
-        };
-        if !is_self {
-            j_dot_v += cons.lin_jac.dot(free.linear) + gdot(cons.ang_jac, free.angular);
-        }
-
-        let rhs_total = j_dot_v + cons.rhs;
-        // CFM-factor form (rapier's `*ContactConstraintNormalPart::generic_solve`):
-        // `new = cfm_factor · (impulse − r · Δvel)`. `cfm_factor < 1` provides the
-        // soft-constraint compliance that keeps resting contacts from jittering.
-        let raw_imp = cons.cfm_factor * (cons.impulse - cons.inv_lhs * rhs_total);
-
-        // Normal: clamp to ≥ 0 (no separation impulse). Friction tangent:
-        // clamp to `±μ · normal_impulse` — looks up the paired normal slot
-        // for the current accumulated impulse. Mirrors rapier's
-        // `ContactConstraintNormalPart::generic_solve` /
-        // `ContactConstraintTangentPart::generic_solve` (independent
-        // per-tangent clamp, i.e. box friction; rapier's circular-cone
-        // joint clamp is a future refinement).
-        let new_imp = if cons.kind == MB_CONTACT_KIND_TANGENT {
-            let normal = contact_constraints.read(cons_base + cons.normal_constraint_slot as usize);
-            let limit = cons.friction_coeff * normal.impulse;
-            if raw_imp > limit {
-                limit
-            } else if raw_imp < -limit {
-                -limit
-            } else {
-                raw_imp
-            }
-        } else if raw_imp < 0.0 {
-            0.0
-        } else {
-            raw_imp
-        };
-        let delta = new_imp - cons.impulse;
-        cons.impulse = new_imp;
-        contact_constraints.write(cons_base + s as usize, cons);
-
-        if delta != 0.0 {
-            for i in 0..ndofs {
-                let v_idx = v_base + i as usize;
-                let cur = dof_state.read(v_idx);
-                let col = contact_constraint_columns.read(col_offset + i as usize);
-                dof_state.write(v_idx, cur + delta * col);
-            }
-            if !is_self {
-                let mut new_free = free;
-                new_free.linear += cons.lin_jac * (cons.free_body_im * delta);
-                new_free.angular += cons.ii_ang_jac * delta;
-                solver_vels.write(colliders_start + cons.free_body_id as usize, new_free);
-            }
-        }
-    }
-}
-
-/// Strip the positional bias from each active contact constraint's `rhs`,
-/// matching `gpu_mb_remove_joint_constraint_bias`.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_mb_remove_contact_constraint_bias(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
-    contact_constraints: &mut [MultibodyContactConstraint],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] multibody_info: &[MultibodyInfo],
-    #[spirv(uniform, descriptor_set = 0, binding = 2)] batch_ids: &BatchIndices,
-) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
-    let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
-        return;
-    }
-
-    let mb_start = batch_ids.mb_start(batch_id);
-    let cons_start = batch_ids.mb_contact_constraints_start(batch_id);
-    let cons_base = cons_start + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
-    let mb = multibody_info.read(mb_start + mb_idx as usize);
-    let count = mb.contact_constraint_count;
-
-    for s in 0..count {
-        let mut cons = contact_constraints.read(cons_base + s as usize);
-        if cons.kind == 0 {
-            continue;
-        }
-        cons.rhs = cons.rhs_wo_bias;
-        contact_constraints.write(cons_base + s as usize, cons);
-    }
-}
diff --git a/src_rbd_shaders/dynamics/multibody/gravity_and_lu.rs b/src_rbd_shaders/dynamics/multibody/gravity_and_lu.rs
index b2eeb78..62aa99d 100644
--- a/src_rbd_shaders/dynamics/multibody/gravity_and_lu.rs
+++ b/src_rbd_shaders/dynamics/multibody/gravity_and_lu.rs
@@ -17,14 +17,67 @@ use glamx::Vec4;
 
 use crate::dynamics::body::Velocity;
 use crate::dynamics::joint::SPATIAL_DIM;
-use crate::utils::linalg::{MAX_MB_DOFS, MatSlice, fill_par, gemv_tr_spatial_split_par};
-use crate::utils::{BatchIndices, Slice};
+use crate::utils::linalg::{
+    MAX_MB_DOFS, fill_par, gemv_tr_spatial_split_par, lu_decompose, lu_solve_in_place,
+};
+use crate::utils::{BatchIndices, ISlice};
 use crate::{AngVector, Vector, gcross_av};
 
 use super::lu::{
-    LANES, lu_apply_pivots, lu_factor_in_shared, lu_triangular_solve_in_place, sm_idx,
+    LANES, lu_apply_pivots, lu_apply_pivots_packed, lu_factor_in_shared,
+    lu_factor_in_shared_packed, lu_triangular_solve_in_place,
+    lu_triangular_solve_in_place_packed, sm_idx, sm_idx_packed,
+};
+use super::types::{MultibodyInfo, MultibodyLinkStatic};
+use super::ws_soa::{
+    WS_JOINT_VEL, WS_KIN_ACC, WS_LTW, WS_RB_VELS, WS_SHIFT02, WS_SHIFT23, WsAddr, ws_coord,
+    ws_pose, ws_set_vel, ws_vec, ws_vel, ws_vel_ang, ws_world_inertia,
 };
-use super::types::{MultibodyInfo, MultibodyLinkStatic, MultibodyLinkWorkspace};
+
+/// Adds the per-DoF joint-spring generalized forces (rapier's
+/// `update_acceleration` spring loop): backward Euler needs both the
+/// `-k·(q − rest)` and `-k·dt·v` terms here, the `v⁺` part being implicit via
+/// the `dt²·k` mass-matrix diagonal added by the `pre` kernel. Serial (call
+/// from one lane): one cheap update per sprung axis, and springs are rare.
+#[allow(clippy::too_many_arguments)]
+#[inline]
+fn apply_spring_forces(
+    gen_forces: &mut [f32],
+    batch_ids: &BatchIndices,
+    batch_id: u32,
+    gen_base: usize,
+    stat_slice: &ISlice<MultibodyLinkStatic>,
+    links_workspace: &[Vec4],
+    wa: WsAddr,
+    num_links: u32,
+    stiffness_slice: &ISlice<f32>,
+    spring_ref_slice: &ISlice<f32>,
+    vel_slice: &ISlice<f32>,
+    dt: f32,
+) {
+    for k in 0..num_links {
+        let stat = stat_slice[k as usize];
+        let locked = stat.data.locked_axes;
+        let mut curr_free = 0u32;
+        // Free axes in `0..SPATIAL_DIM` order = linear DOFs then angular
+        // DOFs, matching the generalized-coordinate layout.
+        for axis in 0..(SPATIAL_DIM as u32) {
+            if (locked & (1 << axis)) != 0 {
+                continue;
+            }
+            let dof = (stat.assembly_id + curr_free) as usize;
+            let k_s = stiffness_slice[dof];
+            if k_s != 0.0 {
+                let q = ws_coord(links_workspace, wa, k, axis);
+                let rest = spring_ref_slice[dof];
+                let idx = batch_ids.mbi(batch_id, gen_base + dof);
+                let cur = gen_forces.read(idx);
+                gen_forces.write(idx, cur - k_s * (q - rest) - k_s * dt * vel_slice[dof]);
+            }
+            curr_free += 1;
+        }
+    }
+}
 
 /// Fused gravity / Coriolis-force assembly + LU factor + LU solve.
 #[spirv_bindgen]
@@ -36,7 +89,7 @@ pub fn gpu_mb_gravity_and_lu(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
     links_static: &[MultibodyLinkStatic],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
-    links_workspace: &mut [MultibodyLinkWorkspace],
+    links_workspace: &mut [Vec4],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] body_jacobians: &[f32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] gen_forces: &mut [f32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] mass_matrices: &mut [f32],
@@ -44,6 +97,7 @@ pub fn gpu_mb_gravity_and_lu(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 7)] dof_state: &[f32],
     #[spirv(uniform, descriptor_set = 0, binding = 8)] gravity: &Vec4,
     #[spirv(uniform, descriptor_set = 0, binding = 9)] batch_ids: &BatchIndices,
+    #[spirv(uniform, descriptor_set = 0, binding = 10)] dt_uniform: &f32,
     // Mass-matrix tile in shared memory.
     #[spirv(workgroup)] mat: &mut [f32; MAX_MB_DOFS * MAX_MB_DOFS],
     // RHS / solution vector.
@@ -57,32 +111,39 @@ pub fn gpu_mb_gravity_and_lu(
     let batch_id = wg_id.y;
     let mb_idx = wg_id.x;
     let lane = lid.x;
+    let max_ndofs = batch_ids.mb_max_ndofs;
+    let max_links = batch_ids.mb_max_links;
 
     let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
+        .ib(batch_id, multibody_info)
         .read(mb_idx as usize);
     let num_links = mb.num_links;
     let ndofs = mb.ndofs;
-    let mb_jac_base = batch_ids.jac_start(batch_id) + mb.jacobian_offset as usize;
-    let gen_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
-    let mb_mm_base = batch_ids.mm_start(batch_id) + mb.mass_matrix_offset as usize;
-    let piv_offset = gen_base;
-    let rhs_offset = gen_base;
+    let mb_jac_base = mb.jacobian_offset as usize;
+    let gen_base = mb.first_dof as usize;
+    let mb_mm_base = mb.mass_matrix_offset as usize;
+    let piv = batch_ids.ivec(batch_id, gen_base);
 
     let stat_slice = batch_ids
-        .mb_links_batch(batch_id, links_static)
+        .ib(batch_id, links_static)
         .offset(mb.first_link as usize);
-    let mut ws_slice = batch_ids
-        .mb_links_batch_mut(batch_id, links_workspace)
-        .offset(mb.first_link as usize);
-    let vel_slice = Slice(dof_state, gen_base);
-    let damping_slice = Slice(
-        dof_state,
-        batch_ids.dof_damping_section_offset as usize + gen_base,
-    );
+    let wa = WsAddr::new(mb.first_link as usize, batch_ids.num_batches, batch_id);
+    let vel_slice = batch_ids.ib(batch_id, dof_state).offset(gen_base);
+    let damping_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(batch_ids.dof_batch_capacity as usize + gen_base);
+    let stiffness_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(3 * batch_ids.dof_batch_capacity as usize + gen_base);
+    let spring_ref_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(4 * batch_ids.dof_batch_capacity as usize + gen_base);
+    let kin_mask_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(5 * batch_ids.dof_batch_capacity as usize + gen_base);
 
     // ---- Phase 1: zero the generalized-force vector (parallel across DOFs). ----
-    let accelerations = MatSlice::dense(gen_base, ndofs, 1);
+    let accelerations = batch_ids.imat(batch_id, gen_base, ndofs, 1);
     // TODO(perf): up to a certain number of degrees of freedom, we could actually run all the
     //             calculations in shared memory and only write the result in the end.
     //             Currently, the max number of dofs is 32 but we still accumulate forces/accelerations
@@ -90,17 +151,13 @@ pub fn gpu_mb_gravity_and_lu(
     fill_par(gen_forces, accelerations, 0.0, lane, LANES);
     workgroup_memory_barrier_with_group_sync();
 
-    let _ = stat_slice;
-
     #[cfg(feature = "dim3")]
     let g = Vec3::new(gravity.x, gravity.y, gravity.z);
     #[cfg(feature = "dim2")]
     let g = Vec2::new(gravity.x, gravity.y);
 
     // ---- Phase 2: per-link gravity / Coriolis-force assembly. ----
-    // NOTE: fixed number of iterations for uniform control flow.
-    // TODO(PERF): on non-web platforms we could just use `num_links` as the upper bound.
-    for k in 0..MAX_MB_DOFS as u32 {
+    for k in 0..max_links {
         let active = k < num_links;
         let mut acc_lin = Vector::ZERO;
         #[cfg(feature = "dim3")]
@@ -117,23 +174,24 @@ pub fn gpu_mb_gravity_and_lu(
                 _self_local_to_world,
                 self_rb_ang,
             ) = {
-                let ws = &ws_slice[k as usize];
+                let jv = ws_vel(links_workspace, wa, k, WS_JOINT_VEL);
                 (
-                    ws.joint_velocity.linear,
-                    ws.joint_velocity.angular,
-                    ws.shift02,
-                    ws.shift23,
-                    ws.local_to_world,
-                    ws.rb_vels.angular,
+                    jv.linear,
+                    jv.angular,
+                    ws_vec(links_workspace, wa, k, WS_SHIFT02),
+                    ws_vec(links_workspace, wa, k, WS_SHIFT23),
+                    ws_pose(links_workspace, wa, k, WS_LTW),
+                    ws_vel_ang(links_workspace, wa, k, WS_RB_VELS),
                 )
             };
 
             if k != 0 {
                 let stat = stat_slice[k as usize];
-                let parent_ws = &ws_slice[stat.parent_link_id as usize];
-                let parent_acc_lin = parent_ws.kinematic_acc.linear;
-                let parent_acc_ang = parent_ws.kinematic_acc.angular;
-                let parent_ang = parent_ws.rb_vels.angular;
+                let pid = stat.parent_link_id;
+                let parent_acc = ws_vel(links_workspace, wa, pid, WS_KIN_ACC);
+                let parent_acc_lin = parent_acc.linear;
+                let parent_acc_ang = parent_acc.angular;
+                let parent_ang = ws_vel_ang(links_workspace, wa, pid, WS_RB_VELS);
 
                 acc_lin = parent_acc_lin;
                 acc_ang = parent_acc_ang;
@@ -158,7 +216,7 @@ pub fn gpu_mb_gravity_and_lu(
             acc_lin += gcross_av(acc_ang, self_shift23);
 
             if lane == 0 {
-                ws_slice[k as usize].kinematic_acc = Velocity::new(acc_lin, acc_ang);
+                ws_set_vel(links_workspace, wa, k, WS_KIN_ACC, Velocity::new(acc_lin, acc_ang));
             }
         }
 
@@ -168,12 +226,12 @@ pub fn gpu_mb_gravity_and_lu(
 
         if active {
             #[cfg(feature = "dim3")]
-            let rb_ang = ws_slice[k as usize].rb_vels.angular;
+            let rb_ang = ws_vel_ang(links_workspace, wa, k, WS_RB_VELS);
             let lmp = stat_slice[k as usize].local_mprops;
             let inv_mass_x = lmp.inv_mass.x;
             if inv_mass_x != 0.0 {
                 let mass = 1.0 / inv_mass_x;
-                let rb_inertia = ws_slice[k as usize].link_world_inertia(&lmp);
+                let rb_inertia = ws_world_inertia(links_workspace, wa, k, &lmp);
 
                 #[cfg(feature = "dim3")]
                 let gyroscopic = {
@@ -191,7 +249,7 @@ pub fn gpu_mb_gravity_and_lu(
                 let f_lin = (g - acc_lin) * mass;
                 let f_ang = -gyroscopic - i_acc_ang;
 
-                let body_jacobian = MatSlice::dense(
+                let body_jacobian = batch_ids.imat(batch_id, 
                     mb_jac_base + (k as usize) * SPATIAL_DIM * (ndofs as usize),
                     SPATIAL_DIM as u32,
                     ndofs,
@@ -199,7 +257,7 @@ pub fn gpu_mb_gravity_and_lu(
 
                 gemv_tr_spatial_split_par(
                     gen_forces,
-                    gen_base,
+                    batch_ids.ivec(batch_id, gen_base),
                     1.0,
                     body_jacobians,
                     body_jacobian,
@@ -214,52 +272,728 @@ pub fn gpu_mb_gravity_and_lu(
     }
 
     // Damping subtraction is handled at solve time via the LU rhs (we still
-    // need the read-modify-write done by lane d): we don't have `dt` here, so
-    // damping is applied identically to the original kernel: rhs -=
-    // damping[d] * v[d] regardless of `dt` (matches rapier's `cmpy(-1.0,
-    // damping, velocities, 1.0)` form, which does not include `dt`).
+    // need the read-modify-write done by lane d): rhs -= damping[d] * v[d]
+    // (matches rapier's `cmpy(-1.0, damping, velocities, 1.0)` form, which
+    // does not include `dt`; the implicit `dt·damping` part sits on the
+    // mass-matrix diagonal).
     workgroup_memory_barrier_with_group_sync();
     let i = lane;
     if i < ndofs {
-        let idx = gen_base + i as usize;
+        let idx = batch_ids.mbi(batch_id, gen_base + i as usize);
         let cur = gen_forces.read(idx);
         gen_forces.write(idx, cur - damping_slice[i as usize] * vel_slice[i as usize]);
     }
     workgroup_memory_barrier_with_group_sync();
 
-    // ---- Phase 3: load M into shared memory, factor in place. ----
-    let m_view = MatSlice::dense(mb_mm_base, ndofs, ndofs);
+    // Per-DoF joint springs (serial: one cheap update per sprung axis).
+    if lane == 0 {
+        apply_spring_forces(
+            gen_forces,
+            batch_ids,
+            batch_id,
+            gen_base,
+            &stat_slice,
+            links_workspace,
+            wa,
+            num_links,
+            &stiffness_slice,
+            &spring_ref_slice,
+            &vel_slice,
+            *dt_uniform,
+        );
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    // Kinematic DOFs get zero acceleration: their mass-matrix rows are
+    // identity (see the `pre` kernel), so the solve passes the rhs through:
+    // zeroing it here pins their velocities to the user-driven values.
+    if i < ndofs && kin_mask_slice[i as usize] != 0.0 {
+        gen_forces.write(batch_ids.mbi(batch_id, gen_base + i as usize), 0.0);
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    // ---- Phase 3: factor the acceleration matrix, solve M·x = τ. ----
+    let m_view = batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs);
+    let acc_section = batch_ids.mass_matrix_acc_section_offset as usize;
+    let split = acc_section != 0;
+    let m_acc_view = if split {
+        batch_ids.imat(batch_id, acc_section + mb_mm_base, ndofs, ndofs)
+    } else {
+        m_view
+    };
     if lane < ndofs {
         for r in 0..ndofs {
-            mat.write(sm_idx(r, lane), mass_matrices.read(m_view.idx(r, lane)));
+            mat.write(sm_idx(r, lane), mass_matrices.read(m_acc_view.idx(r, lane)));
         }
-        x.write(lane as usize, gen_forces.read(rhs_offset + lane as usize));
+        x.write(
+            lane as usize,
+            gen_forces.read(batch_ids.mbi(batch_id, gen_base + lane as usize)),
+        );
     }
     workgroup_memory_barrier_with_group_sync();
 
     lu_factor_in_shared(
         ndofs,
+        max_ndofs,
         lane,
         mat,
         lu_pivots,
-        piv_offset,
+        piv,
         pivot_row_shared,
         inv_akk_shared,
     );
 
-    // Persist LU factors to global memory (joint / contact constraint init
-    // reuses them for unit-RHS solves).
-    if lane < ndofs {
+    // Legacy mode only: this factor doubles as the constraints' LU; persist
+    // it (joint / contact constraint init reuses it for unit-RHS solves).
+    if !split && lane < ndofs {
         for r in 0..ndofs {
             mass_matrices.write(m_view.idx(r, lane), mat.read(sm_idx(r, lane)));
         }
     }
 
     // ---- Phase 4: solve M·x = τ for the gravity rhs. ----
-    lu_apply_pivots(ndofs, lane, lu_pivots, piv_offset, x);
-    lu_triangular_solve_in_place(ndofs, lane, mat, x, partial);
+    lu_apply_pivots(ndofs, lane, lu_pivots, piv, x);
+    lu_triangular_solve_in_place(ndofs, max_ndofs, lane, mat, x, partial);
 
     if lane < ndofs {
-        gen_forces.write(rhs_offset + lane as usize, x.read(lane as usize));
+        gen_forces.write(batch_ids.mbi(batch_id, gen_base + lane as usize), x.read(lane as usize));
+    }
+
+    // ---- Phase 5 (split mode only): factor the plain matrix and persist its
+    // LU + pivots for the joint / contact / impulse-joint constraint columns.
+    if split {
+        workgroup_memory_barrier_with_group_sync();
+        if lane < ndofs {
+            for r in 0..ndofs {
+                mat.write(sm_idx(r, lane), mass_matrices.read(m_view.idx(r, lane)));
+            }
+        }
+        workgroup_memory_barrier_with_group_sync();
+        lu_factor_in_shared(
+            ndofs,
+            max_ndofs,
+            lane,
+            mat,
+            lu_pivots,
+            piv,
+            pivot_row_shared,
+            inv_akk_shared,
+        );
+        if lane < ndofs {
+            for r in 0..ndofs {
+                mass_matrices.write(m_view.idx(r, lane), mat.read(sm_idx(r, lane)));
+            }
+        }
     }
 }
+
+/// Packed version of [`gpu_mb_gravity_and_lu`]: `SLOTS = 64 / T` multibodies per
+/// 64-lane workgroup, each owning `T` lanes and a `T×T` shared tile.
+#[inline(always)]
+#[allow(clippy::too_many_arguments)]
+fn gravity_and_lu_packed_impl<const T: u32, const MATN: usize, const SLOTS: usize>(
+    wg_id: UVec3,
+    lid: UVec3,
+    multibody_info: &[MultibodyInfo],
+    links_static: &[MultibodyLinkStatic],
+    links_workspace: &mut [Vec4],
+    body_jacobians: &[f32],
+    gen_forces: &mut [f32],
+    mass_matrices: &mut [f32],
+    lu_pivots: &mut [u32],
+    dof_state: &[f32],
+    gravity: &Vec4,
+    batch_ids: &BatchIndices,
+    dt_uniform: &f32,
+    mat: &mut [f32; MATN],
+    x: &mut [f32; 64],
+    partial: &mut [f32; 64],
+    pivot_row_shared: &mut [u32; SLOTS],
+    inv_akk_shared: &mut [f32; SLOTS],
+) {
+    let slot = lid.x / T;
+    let lane = lid.x % T;
+    let seg = (slot * T) as usize;
+
+    let num_mb = batch_ids.multibodies_len;
+    let total_mb = num_mb * batch_ids.num_batches;
+    let global_mb = wg_id.x * SLOTS as u32 + slot;
+    let active_slot = global_mb < total_mb;
+    // Clamped so index math stays in-bounds for inactive slots; their loops
+    // all no-op (dummy `mb`) and every store is guarded.
+    let clamped_mb = if active_slot { global_mb } else { total_mb - 1 };
+    let batch_id = clamped_mb / num_mb;
+    let mb_idx = clamped_mb % num_mb;
+    let max_ndofs = batch_ids.mb_max_ndofs;
+    let max_links = batch_ids.mb_max_links;
+
+    let mb = if active_slot {
+        batch_ids
+            .ib(batch_id, multibody_info)
+            .read(mb_idx as usize)
+    } else {
+        MultibodyInfo::default()
+    };
+    let num_links = mb.num_links;
+    let ndofs = mb.ndofs;
+    let mb_jac_base = mb.jacobian_offset as usize;
+    let gen_base = mb.first_dof as usize;
+    let mb_mm_base = mb.mass_matrix_offset as usize;
+    let piv = batch_ids.ivec(batch_id, gen_base);
+
+    let stat_slice = batch_ids
+        .ib(batch_id, links_static)
+        .offset(mb.first_link as usize);
+    let wa = WsAddr::new(mb.first_link as usize, batch_ids.num_batches, batch_id);
+    let vel_slice = batch_ids.ib(batch_id, dof_state).offset(gen_base);
+    let damping_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(batch_ids.dof_batch_capacity as usize + gen_base);
+    let stiffness_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(3 * batch_ids.dof_batch_capacity as usize + gen_base);
+    let spring_ref_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(4 * batch_ids.dof_batch_capacity as usize + gen_base);
+    let kin_mask_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(5 * batch_ids.dof_batch_capacity as usize + gen_base);
+
+    // ---- Phase 1: zero the generalized-force vector (parallel across DOFs). ----
+    let accelerations = batch_ids.imat(batch_id, gen_base, ndofs, 1);
+    fill_par(gen_forces, accelerations, 0.0, lane, T);
+    workgroup_memory_barrier_with_group_sync();
+
+    #[cfg(feature = "dim3")]
+    let g = Vec3::new(gravity.x, gravity.y, gravity.z);
+    #[cfg(feature = "dim2")]
+    let g = Vec2::new(gravity.x, gravity.y);
+
+    // ---- Phase 2: per-link gravity / Coriolis-force assembly. ----
+    for k in 0..max_links {
+        let active = k < num_links;
+        let mut acc_lin = Vector::ZERO;
+        #[cfg(feature = "dim3")]
+        let mut acc_ang: AngVector = AngVector::ZERO;
+        #[cfg(feature = "dim2")]
+        let mut acc_ang: AngVector = 0.0;
+
+        if active {
+            let (
+                self_joint_vel_lin,
+                self_joint_vel_ang,
+                self_shift02,
+                self_shift23,
+                _self_local_to_world,
+                self_rb_ang,
+            ) = {
+                let jv = ws_vel(links_workspace, wa, k, WS_JOINT_VEL);
+                (
+                    jv.linear,
+                    jv.angular,
+                    ws_vec(links_workspace, wa, k, WS_SHIFT02),
+                    ws_vec(links_workspace, wa, k, WS_SHIFT23),
+                    ws_pose(links_workspace, wa, k, WS_LTW),
+                    ws_vel_ang(links_workspace, wa, k, WS_RB_VELS),
+                )
+            };
+
+            if k != 0 {
+                let stat = stat_slice[k as usize];
+                let pid = stat.parent_link_id;
+                let parent_acc = ws_vel(links_workspace, wa, pid, WS_KIN_ACC);
+                let parent_acc_lin = parent_acc.linear;
+                let parent_acc_ang = parent_acc.angular;
+                let parent_ang = ws_vel_ang(links_workspace, wa, pid, WS_RB_VELS);
+
+                acc_lin = parent_acc_lin;
+                acc_ang = parent_acc_ang;
+
+                acc_lin += gcross_av(parent_ang, self_joint_vel_lin) * 2.0;
+                #[cfg(feature = "dim3")]
+                {
+                    acc_ang += parent_ang.cross(self_joint_vel_ang);
+                }
+                #[cfg(feature = "dim2")]
+                {
+                    let _ = self_joint_vel_ang;
+                }
+                acc_lin += gcross_av(parent_ang, gcross_av(parent_ang, self_shift02));
+                acc_lin += gcross_av(parent_acc_ang, self_shift02);
+            } else {
+                let _ = self_joint_vel_ang;
+                let _ = self_shift02;
+            }
+            let rb_ang = self_rb_ang;
+            acc_lin += gcross_av(rb_ang, gcross_av(rb_ang, self_shift23));
+            acc_lin += gcross_av(acc_ang, self_shift23);
+
+            if lane == 0 {
+                ws_set_vel(links_workspace, wa, k, WS_KIN_ACC, Velocity::new(acc_lin, acc_ang));
+            }
+        }
+
+        // Top-level barrier: reached uniformly by every lane on every outer
+        // iteration so children see the just-published `kinematic_acc`.
+        workgroup_memory_barrier_with_group_sync();
+
+        if active {
+            #[cfg(feature = "dim3")]
+            let rb_ang = ws_vel_ang(links_workspace, wa, k, WS_RB_VELS);
+            let lmp = stat_slice[k as usize].local_mprops;
+            let inv_mass_x = lmp.inv_mass.x;
+            if inv_mass_x != 0.0 {
+                let mass = 1.0 / inv_mass_x;
+                let rb_inertia = ws_world_inertia(links_workspace, wa, k, &lmp);
+
+                #[cfg(feature = "dim3")]
+                let gyroscopic = {
+                    let i_omega = rb_inertia * rb_ang;
+                    rb_ang.cross(i_omega)
+                };
+                #[cfg(feature = "dim2")]
+                let gyroscopic: AngVector = 0.0;
+
+                let i_acc_ang = rb_inertia * acc_ang;
+
+                let f_lin = (g - acc_lin) * mass;
+                let f_ang = -gyroscopic - i_acc_ang;
+
+                let body_jacobian = batch_ids.imat(batch_id, 
+                    mb_jac_base + (k as usize) * SPATIAL_DIM * (ndofs as usize),
+                    SPATIAL_DIM as u32,
+                    ndofs,
+                );
+
+                gemv_tr_spatial_split_par(
+                    gen_forces,
+                    batch_ids.ivec(batch_id, gen_base),
+                    1.0,
+                    body_jacobians,
+                    body_jacobian,
+                    f_lin,
+                    f_ang,
+                    1.0,
+                    lane,
+                    T,
+                );
+            }
+        }
+    }
+
+    // Apply damping.
+    workgroup_memory_barrier_with_group_sync();
+    let i = lane;
+    if i < ndofs {
+        let idx = batch_ids.mbi(batch_id, gen_base + i as usize);
+        let cur = gen_forces.read(idx);
+        gen_forces.write(idx, cur - damping_slice[i as usize] * vel_slice[i as usize]);
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    // Per-DoF joint springs (serial per slot: one cheap update per sprung axis).
+    if lane == 0 && active_slot {
+        apply_spring_forces(
+            gen_forces,
+            batch_ids,
+            batch_id,
+            gen_base,
+            &stat_slice,
+            links_workspace,
+            wa,
+            num_links,
+            &stiffness_slice,
+            &spring_ref_slice,
+            &vel_slice,
+            *dt_uniform,
+        );
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    // Kinematic DOFs get zero acceleration.
+    if i < ndofs && kin_mask_slice[i as usize] != 0.0 {
+        gen_forces.write(batch_ids.mbi(batch_id, gen_base + i as usize), 0.0);
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    // ---- Phase 3: factor the acceleration matrix, solve M·x = τ. ----
+    let m_view = batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs);
+    let acc_section = batch_ids.mass_matrix_acc_section_offset as usize;
+    let split = acc_section != 0;
+    let m_acc_view = if split {
+        batch_ids.imat(batch_id, acc_section + mb_mm_base, ndofs, ndofs)
+    } else {
+        m_view
+    };
+    if lane < ndofs {
+        for r in 0..ndofs {
+            mat.write(
+                sm_idx_packed::<T>(slot, r, lane),
+                mass_matrices.read(m_acc_view.idx(r, lane)),
+            );
+        }
+        x.write(
+            seg + lane as usize,
+            gen_forces.read(batch_ids.mbi(batch_id, gen_base + lane as usize)),
+        );
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    lu_factor_in_shared_packed::<T, MATN, SLOTS>(
+        ndofs,
+        max_ndofs,
+        slot,
+        lane,
+        active_slot,
+        mat,
+        lu_pivots,
+        piv,
+        pivot_row_shared,
+        inv_akk_shared,
+    );
+
+    // Legacy mode only: this factor doubles as the constraints' LU; persist
+    // it (joint / contact constraint init reuses it for unit-RHS solves).
+    if !split && lane < ndofs {
+        for r in 0..ndofs {
+            mass_matrices.write(m_view.idx(r, lane), mat.read(sm_idx_packed::<T>(slot, r, lane)));
+        }
+    }
+
+    // ---- Phase 4: solve M·x = τ for the gravity rhs. ----
+    lu_apply_pivots_packed::<T>(ndofs, slot, lane, active_slot, lu_pivots, piv, x);
+    lu_triangular_solve_in_place_packed::<T, MATN>(
+        ndofs,
+        max_ndofs,
+        slot,
+        lane,
+        active_slot,
+        mat,
+        x,
+        partial,
+    );
+
+    if lane < ndofs {
+        gen_forces.write(
+            batch_ids.mbi(batch_id, gen_base + lane as usize),
+            x.read(seg + lane as usize),
+        );
+    }
+
+    // ---- Phase 5 (split mode only): factor the plain matrix and persist its
+    // LU + pivots for the constraint columns.
+    if split {
+        workgroup_memory_barrier_with_group_sync();
+        if lane < ndofs {
+            for r in 0..ndofs {
+                mat.write(
+                    sm_idx_packed::<T>(slot, r, lane),
+                    mass_matrices.read(m_view.idx(r, lane)),
+                );
+            }
+        }
+        workgroup_memory_barrier_with_group_sync();
+        lu_factor_in_shared_packed::<T, MATN, SLOTS>(
+            ndofs,
+            max_ndofs,
+            slot,
+            lane,
+            active_slot,
+            mat,
+            lu_pivots,
+            piv,
+            pivot_row_shared,
+            inv_akk_shared,
+        );
+        if lane < ndofs {
+            for r in 0..ndofs {
+                mass_matrices
+                    .write(m_view.idx(r, lane), mat.read(sm_idx_packed::<T>(slot, r, lane)));
+            }
+        }
+    }
+}
+
+/// Serial version of the fused gravity + LU kernel.
+#[spirv_bindgen]
+#[spirv(compute(threads(64, 1, 1)))]
+pub fn gpu_mb_gravity_and_lu_t1(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    links_static: &[MultibodyLinkStatic],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
+    links_workspace: &mut [Vec4],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] body_jacobians: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] gen_forces: &mut [f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] mass_matrices: &mut [f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 6)] lu_pivots: &mut [u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 7)] dof_state: &[f32],
+    #[spirv(uniform, descriptor_set = 0, binding = 8)] gravity: &Vec4,
+    #[spirv(uniform, descriptor_set = 0, binding = 9)] batch_ids: &BatchIndices,
+    #[spirv(uniform, descriptor_set = 0, binding = 10)] dt_uniform: &f32,
+) {
+    let num_mb = batch_ids.multibodies_len;
+    if invocation_id.x >= num_mb * batch_ids.num_batches {
+        return;
+    }
+    let batch_id = invocation_id.x / num_mb;
+    let mb_idx = invocation_id.x % num_mb;
+
+    let mb = batch_ids
+        .ib(batch_id, multibody_info)
+        .read(mb_idx as usize);
+    let num_links = mb.num_links;
+    let ndofs = mb.ndofs;
+    if ndofs == 0 {
+        return;
+    }
+    let mb_jac_base = mb.jacobian_offset as usize;
+    let gen_base = mb.first_dof as usize;
+    let mb_mm_base = mb.mass_matrix_offset as usize;
+    let piv = batch_ids.ivec(batch_id, gen_base);
+
+    let stat_slice = batch_ids
+        .ib(batch_id, links_static)
+        .offset(mb.first_link as usize);
+    let wa = WsAddr::new(mb.first_link as usize, batch_ids.num_batches, batch_id);
+    let vel_slice = batch_ids.ib(batch_id, dof_state).offset(gen_base);
+    let damping_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(batch_ids.dof_batch_capacity as usize + gen_base);
+    let stiffness_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(3 * batch_ids.dof_batch_capacity as usize + gen_base);
+    let spring_ref_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(4 * batch_ids.dof_batch_capacity as usize + gen_base);
+    let kin_mask_slice = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(5 * batch_ids.dof_batch_capacity as usize + gen_base);
+
+    // ---- Phase 1: zero the generalized-force vector. ----
+    for d in 0..ndofs {
+        gen_forces.write(batch_ids.mbi(batch_id, gen_base + d as usize), 0.0);
+    }
+
+    #[cfg(feature = "dim3")]
+    let g = Vec3::new(gravity.x, gravity.y, gravity.z);
+    #[cfg(feature = "dim2")]
+    let g = Vec2::new(gravity.x, gravity.y);
+
+    // ---- Phase 2: per-link gravity / Coriolis-force assembly (serial:
+    // parents precede children in link order, so `kinematic_acc` reads see
+    // the parent's write in program order). ----
+    for k in 0..num_links {
+        let mut acc_lin = Vector::ZERO;
+        #[cfg(feature = "dim3")]
+        let mut acc_ang: AngVector = AngVector::ZERO;
+        #[cfg(feature = "dim2")]
+        let mut acc_ang: AngVector = 0.0;
+
+        let (self_joint_vel_lin, self_joint_vel_ang, self_shift02, self_shift23, self_rb_ang) = {
+            let jv = ws_vel(links_workspace, wa, k, WS_JOINT_VEL);
+            (
+                jv.linear,
+                jv.angular,
+                ws_vec(links_workspace, wa, k, WS_SHIFT02),
+                ws_vec(links_workspace, wa, k, WS_SHIFT23),
+                ws_vel_ang(links_workspace, wa, k, WS_RB_VELS),
+            )
+        };
+
+        if k != 0 {
+            let stat = stat_slice[k as usize];
+            let pid = stat.parent_link_id;
+            let parent_acc = ws_vel(links_workspace, wa, pid, WS_KIN_ACC);
+            let parent_acc_lin = parent_acc.linear;
+            let parent_acc_ang = parent_acc.angular;
+            let parent_ang = ws_vel_ang(links_workspace, wa, pid, WS_RB_VELS);
+
+            acc_lin = parent_acc_lin;
+            acc_ang = parent_acc_ang;
+
+            acc_lin += gcross_av(parent_ang, self_joint_vel_lin) * 2.0;
+            #[cfg(feature = "dim3")]
+            {
+                acc_ang += parent_ang.cross(self_joint_vel_ang);
+            }
+            #[cfg(feature = "dim2")]
+            {
+                let _ = self_joint_vel_ang;
+            }
+            acc_lin += gcross_av(parent_ang, gcross_av(parent_ang, self_shift02));
+            acc_lin += gcross_av(parent_acc_ang, self_shift02);
+        } else {
+            let _ = self_joint_vel_ang;
+            let _ = self_shift02;
+        }
+        let rb_ang = self_rb_ang;
+        acc_lin += gcross_av(rb_ang, gcross_av(rb_ang, self_shift23));
+        acc_lin += gcross_av(acc_ang, self_shift23);
+
+        ws_set_vel(links_workspace, wa, k, WS_KIN_ACC, Velocity::new(acc_lin, acc_ang));
+
+        let lmp = stat_slice[k as usize].local_mprops;
+        let inv_mass_x = lmp.inv_mass.x;
+        if inv_mass_x != 0.0 {
+            let mass = 1.0 / inv_mass_x;
+            let rb_inertia = ws_world_inertia(links_workspace, wa, k, &lmp);
+
+            #[cfg(feature = "dim3")]
+            let gyroscopic = {
+                let i_omega = rb_inertia * rb_ang;
+                rb_ang.cross(i_omega)
+            };
+            #[cfg(feature = "dim2")]
+            let gyroscopic: AngVector = 0.0;
+
+            let i_acc_ang = rb_inertia * acc_ang;
+
+            let f_lin = (g - acc_lin) * mass;
+            let f_ang = -gyroscopic - i_acc_ang;
+
+            let body_jacobian = batch_ids.imat(batch_id, 
+                mb_jac_base + (k as usize) * SPATIAL_DIM * (ndofs as usize),
+                SPATIAL_DIM as u32,
+                ndofs,
+            );
+
+            // Single lane owns the whole gemv (lane = 0, lanes = 1).
+            gemv_tr_spatial_split_par(
+                gen_forces,
+                batch_ids.ivec(batch_id, gen_base),
+                1.0,
+                body_jacobians,
+                body_jacobian,
+                f_lin,
+                f_ang,
+                1.0,
+                0,
+                1,
+            );
+        }
+    }
+
+    // Damping subtraction.
+    for i in 0..ndofs {
+        let idx = batch_ids.mbi(batch_id, gen_base + i as usize);
+        let cur = gen_forces.read(idx);
+        gen_forces.write(idx, cur - damping_slice[i as usize] * vel_slice[i as usize]);
+    }
+
+    // Per-DoF joint springs.
+    apply_spring_forces(
+        gen_forces,
+        batch_ids,
+        batch_id,
+        gen_base,
+        &stat_slice,
+        links_workspace,
+        wa,
+        num_links,
+        &stiffness_slice,
+        &spring_ref_slice,
+        &vel_slice,
+        *dt_uniform,
+    );
+
+    // Kinematic DOFs get zero acceleration.
+    for i in 0..ndofs {
+        if kin_mask_slice[i as usize] != 0.0 {
+            gen_forces.write(batch_ids.mbi(batch_id, gen_base + i as usize), 0.0);
+        }
+    }
+
+    // ---- Phase 3 + 4: factor the acceleration matrix in place in global
+    // memory, solve M·x = τ in place on the gravity rhs; in split mode
+    // (see `gpu_mb_gravity_and_lu`), then factor the plain matrix in place;
+    // its LU + pivots are what the constraint columns consume. ----
+    let m_view = batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs);
+    let acc_section = batch_ids.mass_matrix_acc_section_offset as usize;
+    let split = acc_section != 0;
+    let m_acc_view = if split {
+        batch_ids.imat(batch_id, acc_section + mb_mm_base, ndofs, ndofs)
+    } else {
+        m_view
+    };
+    lu_decompose(mass_matrices, m_acc_view, lu_pivots, piv);
+    lu_solve_in_place(
+        mass_matrices,
+        m_acc_view,
+        lu_pivots,
+        piv,
+        gen_forces,
+        batch_ids.ivec(batch_id, gen_base),
+    );
+    if split {
+        lu_decompose(mass_matrices, m_view, lu_pivots, piv);
+    }
+}
+
+/// Stamps one packed-tier entry point of the fused gravity + LU kernel.
+/// `MATN = 64·T`, `SLOTS = 64/T`.
+macro_rules! gravity_and_lu_packed_entry {
+    ($(#[$doc:meta])* $name:ident, $t:literal, $matn:literal, $slots:literal) => {
+        $(#[$doc])*
+        #[spirv_bindgen]
+        #[spirv(compute(threads(64, 1, 1)))]
+        pub fn $name(
+            #[spirv(workgroup_id)] wg_id: UVec3,
+            #[spirv(local_invocation_id)] lid: UVec3,
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
+            multibody_info: &[MultibodyInfo],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+            links_static: &[MultibodyLinkStatic],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
+            links_workspace: &mut [Vec4],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] body_jacobians: &[f32],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] gen_forces: &mut [f32],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] mass_matrices: &mut [f32],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 6)] lu_pivots: &mut [u32],
+            #[spirv(storage_buffer, descriptor_set = 0, binding = 7)] dof_state: &[f32],
+            #[spirv(uniform, descriptor_set = 0, binding = 8)] gravity: &Vec4,
+            #[spirv(uniform, descriptor_set = 0, binding = 9)] batch_ids: &BatchIndices,
+            #[spirv(uniform, descriptor_set = 0, binding = 10)] dt_uniform: &f32,
+            #[spirv(workgroup)] mat: &mut [f32; $matn],
+            #[spirv(workgroup)] x: &mut [f32; 64],
+            #[spirv(workgroup)] partial: &mut [f32; 64],
+            #[spirv(workgroup)] pivot_row_shared: &mut [u32; $slots],
+            #[spirv(workgroup)] inv_akk_shared: &mut [f32; $slots],
+        ) {
+            gravity_and_lu_packed_impl::<$t, $matn, $slots>(
+                wg_id,
+                lid,
+                multibody_info,
+                links_static,
+                links_workspace,
+                body_jacobians,
+                gen_forces,
+                mass_matrices,
+                lu_pivots,
+                dof_state,
+                gravity,
+                batch_ids,
+                dt_uniform,
+                mat,
+                x,
+                partial,
+                pivot_row_shared,
+                inv_akk_shared,
+            );
+        }
+    };
+}
+
+gravity_and_lu_packed_entry!(
+    /// We have `max_ndofs ≤ 8` => 8 multibodies per workgroup.
+    gpu_mb_gravity_and_lu_t8, 8u32, 512, 8
+);
+gravity_and_lu_packed_entry!(
+    /// We have `max_ndofs ≤ 16` => 4 multibodies per workgroup.
+    gpu_mb_gravity_and_lu_t16, 16u32, 1024, 4
+);
+gravity_and_lu_packed_entry!(
+    /// We have `max_ndofs ≤ 32` => 2 multibodies per workgroup.
+    gpu_mb_gravity_and_lu_t32, 32u32, 2048, 2
+);
diff --git a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/helper.rs b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/helper.rs
index 45634ab..49ecc03 100644
--- a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/helper.rs
+++ b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/helper.rs
@@ -12,6 +12,7 @@ use crate::{AngVector, MAX_FLT, Pose, Vector, rotation_to_matrix};
 
 use super::super::types::MultibodyInfo;
 use super::jacobians::*;
+use crate::utils::linalg::VSlice;
 use super::types::*;
 
 /// `JointConstraintHelper`-equivalent: precomputed per-joint quantities used
@@ -176,7 +177,7 @@ pub(super) fn lock_jacobians_generic(
     ang_jac1: AngVector,
     ang_jac2: AngVector,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -213,7 +214,7 @@ pub(super) fn lock_jacobians_generic(
                 lin_jac,
                 ang_jac2,
                 body_jacobians,
-                jac_start,
+                il,
             );
 
             out.kind = 1;
@@ -253,7 +254,7 @@ pub(super) fn lock_jacobians_generic(
             lin_jac,
             ang_jac1,
             body_jacobians,
-            jac_start,
+            il,
         );
     }
 
@@ -281,7 +282,7 @@ pub(super) fn lock_jacobians_generic(
             lin_jac,
             ang_jac2,
             body_jacobians,
-            jac_start,
+            il,
         );
     }
 
@@ -381,7 +382,7 @@ pub(super) fn lock_linear_generic(
     j_id_a: u32,
     j_id_b: u32,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -406,7 +407,7 @@ pub(super) fn lock_linear_generic(
         ang_jac1,
         ang_jac2,
         body_jacobians,
-        jac_start,
+        il,
         mprops,
         colliders_start,
     );
@@ -434,7 +435,7 @@ pub(super) fn lock_angular_generic(
     j_id_a: u32,
     j_id_b: u32,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -450,7 +451,7 @@ pub(super) fn lock_angular_generic(
         ang_jac,
         ang_jac,
         body_jacobians,
-        jac_start,
+        il,
         mprops,
         colliders_start,
     );
@@ -479,7 +480,7 @@ pub(super) fn limit_linear_generic(
     j_id_a: u32,
     j_id_b: u32,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -504,7 +505,7 @@ pub(super) fn limit_linear_generic(
         ang_jac1,
         ang_jac2,
         body_jacobians,
-        jac_start,
+        il,
         mprops,
         colliders_start,
     );
@@ -539,7 +540,7 @@ pub(super) fn limit_angular_generic(
     j_id_a: u32,
     j_id_b: u32,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -555,7 +556,7 @@ pub(super) fn limit_angular_generic(
         ang_jac,
         ang_jac,
         body_jacobians,
-        jac_start,
+        il,
         mprops,
         colliders_start,
     );
@@ -591,7 +592,7 @@ pub(super) fn motor_linear_generic(
     j_id_a: u32,
     j_id_b: u32,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -617,7 +618,7 @@ pub(super) fn motor_linear_generic(
         ang_jac1,
         ang_jac2,
         body_jacobians,
-        jac_start,
+        il,
         mprops,
         colliders_start,
     );
@@ -655,7 +656,7 @@ pub(super) fn motor_angular_generic(
     j_id_a: u32,
     j_id_b: u32,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
     mprops: &[WorldMassProperties],
     colliders_start: usize,
 ) {
@@ -672,7 +673,7 @@ pub(super) fn motor_angular_generic(
         ang_jac,
         ang_jac,
         body_jacobians,
-        jac_start,
+        il,
         mprops,
         colliders_start,
     );
diff --git a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/jacobians.rs b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/jacobians.rs
index ca9e9e3..c7a589d 100644
--- a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/jacobians.rs
+++ b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/jacobians.rs
@@ -6,7 +6,7 @@ use khal_std::sync::workgroup_memory_barrier_with_group_sync;
 
 use crate::dynamics::body::{Velocity, WorldMassProperties};
 use crate::dynamics::joint::SPATIAL_DIM;
-use crate::utils::linalg::MatSlice;
+use crate::utils::linalg::{MatSlice, VSlice};
 use crate::{ANG_DIM, AngVector, DIM, Vector};
 
 use super::super::lu::LANES;
@@ -70,7 +70,7 @@ pub(super) fn side_dot_vel_par(
     body_id: u32,
     jacobians: &[f32],
     dof_vels: &[f32],
-    dof_base_for_mb: usize,
+    dof_base_for_mb: VSlice,
     solver_vels: &[Velocity],
     colliders_start: usize,
     lane: u32,
@@ -93,7 +93,7 @@ pub(super) fn side_dot_vel_par(
         // SIDE_KIND_MB
         if lane < ndofs {
             jacobians.read(j_id as usize + lane as usize)
-                * dof_vels.read(dof_base_for_mb + lane as usize)
+                * dof_vels.read(dof_base_for_mb.at(lane))
         } else {
             0.0f32
         }
@@ -131,7 +131,7 @@ pub(super) fn side_apply_impulse_par(
     delta: f32,
     jacobians: &[f32],
     dof_vels: &mut [f32],
-    dof_base_for_mb: usize,
+    dof_base_for_mb: VSlice,
     solver_vels: &mut [Velocity],
     colliders_start: usize,
     lane: u32,
@@ -167,7 +167,7 @@ pub(super) fn side_apply_impulse_par(
     }
     // SIDE_KIND_MB — lane l owns DOF l (disjoint → race-free).
     if lane < ndofs {
-        let v_idx = dof_base_for_mb + lane as usize;
+        let v_idx = dof_base_for_mb.at(lane);
         let cur = dof_vels.read(v_idx);
         let w = jacobians.read(wj0 + lane as usize);
         dof_vels.write(v_idx, cur + scaled * w);
@@ -245,12 +245,13 @@ pub(super) fn fill_mb_jacobians(
     unit_force: Vector,
     unit_torque: AngVector,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
 ) {
     let ndofs = mb.ndofs;
-    let mb_jac_base = jac_start + mb.jacobian_offset as usize;
+    let mb_jac_base = mb.jacobian_offset as usize;
     let link_jac_base = mb_jac_base + (link_id as usize) * SPATIAL_DIM * (ndofs as usize);
-    let link_j = MatSlice::dense(link_jac_base, SPATIAL_DIM as u32, ndofs);
+    let link_j =
+        MatSlice::interleaved(link_jac_base, SPATIAL_DIM as u32, ndofs, il.stride, il.shift);
     let (link_j_v, link_j_w) = link_j.rows_range_pair(0, DIM, DIM, ANG_DIM);
 
     // 1) j = link_J^T · (unit_force, unit_torque). Same kernel used by
@@ -311,14 +312,14 @@ pub(super) fn fill_relative_mb_jacobians(
     lin_b: Vector,
     ang_b: AngVector,
     body_jacobians: &[f32],
-    jac_start: usize,
+    il: VSlice,
 ) {
     let ndofs = mb.ndofs;
-    let mb_jac_base = jac_start + mb.jacobian_offset as usize;
+    let mb_jac_base = mb.jacobian_offset as usize;
     let la_base = mb_jac_base + (link_a as usize) * SPATIAL_DIM * (ndofs as usize);
     let lb_base = mb_jac_base + (link_b as usize) * SPATIAL_DIM * (ndofs as usize);
-    let la = MatSlice::dense(la_base, SPATIAL_DIM as u32, ndofs);
-    let lb = MatSlice::dense(lb_base, SPATIAL_DIM as u32, ndofs);
+    let la = MatSlice::interleaved(la_base, SPATIAL_DIM as u32, ndofs, il.stride, il.shift);
+    let lb = MatSlice::interleaved(lb_base, SPATIAL_DIM as u32, ndofs, il.stride, il.shift);
     let (la_v, la_w) = la.rows_range_pair(0, DIM, DIM, ANG_DIM);
     let (lb_v, lb_w) = lb.rows_range_pair(0, DIM, DIM, ANG_DIM);
 
diff --git a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/kernels.rs b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/kernels.rs
index b643e9d..25cd281 100644
--- a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/kernels.rs
+++ b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/kernels.rs
@@ -2,6 +2,7 @@
 //! (update / finalize / solve / remove-bias).
 
 use khal_std::glamx::UVec3;
+use glamx::Vec4;
 use khal_std::index::MaybeIndexUnchecked;
 use khal_std::macros::{spirv, spirv_bindgen};
 use khal_std::sync::workgroup_memory_barrier_with_group_sync;
@@ -10,9 +11,10 @@ use crate::Pose;
 use crate::dynamics::ConstraintSoftness;
 use crate::dynamics::body::{Velocity, WorldMassProperties};
 use crate::utils::BatchIndices;
+use crate::utils::linalg::VSlice;
 
 use super::super::lu::LANES;
-use super::super::types::{MultibodyInfo, MultibodyLinkWorkspace};
+use super::super::types::MultibodyInfo;
 
 use super::jacobians::*;
 use super::types::*;
@@ -35,7 +37,7 @@ pub fn gpu_mb_update_impulse_joint_constraints(
     #[spirv(uniform, descriptor_set = 0, binding = 3)] softness: &ConstraintSoftness,
     #[spirv(storage_buffer, descriptor_set = 1, binding = 0)] multibody_info: &[MultibodyInfo],
     #[spirv(storage_buffer, descriptor_set = 1, binding = 1)]
-    links_workspace: &[MultibodyLinkWorkspace],
+    links_workspace: &[Vec4],
     #[spirv(storage_buffer, descriptor_set = 1, binding = 2)] body_jacobians: &[f32],
     #[spirv(storage_buffer, descriptor_set = 1, binding = 3)] poses: &[Pose],
     #[spirv(storage_buffer, descriptor_set = 1, binding = 4)] mprops: &[WorldMassProperties],
@@ -57,9 +59,9 @@ pub fn gpu_mb_update_impulse_joint_constraints(
     let joints_start = batch_ids.mb_imp_joints_start(batch_id);
     let cons_start = batch_ids.mb_imp_joint_constraints_start(batch_id);
     let jac_buf_start = batch_ids.mb_imp_joint_jacobians_start(batch_id);
-    let mb_start = batch_ids.mb_start(batch_id);
-    let links_start = batch_ids.links_start(batch_id);
-    let body_jac_start = batch_ids.jac_start(batch_id);
+    // Interleaved dynamics-buffer view (multibody_info / links_workspace /
+    // body_jacobians / mass_matrices / lu_pivots / dof_state).
+    let il = VSlice::interleaved(0, batch_ids.num_batches, batch_id);
     let colliders_start = batch_ids.coll_start(batch_id);
 
     // Loop chunked across `num_threads` so a single workgroup row processes
@@ -80,11 +82,9 @@ pub fn gpu_mb_update_impulse_joint_constraints(
                 jacobians,
                 jac_buf_start,
                 multibody_info,
-                mb_start,
                 links_workspace,
-                links_start,
                 body_jacobians,
-                body_jac_start,
+                il,
                 poses,
                 colliders_start,
                 mprops,
@@ -124,9 +124,7 @@ pub fn gpu_mb_finalize_impulse_joint_constraints(
 
     let joints_start = batch_ids.mb_imp_joints_start(batch_id);
     let cons_start = batch_ids.mb_imp_joint_constraints_start(batch_id);
-    let mb_start = batch_ids.mb_start(batch_id);
-    let mm_start = batch_ids.mm_start(batch_id);
-    let dof_start = batch_ids.dof_start(batch_id);
+    let il = VSlice::interleaved(0, batch_ids.num_batches, batch_id);
 
     let mut i = invocation_id.x;
     while i < cap {
@@ -144,29 +142,27 @@ pub fn gpu_mb_finalize_impulse_joint_constraints(
                     // is stored on side B as a normal MB jacobian, so it's
                     // handled here too.
                     if c.side_a_kind == SIDE_KIND_MB && c.ndofs_a > 0 {
-                        let mb = multibody_info.read(mb_start + c.side_a_id as usize);
+                        let mb = multibody_info.read(il.atz(c.side_a_id as usize));
                         solve_mb_wj(
                             jacobians,
                             c.j_id_a,
                             c.ndofs_a,
                             &mb,
                             mass_matrices,
-                            mm_start,
                             lu_pivots,
-                            dof_start,
+                            il,
                         );
                     }
                     if c.side_b_kind == SIDE_KIND_MB && c.ndofs_b > 0 {
-                        let mb = multibody_info.read(mb_start + c.side_b_id as usize);
+                        let mb = multibody_info.read(il.atz(c.side_b_id as usize));
                         solve_mb_wj(
                             jacobians,
                             c.j_id_b,
                             c.ndofs_b,
                             &mb,
                             mass_matrices,
-                            mm_start,
                             lu_pivots,
-                            dof_start,
+                            il,
                         );
                     }
                     c.finalize_generic_constraint(jacobians);
@@ -210,8 +206,7 @@ pub fn gpu_mb_solve_impulse_joint_constraints(
 
     let joints_start = batch_ids.mb_imp_joints_start(batch_id);
     let cons_start = batch_ids.mb_imp_joint_constraints_start(batch_id);
-    let mb_start = batch_ids.mb_start(batch_id);
-    let dof_start = batch_ids.dof_start(batch_id);
+    let il = VSlice::interleaved(0, batch_ids.num_batches, batch_id);
     let colliders_start = batch_ids.coll_start(batch_id);
 
     // `color_groups` is a per-batch prefix-sum over the color-sorted
@@ -240,16 +235,16 @@ pub fn gpu_mb_solve_impulse_joint_constraints(
 
     // Per-multibody dof base: same for every axis constraint of this joint.
     let dof_base_a = if builder.side_a_kind == SIDE_KIND_MB {
-        let mb = multibody_info.at(mb_start + builder.side_a_id as usize);
-        dof_start + mb.first_dof as usize
+        let mb = multibody_info.at(il.atz(builder.side_a_id as usize));
+        VSlice::interleaved(mb.first_dof as usize, il.stride, il.shift)
     } else {
-        0
+        VSlice::dense(0)
     };
     let dof_base_b = if builder.side_b_kind == SIDE_KIND_MB {
-        let mb = multibody_info.at(mb_start + builder.side_b_id as usize);
-        dof_start + mb.first_dof as usize
+        let mb = multibody_info.at(il.atz(builder.side_b_id as usize));
+        VSlice::interleaved(mb.first_dof as usize, il.stride, il.shift)
     } else {
-        0
+        VSlice::dense(0)
     };
 
     // TODO(PERF): load jacobians into shared memory and keep the velocity deltat on shared
diff --git a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/types.rs b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/types.rs
index 36f9057..125a07f 100644
--- a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/types.rs
+++ b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/types.rs
@@ -69,7 +69,6 @@ pub struct MbImpulseJointBuilder {
     pub jacobian_offset: u32,
     /// Total floats reserved for this joint's jacobian block.
     pub jacobian_capacity: u32,
-    /// Pad to GenericJoint's alignment (16 bytes in 3D — see ImpulseJoint).
     #[cfg(feature = "dim3")]
     pub _pad0: [u32; 2],
 }
diff --git a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/update.rs b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/update.rs
index 5e3dabd..8143248 100644
--- a/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/update.rs
+++ b/src_rbd_shaders/dynamics/multibody/impulse_joint_constraints/update.rs
@@ -1,14 +1,16 @@
 //! Per-joint (re)build of all axis constraints (`update_one_joint`) and the
 //! multibody-side `M⁻¹·Jᵀ` back-solve used by the finalize pass.
 
+use glamx::Vec4;
 use khal_std::index::MaybeIndexUnchecked;
 
 use crate::dynamics::body::WorldMassProperties;
 use crate::dynamics::joint::{ANG_AXES_MASK, LIN_AXES_MASK, SPATIAL_DIM};
-use crate::utils::linalg::{MatSlice, lu_solve_in_place};
+use crate::utils::linalg::{MatSlice, lu_solve_in_place, VSlice};
 use crate::{DIM, Pose};
 
-use super::super::types::{MultibodyInfo, MultibodyLinkWorkspace};
+use super::super::types::MultibodyInfo;
+use super::super::ws_soa::{WS_LTW, WsAddr, ws_pose};
 use super::helper::*;
 use super::jacobians::*;
 use super::types::*;
@@ -24,9 +26,10 @@ pub(super) fn solve_mb_wj(
     ndofs: u32,
     mb: &MultibodyInfo,
     mass_matrices: &[f32],
-    mm_start: usize,
     lu_pivots: &[u32],
-    dof_start: usize,
+    // Interleaved dynamics-buffer view (`stride = num_batches`, `shift =
+    // batch_id`).
+    il: VSlice,
 ) {
     // Copy J into the W·J slot, then LU back-solve in place (matches the old
     // fused path: `wj = M⁻¹·j`).
@@ -35,10 +38,15 @@ pub(super) fn solve_mb_wj(
         let v = jacobians.read(j_id as usize + k as usize);
         jacobians.write(wj_base + k as usize, v);
     }
-    let mb_mm_base = mm_start + mb.mass_matrix_offset as usize;
-    let m = MatSlice::dense(mb_mm_base, ndofs, ndofs);
-    let piv_offset = dof_start + mb.first_dof as usize;
-    lu_solve_in_place(mass_matrices, m, lu_pivots, piv_offset, jacobians, wj_base);
+    let m = MatSlice::interleaved(
+        mb.mass_matrix_offset as usize,
+        ndofs,
+        ndofs,
+        il.stride,
+        il.shift,
+    );
+    let piv = VSlice::interleaved(mb.first_dof as usize, il.stride, il.shift);
+    lu_solve_in_place(mass_matrices, m, lu_pivots, piv, jacobians, VSlice::dense(wj_base));
 }
 
 impl MbImpulseJointBuilder {
@@ -50,11 +58,11 @@ impl MbImpulseJointBuilder {
         jacobians: &mut [f32],
         jac_buf_start: usize,
         multibody_info: &[MultibodyInfo],
-        mb_start: usize,
-        links_workspace: &[MultibodyLinkWorkspace],
-        links_start: usize,
+        links_workspace: &[Vec4],
         body_jacobians: &[f32],
-        body_jac_start: usize,
+        // Interleaved dynamics-buffer view (`stride = num_batches`, `shift =
+        // batch_id`).
+        il: VSlice,
         poses: &[Pose],
         colliders_start: usize,
         mprops: &[WorldMassProperties],
@@ -78,12 +86,12 @@ impl MbImpulseJointBuilder {
         // SPIR-V's "pointer to arbitrary element" restriction). Free / fixed
         // sides ignore the read.
         let mb_a = if self.side_a_kind == SIDE_KIND_MB {
-            multibody_info.read(mb_start + self.side_a_id as usize)
+            multibody_info.read(il.atz(self.side_a_id as usize))
         } else {
             MultibodyInfo::default()
         };
         let mb_b = if self.side_b_kind == SIDE_KIND_MB {
-            multibody_info.read(mb_start + self.side_b_id as usize)
+            multibody_info.read(il.atz(self.side_b_id as usize))
         } else {
             MultibodyInfo::default()
         };
@@ -94,7 +102,7 @@ impl MbImpulseJointBuilder {
             self.side_a_link,
             &mb_a,
             links_workspace,
-            links_start,
+            il,
             poses,
             colliders_start,
         );
@@ -104,7 +112,7 @@ impl MbImpulseJointBuilder {
             self.side_b_link,
             &mb_b,
             links_workspace,
-            links_start,
+            il,
             poses,
             colliders_start,
         );
@@ -187,7 +195,7 @@ impl MbImpulseJointBuilder {
                     j_id_a,
                     j_id_b,
                     body_jacobians,
-                    body_jac_start,
+                    il,
                     mprops,
                     colliders_start,
                 );
@@ -219,7 +227,7 @@ impl MbImpulseJointBuilder {
                     j_id_a,
                     j_id_b,
                     body_jacobians,
-                    body_jac_start,
+                    il,
                     mprops,
                     colliders_start,
                 );
@@ -251,7 +259,7 @@ impl MbImpulseJointBuilder {
                     j_id_a,
                     j_id_b,
                     body_jacobians,
-                    body_jac_start,
+                    il,
                     mprops,
                     colliders_start,
                 );
@@ -283,7 +291,7 @@ impl MbImpulseJointBuilder {
                     j_id_a,
                     j_id_b,
                     body_jacobians,
-                    body_jac_start,
+                    il,
                     mprops,
                     colliders_start,
                 );
@@ -317,7 +325,7 @@ impl MbImpulseJointBuilder {
                     j_id_a,
                     j_id_b,
                     body_jacobians,
-                    body_jac_start,
+                    il,
                     mprops,
                     colliders_start,
                 );
@@ -351,7 +359,7 @@ impl MbImpulseJointBuilder {
                     j_id_a,
                     j_id_b,
                     body_jacobians,
-                    body_jac_start,
+                    il,
                     mprops,
                     colliders_start,
                 );
@@ -380,8 +388,8 @@ pub(super) fn side_world_pose(
     side_id: u32,
     side_link: u32,
     mb: &MultibodyInfo,
-    links_workspace: &[MultibodyLinkWorkspace],
-    links_start: usize,
+    links_workspace: &[Vec4],
+    il: VSlice,
     poses: &[Pose],
     colliders_start: usize,
 ) -> Pose {
@@ -391,6 +399,6 @@ pub(super) fn side_world_pose(
     if side_kind == SIDE_KIND_BODY {
         return poses.read(colliders_start + side_id as usize);
     }
-    let link_global = links_start + mb.first_link as usize + side_link as usize;
-    links_workspace.read(link_global).local_to_world
+    let wa = WsAddr::new(mb.first_link as usize, il.stride, il.shift);
+    ws_pose(links_workspace, wa, side_link, WS_LTW)
 }
diff --git a/src_rbd_shaders/dynamics/multibody/integrate.rs b/src_rbd_shaders/dynamics/multibody/integrate.rs
index 5c1b5b6..9414a3b 100644
--- a/src_rbd_shaders/dynamics/multibody/integrate.rs
+++ b/src_rbd_shaders/dynamics/multibody/integrate.rs
@@ -5,19 +5,21 @@
 //! refresh link poses.
 
 use khal_std::glamx::UVec3;
+use glamx::Vec4;
 use khal_std::index::MaybeIndexUnchecked;
 use khal_std::macros::{spirv, spirv_bindgen};
 
 #[cfg(feature = "dim2")]
 use crate::rotation_from_angle;
-use crate::utils::{BatchIndices, Slice, SliceMut};
+use crate::utils::BatchIndices;
 use crate::{ANG_DIM, DIM};
 #[cfg(feature = "dim3")]
 use crate::{Vector, rotation_from_scaled_axis, rotation_renormalize_fast};
 #[cfg(feature = "dim3")]
 use parry::math::VectorExt;
 
-use super::types::{MultibodyInfo, MultibodyLinkStatic, MultibodyLinkWorkspace};
+use super::types::{MultibodyInfo, MultibodyLinkStatic};
+use super::ws_soa::{WS_JOINT_ROT, WsAddr, ws_coord, ws_rot, ws_set_coord, ws_set_rot};
 
 /// Update generalized velocities: `v += a · dt`.
 ///
@@ -25,7 +27,7 @@ use super::types::{MultibodyInfo, MultibodyLinkStatic, MultibodyLinkWorkspace};
 /// constraints can run in between (rapier's order: velocity update → constraint
 /// solver → position update).
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_integrate_velocities(
     #[spirv(global_invocation_id)] invocation_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
@@ -34,21 +36,24 @@ pub fn gpu_mb_integrate_velocities(
     #[spirv(uniform, descriptor_set = 0, binding = 3)] dt_uniform: &f32,
     #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
     let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
+    if invocation_id.x >= num_mb * batch_ids.num_batches {
         return;
     }
+    let batch_id = invocation_id.x / num_mb;
+    let mb_idx = invocation_id.x % num_mb;
     let dt = *dt_uniform;
 
     let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
+        .ib(batch_id, multibody_info)
         .read(mb_idx as usize);
-    let gen_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
 
-    let mut dof_vel = SliceMut(dof_state, gen_base);
-    let acc = Slice(gen_accelerations, gen_base);
+    let mut dof_vel = batch_ids
+        .ib_mut(batch_id, dof_state)
+        .offset(mb.first_dof as usize);
+    let acc = batch_ids
+        .ib(batch_id, gen_accelerations)
+        .offset(mb.first_dof as usize);
 
     for d in 0..mb.ndofs {
         let di = d as usize;
@@ -57,41 +62,41 @@ pub fn gpu_mb_integrate_velocities(
 }
 
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_integrate(
     #[spirv(global_invocation_id)] invocation_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
     links_static: &[MultibodyLinkStatic],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
-    links_workspace: &mut [MultibodyLinkWorkspace],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] links_workspace: &mut [Vec4],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] dof_values: &mut [f32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] dof_state: &[f32],
     #[spirv(uniform, descriptor_set = 0, binding = 5)] dt_uniform: &f32,
     #[spirv(uniform, descriptor_set = 0, binding = 6)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
     let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
+    if invocation_id.x >= num_mb * batch_ids.num_batches {
         return;
     }
+    let batch_id = invocation_id.x / num_mb;
+    let mb_idx = invocation_id.x % num_mb;
     let dt = *dt_uniform;
 
     let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
+        .ib(batch_id, multibody_info)
         .read(mb_idx as usize);
     let num_links = mb.num_links;
-    let gen_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
 
     let stat_slice = batch_ids
-        .mb_links_batch(batch_id, links_static)
+        .ib(batch_id, links_static)
         .offset(mb.first_link as usize);
-    let mut ws_slice = batch_ids
-        .mb_links_batch_mut(batch_id, links_workspace)
-        .offset(mb.first_link as usize);
-    let dof_val = SliceMut(dof_values, gen_base);
-    let dof_vel = Slice(dof_state, gen_base);
+    let wa = WsAddr::new(mb.first_link as usize, batch_ids.num_batches, batch_id);
+    let dof_val = batch_ids
+        .ib_mut(batch_id, dof_values)
+        .offset(mb.first_dof as usize);
+    let dof_vel = batch_ids
+        .ib(batch_id, dof_state)
+        .offset(mb.first_dof as usize);
 
     // Per-link coord / joint_rot update (uses the already-corrected `dof_velocities`).
     //
@@ -99,18 +104,17 @@ pub fn gpu_mb_integrate(
     // in place through `&mut ws_slice[k]` so SPIR-V emits field-targeted stores
     // instead of a whole `MultibodyLinkWorkspace` round-trip (~240 B in 3D).
     for k in 0..num_links {
-        let k_usize = k as usize;
-        let stat = stat_slice[k_usize];
+        let stat = stat_slice[k as usize];
         let locked = stat.data.locked_axes;
         let aid = stat.assembly_id as usize;
-        let ws = &mut ws_slice[k_usize];
 
         // Free linear DOFs first, in axis order.
         let mut curr_free = 0u32;
         for i in 0..DIM {
             if (locked & (1 << i)) == 0 {
                 let v = dof_vel[aid + curr_free as usize];
-                *ws.coords.at_mut(i as usize) += v * dt;
+                let cur = ws_coord(links_workspace, wa, k, i);
+                ws_set_coord(links_workspace, wa, k, i, cur + v * dt);
                 curr_free += 1;
             }
         }
@@ -124,16 +128,22 @@ pub fn gpu_mb_integrate(
                 let dof_id = (!ang_locked & 0x7).trailing_zeros();
                 let v = dof_vel[aid + curr_free as usize];
                 let idx = 3 + dof_id;
-                let new = ws.coords.read(idx as usize) + v * dt;
-                ws.coords.write(idx as usize, new);
-                ws.joint_rot = rotation_from_scaled_axis(Vector::ith(dof_id as usize, new));
+                let new = ws_coord(links_workspace, wa, k, idx) + v * dt;
+                ws_set_coord(links_workspace, wa, k, idx, new);
+                ws_set_rot(
+                    links_workspace,
+                    wa,
+                    k,
+                    WS_JOINT_ROT,
+                    rotation_from_scaled_axis(Vector::ith(dof_id as usize, new)),
+                );
             }
             #[cfg(feature = "dim2")]
             {
                 let v = dof_vel[aid + curr_free as usize];
-                let new = ws.coords.read(DIM as usize) + v * dt;
-                ws.coords.write(DIM as usize, new);
-                ws.joint_rot = rotation_from_angle(new);
+                let new = ws_coord(links_workspace, wa, k, DIM) + v * dt;
+                ws_set_coord(links_workspace, wa, k, DIM, new);
+                ws_set_rot(links_workspace, wa, k, WS_JOINT_ROT, rotation_from_angle(new));
             }
         } else if num_ang == 3 {
             #[cfg(feature = "dim3")]
@@ -143,10 +153,20 @@ pub fn gpu_mb_integrate(
                 let vz = dof_vel[aid + (curr_free + 2) as usize];
                 let ang = Vector::new(vx, vy, vz);
                 let disp = rotation_from_scaled_axis(ang * dt);
-                ws.joint_rot = rotation_renormalize_fast(disp * ws.joint_rot);
-                *ws.coords.at_mut(3) += vx * dt;
-                *ws.coords.at_mut(4) += vy * dt;
-                *ws.coords.at_mut(5) += vz * dt;
+                let jr = ws_rot(links_workspace, wa, k, WS_JOINT_ROT);
+                ws_set_rot(
+                    links_workspace,
+                    wa,
+                    k,
+                    WS_JOINT_ROT,
+                    rotation_renormalize_fast(disp * jr),
+                );
+                let c3 = ws_coord(links_workspace, wa, k, 3);
+                ws_set_coord(links_workspace, wa, k, 3, c3 + vx * dt);
+                let c4 = ws_coord(links_workspace, wa, k, 4);
+                ws_set_coord(links_workspace, wa, k, 4, c4 + vy * dt);
+                let c5 = ws_coord(links_workspace, wa, k, 5);
+                ws_set_coord(links_workspace, wa, k, 5, c5 + vz * dt);
             }
         }
         // num_ang == 0: no-op.
@@ -154,5 +174,5 @@ pub fn gpu_mb_integrate(
 
     // Silence dof_val unused warning — it will be used once we also support
     // setting coords directly (e.g. user-controlled kinematic DOFs).
-    let _ = dof_val.0;
+    let _ = dof_val.buf;
 }
diff --git a/src_rbd_shaders/dynamics/multibody/joint_constraints.rs b/src_rbd_shaders/dynamics/multibody/joint_constraints.rs
index 29a4511..cc2a292 100644
--- a/src_rbd_shaders/dynamics/multibody/joint_constraints.rs
+++ b/src_rbd_shaders/dynamics/multibody/joint_constraints.rs
@@ -5,18 +5,24 @@
 //! are skipped).
 
 use khal_std::glamx::UVec3;
+use glamx::Vec4;
 use khal_std::index::MaybeIndexUnchecked;
+use khal_std::iter::StepRng;
 use khal_std::macros::{spirv, spirv_bindgen};
+use khal_std::sync::control_barrier;
 
 use crate::dynamics::ConstraintSoftness;
 use crate::dynamics::joint::SPATIAL_DIM;
 use crate::utils::BatchIndices;
-use crate::utils::linalg::{MatSlice, lu_solve_in_place};
+use crate::utils::linalg::{MatSlice, VSlice, lu_solve_in_place};
 use crate::{DIM, MAX_FLT};
 
 use super::types::{
-    MultibodyInfo, MultibodyJointConstraint, MultibodyLinkStatic, MultibodyLinkWorkspace,
+    MB_JOINT_KIND_COUPLING, MB_JOINT_KIND_LIMIT, MB_JOINT_KIND_LIMIT_INACTIVE,
+    MB_JOINT_KIND_MOTOR, MbDofCoupling, MultibodyInfo, MultibodyJointConstraint,
+    MultibodyLinkStatic,
 };
+use super::ws_soa::{WsAddr, ws_coord};
 
 /// Compute joint motor parameters mirroring rapier's `JointMotor::motor_params`.
 #[inline]
@@ -33,139 +39,63 @@ fn motor_params(motor: &crate::dynamics::joint::JointMotor, dt: f32) -> (f32, f3
     )
 }
 
-/// Solve `M · x = e_d` in place (writes `x` into `dst[0..n]`). Uses the same
-/// LU factor + pivots produced by `gpu_mb_lu_decompose`.
+/// Solve `M · x = J` in place (writes `x` into `dst[0..n]`) where `J =
+/// e_{dof_id} − coeff·e_{dof2_id}` (a plain unit rhs when `coeff == 0`).
+/// Uses the same LU factor + pivots produced by `gpu_mb_lu_decompose`.
 #[inline]
+#[allow(clippy::too_many_arguments)]
 fn lu_solve_unit(
     buf_m: &[f32],
     m: MatSlice,
     buf_pivots: &[u32],
-    pivots_offset: usize,
+    piv: VSlice,
     dst: &mut [f32],
     dst_offset: usize,
     dof_id: u32,
+    dof2_id: u32,
+    coeff: f32,
 ) {
     let n = m.rows;
-    // dst[0..n] := e_{dof_id}  (then permuted by lu_solve_in_place).
+    // dst[0..n] := e_{dof_id} − coeff·e_{dof2_id} (then permuted by
+    // lu_solve_in_place).
     for i in 0..n {
-        dst[dst_offset + i as usize] = if i == dof_id { 1.0 } else { 0.0 };
-    }
-    lu_solve_in_place(buf_m, m, buf_pivots, pivots_offset, dst, dst_offset);
-}
-
-/// PGS sweep body — shared between `gpu_mb_solve_joint_constraints` and
-/// the fused init+solve / remove-bias+solve kernels. Writes back `cons`
-/// before subtracting `delta · column` from `v`.
-#[inline]
-fn solve_joint_constraints_body(
-    multibody_info: &[MultibodyInfo],
-    joint_constraints: &mut [MultibodyJointConstraint],
-    joint_constraint_columns: &[f32],
-    dof_state: &mut [f32],
-    batch_id: u32,
-    mb_idx: u32,
-    batch_ids: &BatchIndices,
-) {
-    let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
-        .read(mb_idx as usize);
-    let ndofs = mb.ndofs;
-    if ndofs == 0 || mb.max_constraints == 0 {
-        return;
-    }
-    let v_base = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
-    let cons_base = batch_ids.mb_joint_constraints_start(batch_id) + mb.first_constraint as usize;
-    let dofs_stride = batch_ids.dof_batch_capacity as usize;
-    let col_base = batch_ids.mb_joint_constraint_columns_start(batch_id)
-        + (mb.first_constraint as usize) * dofs_stride;
-
-    for s in 0..mb.max_constraints {
-        let mut cons = joint_constraints.read(cons_base + s as usize);
-        if cons.kind == 0 {
-            continue;
-        }
-
-        let v_d = dof_state.read(v_base + cons.dof_id as usize);
-        let rhs_total = v_d + cons.rhs;
-        let raw_imp = cons.impulse + cons.inv_lhs * (rhs_total - cons.cfm_gain * cons.impulse);
-        let mut new_imp = raw_imp;
-        if new_imp < cons.impulse_lo {
-            new_imp = cons.impulse_lo;
-        }
-        if new_imp > cons.impulse_hi {
-            new_imp = cons.impulse_hi;
-        }
-        let delta = new_imp - cons.impulse;
-        cons.impulse = new_imp;
-        joint_constraints.write(cons_base + s as usize, cons);
-
-        for i in 0..ndofs {
-            let v_idx = v_base + i as usize;
-            let cur = dof_state.read(v_idx);
-            let col =
-                joint_constraint_columns.read(col_base + (s as usize) * dofs_stride + i as usize);
-            dof_state.write(v_idx, cur - delta * col);
+        let mut v = if i == dof_id { 1.0 } else { 0.0 };
+        if i == dof2_id {
+            v -= coeff;
         }
+        dst[dst_offset + i as usize] = v;
     }
+    lu_solve_in_place(buf_m, m, buf_pivots, piv, dst, VSlice::dense(dst_offset));
 }
 
+/// Serially writes the metadata of every active limit/motor constraint slot.
 #[inline]
-fn init_joint_constraints_body(
-    multibody_info: &[MultibodyInfo],
+#[allow(clippy::too_many_arguments)]
+fn emit_joint_constraints(
     links_static: &[MultibodyLinkStatic],
-    links_workspace: &[MultibodyLinkWorkspace],
-    mass_matrices: &[f32],
-    lu_pivots: &[u32],
+    links_workspace: &[Vec4],
+    dof_couplings: &[MbDofCoupling],
     joint_constraints: &mut [MultibodyJointConstraint],
-    joint_constraint_columns: &mut [f32],
+    mb: &MultibodyInfo,
+    cons_base: usize,
     batch_id: u32,
-    mb_idx: u32,
     dt: f32,
     joint_erp_inv_dt: f32,
     joint_cfm_coeff: f32,
     batch_ids: &BatchIndices,
 ) {
-    let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
-        .read(mb_idx as usize);
     let num_links = mb.num_links;
-    let ndofs = mb.ndofs;
-    if ndofs == 0 {
-        return;
-    }
-    let mb_mm_base = batch_ids.mm_start(batch_id) + mb.mass_matrix_offset as usize;
-    let piv_offset = batch_ids.dof_start(batch_id) + mb.first_dof as usize;
-    let cons_base = batch_ids.mb_joint_constraints_start(batch_id) + mb.first_constraint as usize;
-    // One column of M⁻¹ per constraint slot — `dof_batch_capacity` floats
-    // per slot (only the first `ndofs` of each are meaningful, but we use
-    // the batch-wide max as the stride to match the host allocation
-    // `cons_col_cap = cons_cap * dofs_cap` and to avoid two multibodies
-    // with different ndofs stomping on each other's columns).
-    let dofs_stride = batch_ids.dof_batch_capacity as usize;
-    let col_base = batch_ids.mb_joint_constraint_columns_start(batch_id)
-        + (mb.first_constraint as usize) * dofs_stride;
 
     let stat_slice = batch_ids
-        .mb_links_batch(batch_id, links_static)
-        .offset(mb.first_link as usize);
-    let ws_slice = batch_ids
-        .mb_links_batch(batch_id, links_workspace)
+        .ib(batch_id, links_static)
         .offset(mb.first_link as usize);
-    let m = MatSlice::dense(mb_mm_base, ndofs, ndofs);
-
-    for s in 0..mb.max_constraints {
-        let mut cz: MultibodyJointConstraint = joint_constraints.read(cons_base + s as usize);
-        cz.kind = 0;
-        cz.impulse = 0.0;
-        joint_constraints.write(cons_base + s as usize, cz);
-    }
+    let wa = WsAddr::new(mb.first_link as usize, batch_ids.num_batches, batch_id);
 
     let inv_dt = if dt != 0.0 { 1.0 / dt } else { 0.0 };
 
     let mut slot = 0u32;
     for k in 0..num_links {
         let stat = &stat_slice[k as usize];
-        let ws = &ws_slice[k as usize];
         let locked = stat.data.locked_axes;
         let limit_axes = stat.data.limit_axes & !locked;
         let motor_axes = stat.data.motor_axes & !locked;
@@ -188,21 +118,16 @@ fn init_joint_constraints_body(
                 continue;
             }
             let abs_dof = stat.assembly_id + curr_free_dof;
-            let curr_pos = ws.coords.read(axis as usize);
+            let curr_pos = ws_coord(links_workspace, wa, k, axis);
 
             if (motor_axes & (1 << axis)) != 0 {
                 let has_limits = (limit_axes & (1 << axis)) != 0;
                 let limit_min = stat.data.limits[axis as usize].min;
                 let limit_max = stat.data.limits[axis as usize].max;
-                emit_motor_constraint(
-                    joint_constraints,
-                    joint_constraint_columns,
-                    cons_base,
-                    col_base,
-                    dofs_stride,
-                    slot,
+                let cons = build_motor_constraint(
                     abs_dof,
-                    ndofs,
+                    k,
+                    axis,
                     curr_pos,
                     inv_dt,
                     dt,
@@ -210,23 +135,15 @@ fn init_joint_constraints_body(
                     has_limits,
                     limit_min,
                     limit_max,
-                    mass_matrices,
-                    m,
-                    lu_pivots,
-                    piv_offset,
                 );
+                joint_constraints.write(cons_base + slot as usize, cons);
                 slot += 1;
             }
             if (limit_axes & (1 << axis)) != 0 {
-                emit_limit_constraint(
-                    joint_constraints,
-                    joint_constraint_columns,
-                    cons_base,
-                    col_base,
-                    dofs_stride,
-                    slot,
+                let cons = build_limit_constraint(
                     abs_dof,
-                    ndofs,
+                    k,
+                    axis,
                     curr_pos,
                     [
                         stat.data.limits[axis as usize].min,
@@ -234,11 +151,8 @@ fn init_joint_constraints_body(
                     ],
                     joint_erp_inv_dt,
                     joint_cfm_coeff,
-                    mass_matrices,
-                    m,
-                    lu_pivots,
-                    piv_offset,
                 );
+                joint_constraints.write(cons_base + slot as usize, cons);
                 slot += 1;
             }
             curr_free_dof += 1;
@@ -250,18 +164,13 @@ fn init_joint_constraints_body(
                 continue;
             }
             let abs_dof = stat.assembly_id + curr_free_dof;
-            let curr_pos = ws.coords.read(axis as usize);
+            let curr_pos = ws_coord(links_workspace, wa, k, axis);
 
             if (limit_axes & (1 << axis)) != 0 {
-                emit_limit_constraint(
-                    joint_constraints,
-                    joint_constraint_columns,
-                    cons_base,
-                    col_base,
-                    dofs_stride,
-                    slot,
+                let cons = build_limit_constraint(
                     abs_dof,
-                    ndofs,
+                    k,
+                    axis,
                     curr_pos,
                     [
                         stat.data.limits[axis as usize].min,
@@ -269,26 +178,18 @@ fn init_joint_constraints_body(
                     ],
                     joint_erp_inv_dt,
                     joint_cfm_coeff,
-                    mass_matrices,
-                    m,
-                    lu_pivots,
-                    piv_offset,
                 );
+                joint_constraints.write(cons_base + slot as usize, cons);
                 slot += 1;
             }
             if (motor_axes & (1 << axis)) != 0 {
                 let has_limits = (limit_axes & (1 << axis)) != 0;
                 let limit_min = stat.data.limits[axis as usize].min;
                 let limit_max = stat.data.limits[axis as usize].max;
-                emit_motor_constraint(
-                    joint_constraints,
-                    joint_constraint_columns,
-                    cons_base,
-                    col_base,
-                    dofs_stride,
-                    slot,
+                let cons = build_motor_constraint(
                     abs_dof,
-                    ndofs,
+                    k,
+                    axis,
                     curr_pos,
                     inv_dt,
                     dt,
@@ -296,21 +197,42 @@ fn init_joint_constraints_body(
                     has_limits,
                     limit_min,
                     limit_max,
-                    mass_matrices,
-                    m,
-                    lu_pivots,
-                    piv_offset,
                 );
+                joint_constraints.write(cons_base + slot as usize, cons);
                 slot += 1;
             }
             curr_free_dof += 1;
         }
     }
+
+    // DoF couplings: one bilateral row per coupling, solved among the joint
+    // constraints (rapier's `coupling_velocity_constraints`).
+    let coupling_base = batch_ids.mb_dof_couplings_start(batch_id) + mb.first_coupling as usize;
+    for c in 0..mb.num_couplings {
+        let coupling = dof_couplings.read(coupling_base + c as usize);
+        let q1 = ws_coord(
+            links_workspace,
+            wa,
+            coupling.link_axis1 & 0xffff,
+            coupling.link_axis1 >> 16,
+        );
+        let q2 = ws_coord(
+            links_workspace,
+            wa,
+            coupling.link_axis2 & 0xffff,
+            coupling.link_axis2 >> 16,
+        );
+        let cons = build_coupling_constraint(&coupling, q1, q2, joint_erp_inv_dt);
+        joint_constraints.write(cons_base + slot as usize, cons);
+        slot += 1;
+    }
 }
 
-/// Solve `M · column = e_{dof_id}` (writes the M⁻¹ column) and return the raw
-/// `lhs = column[dof_id]` for J = e_{dof_id}.
+/// Solve `M · column = J` (writes the `M⁻¹·J` column) and return the raw
+/// `lhs = J·M⁻¹·J`, for `J = e_{dof_id} − coeff·e_{dof2_id}` (`coeff == 0`
+/// for limit / motor rows).
 #[inline]
+#[allow(clippy::too_many_arguments)]
 fn compute_constraint_column(
     joint_constraint_columns: &mut [f32],
     col_base: usize,
@@ -318,10 +240,12 @@ fn compute_constraint_column(
     dofs_stride: usize,
     ndofs: u32,
     dof_id: u32,
+    dof2_id: u32,
+    coeff: f32,
     mass_matrices: &[f32],
     m: MatSlice,
     lu_pivots: &[u32],
-    piv_offset: usize,
+    piv: VSlice,
 ) -> f32 {
     let _ = ndofs;
     let col_offset = col_base + (slot as usize) * dofs_stride;
@@ -329,12 +253,15 @@ fn compute_constraint_column(
         mass_matrices,
         m,
         lu_pivots,
-        piv_offset,
+        piv,
         joint_constraint_columns,
         col_offset,
         dof_id,
+        dof2_id,
+        coeff,
     );
     joint_constraint_columns.read(col_offset + dof_id as usize)
+        - coeff * joint_constraint_columns.read(col_offset + dof2_id as usize)
 }
 
 /// `1 / x`, or 0 when `x == 0` — matches rapier's `crate::utils::inv`.
@@ -343,96 +270,101 @@ fn inv(x: f32) -> f32 {
     if x != 0.0 { 1.0 / x } else { 0.0 }
 }
 
-/// Initialize a single limit constraint slot. Mirrors rapier's
-/// `unit_joint_limit_constraint`.
+/// Initialize a single limit constraint slot.
 #[inline]
-fn emit_limit_constraint(
-    joint_constraints: &mut [MultibodyJointConstraint],
-    joint_constraint_columns: &mut [f32],
-    cons_base: usize,
-    col_base: usize,
-    dofs_stride: usize,
-    slot: u32,
+#[allow(clippy::too_many_arguments)]
+fn build_limit_constraint(
     dof_id: u32,
-    ndofs: u32,
+    link_id: u32,
+    axis: u32,
     curr_pos: f32,
     limits: [f32; 2],
     erp_inv_dt: f32,
     cfm_coeff: f32,
-    mass_matrices: &[f32],
-    m: MatSlice,
-    lu_pivots: &[u32],
-    piv_offset: usize,
-) {
+) -> MultibodyJointConstraint {
     // rapier (`limit_*` builder): erp_inv_dt = joint.softness.erp_inv_dt(dt),
     // cfm_coeff = joint.softness.cfm_coeff(dt), cfm_gain = 0 — configurable via
     // `joint_natural_frequency` / `joint_damping_ratio` (defaults make this
     // near-rigid, matching the old hardcoded `1/dt`).
     let min_enabled = curr_pos < limits[0];
     let max_enabled = limits[1] < curr_pos;
-    // No limit is active, skip the constraint for the current substep.
-    if !min_enabled && !max_enabled {
-        return;
-    }
     let lo_excess = (limits[0] - curr_pos).max(0.0);
     let hi_excess = (curr_pos - limits[1]).max(0.0);
     let rhs_bias = (hi_excess - lo_excess) * erp_inv_dt;
     let rhs_wo_bias = 0.0f32;
 
-    let lhs = compute_constraint_column(
-        joint_constraint_columns,
-        col_base,
-        slot,
-        dofs_stride,
-        ndofs,
-        dof_id,
-        mass_matrices,
-        m,
-        lu_pivots,
-        piv_offset,
-    );
-    // rapier `finalize_generic_constraints` (the multibody-internal constraints
-    // ARE finalized, after `unit_joint_limit_constraint` sets the preliminary
-    // 1/lhs): cfm_gain = lhs·cfm_coeff + cfm_gain_init; inv_lhs = 1/(lhs +
-    // cfm_gain). The PGS sweep then applies `cfm_gain` directly. For limits
-    // cfm_gain_init = 0 and (near-rigid) cfm_coeff ≈ 2e-6, so this is ~1/lhs —
-    // but we replicate the formula exactly so it's correct for any softness.
-    let cfm_gain = lhs * cfm_coeff;
-    let inv_lhs = inv(lhs + cfm_gain);
-
     let max_neg_impulse = if min_enabled { -MAX_FLT } else { 0.0 };
     let max_pos_impulse = if max_enabled { MAX_FLT } else { 0.0 };
 
-    let cons = MultibodyJointConstraint {
+    let kind = if min_enabled || max_enabled {
+        MB_JOINT_KIND_LIMIT
+    } else {
+        // Inactive this substep: the solve skips it, the finalize stage still
+        // back-solves its column for later refreshes.
+        MB_JOINT_KIND_LIMIT_INACTIVE
+    };
+
+    MultibodyJointConstraint {
         dof_id,
-        kind: 1,
-        _kind_extra: 0,
-        _pad0: 0,
+        kind,
+        _kind_extra: link_id | (axis << 16),
+        dof2_id: 0,
         rhs: rhs_wo_bias + rhs_bias,
         rhs_wo_bias,
-        inv_lhs,
+        inv_lhs: 0.0,
         impulse: 0.0,
         impulse_lo: max_neg_impulse,
         impulse_hi: max_pos_impulse,
         cfm_coeff,
-        cfm_gain,
-    };
-    joint_constraints.write(cons_base + slot as usize, cons);
+        // This will be calculated in the finalize (orthogonalization) step.
+        cfm_gain: 0.0,
+        coupling_coeff: 0.0,
+        coupling_offset: 0.0,
+        _kind_extra2: 0,
+        _pad1: 0,
+    }
 }
 
-/// Initialize a single motor constraint slot. Mirrors rapier's
-/// `unit_joint_motor_constraint`. `has_limits` + `(limit_min, limit_max)` flatten
-/// rapier's `Option<[Real; 2]>` parameter (rust-gpu can't represent enums).
+/// Initialize a single DoF-coupling constraint slot. Mirrors rapier's
+/// `Multibody::coupling_velocity_constraints`: the coupling `q2 = coeff·q1 +
+/// offset` is one bilateral constraint with jacobian `J = e_{g2} −
+/// coeff·e_{g1}` and a rhs pulling the position drift back to zero.
 #[inline]
-fn emit_motor_constraint(
-    joint_constraints: &mut [MultibodyJointConstraint],
-    joint_constraint_columns: &mut [f32],
-    cons_base: usize,
-    col_base: usize,
-    dofs_stride: usize,
-    slot: u32,
+fn build_coupling_constraint(
+    coupling: &MbDofCoupling,
+    q1: f32,
+    q2: f32,
+    erp_inv_dt: f32,
+) -> MultibodyJointConstraint {
+    let drift = q2 - coupling.coeff * q1 - coupling.offset;
+
+    MultibodyJointConstraint {
+        dof_id: coupling.dof2,
+        kind: MB_JOINT_KIND_COUPLING,
+        _kind_extra: coupling.link_axis2,
+        dof2_id: coupling.dof1,
+        rhs: drift * erp_inv_dt,
+        rhs_wo_bias: 0.0,
+        inv_lhs: 0.0,
+        impulse: 0.0,
+        impulse_lo: -MAX_FLT,
+        impulse_hi: MAX_FLT,
+        cfm_coeff: 0.0,
+        cfm_gain: 0.0,
+        coupling_coeff: coupling.coeff,
+        coupling_offset: coupling.offset,
+        _kind_extra2: coupling.link_axis1,
+        _pad1: 0,
+    }
+}
+
+/// Initialize a single motor constraint slot..
+#[inline]
+#[allow(clippy::too_many_arguments)]
+fn build_motor_constraint(
     dof_id: u32,
-    ndofs: u32,
+    link_id: u32,
+    axis: u32,
     curr_pos: f32,
     inv_dt: f32,
     dt: f32,
@@ -440,11 +372,7 @@ fn emit_motor_constraint(
     has_limits: bool,
     limit_min: f32,
     limit_max: f32,
-    mass_matrices: &[f32],
-    m: MatSlice,
-    lu_pivots: &[u32],
-    piv_offset: usize,
-) {
+) -> MultibodyJointConstraint {
     let (erp_inv_dt, cfm_coeff, cfm_gain, _, max_impulse) = motor_params(motor, dt);
 
     let mut rhs_wo_bias = 0.0f32;
@@ -465,45 +393,24 @@ fn emit_motor_constraint(
     }
     rhs_wo_bias += -target_vel;
 
-    let lhs = compute_constraint_column(
-        joint_constraint_columns,
-        col_base,
-        slot,
-        dofs_stride,
-        ndofs,
+    MultibodyJointConstraint {
         dof_id,
-        mass_matrices,
-        m,
-        lu_pivots,
-        piv_offset,
-    );
-    // rapier `finalize_generic_constraints` (run after `unit_joint_motor_
-    // constraint` sets the preliminary 1/lhs): cfm_gain = lhs·cfm_coeff +
-    // cfm_gain_init; inv_lhs = 1/(lhs + cfm_gain). The PGS sweep then applies
-    // `cfm_gain` (`impulse + inv_lhs·(rhs - cfm_gain·impulse)`). This is the
-    // ACCELERATION-based motor's compliance: for an acceleration-based position
-    // servo (`<position kp>`), `cfm_coeff` is large (∝ 1/(dt²·stiffness)), so the
-    // fold dominates and makes the effective gain inertia-independent — without
-    // it the servo is far too weak and the robot sags. `cfm_gain` here is
-    // `motor_params.cfm_gain` (nonzero only for force-based motors).
-    let cfm_gain = lhs * cfm_coeff + cfm_gain;
-    let inv_lhs = inv(lhs + cfm_gain);
-
-    let cons = MultibodyJointConstraint {
-        dof_id,
-        kind: 2,
-        _kind_extra: 0,
-        _pad0: 0,
+        kind: MB_JOINT_KIND_MOTOR,
+        _kind_extra: link_id | (axis << 16),
+        dof2_id: 0,
         rhs: rhs_wo_bias,
         rhs_wo_bias,
-        inv_lhs,
+        inv_lhs: 0.0,
         impulse: 0.0,
         impulse_lo: -max_impulse,
         impulse_hi: max_impulse,
         cfm_coeff,
         cfm_gain,
-    };
-    joint_constraints.write(cons_base + slot as usize, cons);
+        coupling_coeff: 0.0,
+        coupling_offset: 0.0,
+        _kind_extra2: 0,
+        _pad1: 0,
+    }
 }
 
 /// Initialize the multibody's joint-limit / joint-motor unit constraints.
@@ -514,122 +421,128 @@ fn emit_motor_constraint(
 ///
 /// Must run after `gpu_mb_lu_decompose` — the LU factors of `M` are used to compute
 /// the per-constraint M⁻¹ column and effective inverse mass.
+///
+/// One 64-lane workgroup per (multibody, batch), in three stages:
+///   1. lane-parallel: zero all constraint slots;
+///   2. lane 0: the serial link walk emitting constraint metadata (cheap);
+///   3. lane-parallel: one M⁻¹-column LU back-solve per emitted slot plus
+///      rapier's `finalize_generic_constraints`.
 #[spirv_bindgen]
-#[spirv(compute(threads(1)))]
+#[spirv(compute(threads(64)))]
 pub fn gpu_mb_init_joint_constraints(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
     links_static: &[MultibodyLinkStatic],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
-    links_workspace: &[MultibodyLinkWorkspace],
+    links_workspace: &[Vec4],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] mass_matrices: &[f32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] lu_pivots: &[u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 5)]
     joint_constraints: &mut [MultibodyJointConstraint],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 6)]
     joint_constraint_columns: &mut [f32],
-    #[spirv(uniform, descriptor_set = 0, binding = 7)] softness: &ConstraintSoftness,
-    #[spirv(uniform, descriptor_set = 0, binding = 8)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 7)]
+    dof_couplings: &[MbDofCoupling],
+    #[spirv(uniform, descriptor_set = 0, binding = 8)] softness: &ConstraintSoftness,
+    #[spirv(uniform, descriptor_set = 0, binding = 9)] batch_ids: &BatchIndices,
 ) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
-    let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
-        return;
-    }
-    init_joint_constraints_body(
-        multibody_info,
-        links_static,
-        links_workspace,
-        mass_matrices,
-        lu_pivots,
-        joint_constraints,
-        joint_constraint_columns,
-        batch_id,
-        mb_idx,
-        softness.dt,
-        softness.joint_erp_inv_dt,
-        softness.joint_cfm_coeff,
-        batch_ids,
-    );
-}
+    const LANES: u32 = 64;
 
-/// One PGS sweep: iterates the multibody's active limit/motor constraints and
-/// updates `dof_velocities` in place. Mirrors rapier's `JointConstraint::solve_generic`
-/// for a 1-DOF jacobian.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_mb_solve_joint_constraints(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
-    joint_constraints: &mut [MultibodyJointConstraint],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
-    joint_constraint_columns: &mut [f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] dof_state: &mut [f32],
-    #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
-) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
+    // One workgroup per (multibody, batch): grid `[mbs · LANES, batches, 1]`.
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
     let num_mb = batch_ids.multibodies_len;
     if mb_idx >= num_mb {
         return;
     }
-    solve_joint_constraints_body(
-        multibody_info,
-        joint_constraints,
-        joint_constraint_columns,
-        dof_state,
-        batch_id,
-        mb_idx,
-        batch_ids,
-    );
-}
 
-/// Fused `remove_bias + solve_without_bias` for joint constraints — runs once
-/// per substep at the end of the substep, after position integration. Drops
-/// one threads(1) dispatch per substep.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_mb_remove_solve_joint_no_bias(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
-    joint_constraints: &mut [MultibodyJointConstraint],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] joint_constraint_columns: &[f32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] dof_state: &mut [f32],
-    #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
-) {
-    let batch_id = invocation_id.y;
-    let mb_idx = invocation_id.x;
-    let num_mb = batch_ids.multibodies_len;
-    if mb_idx >= num_mb {
+    let mb = batch_ids
+        .ib(batch_id, multibody_info)
+        .read(mb_idx as usize);
+    let ndofs = mb.ndofs;
+    // Uniform per workgroup: every lane of this group returns together.
+    if ndofs == 0 {
         return;
     }
 
-    let mb = batch_ids
-        .mb_batch(batch_id, multibody_info)
-        .read(mb_idx as usize);
+    let mb_mm_base = mb.mass_matrix_offset as usize;
+    let piv = batch_ids.ivec(batch_id, mb.first_dof as usize);
     let cons_base = batch_ids.mb_joint_constraints_start(batch_id) + mb.first_constraint as usize;
+    // One column of M⁻¹ per constraint slot .
+    let dofs_stride = batch_ids.dof_batch_capacity as usize;
+    let col_base = batch_ids.mb_joint_constraint_columns_start(batch_id)
+        + (mb.first_constraint as usize) * dofs_stride;
+    let m = batch_ids.imat(batch_id, mb_mm_base, ndofs, ndofs);
+
+    // Stage 1: lane-parallel slot reset.
+    for s in StepRng::new(lane..mb.max_constraints, LANES) {
+        let mut cz: MultibodyJointConstraint = joint_constraints.read(cons_base + s as usize);
+        cz.kind = 0;
+        cz.impulse = 0.0;
+        joint_constraints.write(cons_base + s as usize, cz);
+    }
 
-    // Inlined `remove_bias`: replace `rhs` with `rhs_wo_bias` for active slots.
-    for s in 0..mb.max_constraints {
+    control_barrier::<
+        { khal_std::memory::Scope::Workgroup as u32 },
+        { khal_std::memory::Scope::QueueFamily as u32 },
+        {
+            khal_std::memory::Semantics::UNIFORM_MEMORY.bits()
+                | khal_std::memory::Semantics::ACQUIRE_RELEASE.bits()
+        },
+    >();
+
+    // Stage 2: serial metadata emission on lane 0.
+    if lane == 0 {
+        emit_joint_constraints(
+            links_static,
+            links_workspace,
+            dof_couplings,
+            joint_constraints,
+            &mb,
+            cons_base,
+            batch_id,
+            softness.dt,
+            softness.joint_erp_inv_dt,
+            softness.joint_cfm_coeff,
+            batch_ids,
+        );
+    }
+
+    control_barrier::<
+        { khal_std::memory::Scope::Workgroup as u32 },
+        { khal_std::memory::Scope::QueueFamily as u32 },
+        {
+            khal_std::memory::Semantics::UNIFORM_MEMORY.bits()
+                | khal_std::memory::Semantics::ACQUIRE_RELEASE.bits()
+        },
+    >();
+
+    // Stage 3: lane-parallel finalize.
+    for s in StepRng::new(lane..mb.max_constraints, LANES) {
         let mut cons = joint_constraints.read(cons_base + s as usize);
         if cons.kind == 0 {
             continue;
         }
-        cons.rhs = cons.rhs_wo_bias;
+        let lhs = compute_constraint_column(
+            joint_constraint_columns,
+            col_base,
+            s,
+            dofs_stride,
+            ndofs,
+            cons.dof_id,
+            cons.dof2_id,
+            cons.coupling_coeff,
+            mass_matrices,
+            m,
+            lu_pivots,
+            piv,
+        );
+        let cfm_gain = lhs * cons.cfm_coeff + cons.cfm_gain;
+        cons.cfm_gain = cfm_gain;
+        cons.inv_lhs = inv(lhs + cfm_gain);
         joint_constraints.write(cons_base + s as usize, cons);
     }
-
-    solve_joint_constraints_body(
-        multibody_info,
-        joint_constraints,
-        joint_constraint_columns,
-        dof_state,
-        batch_id,
-        mb_idx,
-        batch_ids,
-    );
 }
diff --git a/src_rbd_shaders/dynamics/multibody/lu.rs b/src_rbd_shaders/dynamics/multibody/lu.rs
index af33947..58d81f6 100644
--- a/src_rbd_shaders/dynamics/multibody/lu.rs
+++ b/src_rbd_shaders/dynamics/multibody/lu.rs
@@ -7,7 +7,7 @@
 use khal_std::index::MaybeIndexUnchecked;
 use khal_std::sync::workgroup_memory_barrier_with_group_sync;
 
-use crate::utils::linalg::MAX_MB_DOFS;
+use crate::utils::linalg::{MAX_MB_DOFS, VSlice};
 
 /// Workgroup width for the parallelised LU kernels. Must match the
 /// `threads(N, 1, 1)` attribute and `MB_LU_LANES` on the host side.
@@ -27,16 +27,15 @@ pub(super) fn sm_idx(r: u32, c: u32) -> usize {
 #[inline]
 pub(super) fn lu_factor_in_shared(
     n: u32,
+    max_n: u32,
     lane: u32,
     mat: &mut [f32; MAX_MB_DOFS * MAX_MB_DOFS],
     pivots_dst: &mut [u32],
-    pivots_offset: usize,
+    piv: VSlice,
     pivot_row_shared: &mut u32,
     inv_akk_shared: &mut f32,
 ) {
-    // NOTE: fixed number of iterations for uniform control flow.
-    // TODO(PERF): on non-web platforms we could just use `n` as the upper bound.
-    for k in 0..MAX_DOFS_U32 {
+    for k in 0..max_n {
         let active = k < n;
         if active && lane == 0 {
             let mut pivot_row = k;
@@ -53,7 +52,7 @@ pub(super) fn lu_factor_in_shared(
                 }
             }
             *pivot_row_shared = pivot_row;
-            pivots_dst.write(pivots_offset + k as usize, pivot_row);
+            pivots_dst.write(piv.at(k), pivot_row);
         }
         workgroup_memory_barrier_with_group_sync();
         let pivot_row = *pivot_row_shared;
@@ -104,14 +103,13 @@ pub(super) fn lu_factor_in_shared(
 #[inline]
 pub(super) fn lu_triangular_solve_in_place(
     n: u32,
+    max_n: u32,
     lane: u32,
     mat: &[f32; MAX_MB_DOFS * MAX_MB_DOFS],
     x: &mut [f32; MAX_MB_DOFS],
     partial: &mut [f32; LANES as usize],
 ) {
-    // NOTE: fixed number of iterations for uniform control flow.
-    // TODO(PERF): on non-web platforms we could just use `n` as the upper bound.
-    for i in 0..MAX_DOFS_U32 {
+    for i in 0..max_n {
         let active = i < n;
         let s = if active && lane < i {
             mat.read(sm_idx(i, lane)) * x.read(lane as usize)
@@ -135,11 +133,9 @@ pub(super) fn lu_triangular_solve_in_place(
         workgroup_memory_barrier_with_group_sync();
     }
 
-    // NOTE: fixed number of iterations for uniform control flow.
-    // TODO(PERF): on non-web platforms we could just use `n` as the upper bound.
-    for step in 0..MAX_DOFS_U32 {
+    for step in 0..max_n {
         let active = step < n;
-        // For dummy iterations (step >= n), `i` is not meaningful — guard
+        // For dummy iterations (step >= n), `i` is not meaningful; guard
         // every use of it behind `active`.
         let i = if active { n - 1 - step } else { 0 };
         let s = if active && lane > i && lane < n {
@@ -166,6 +162,198 @@ pub(super) fn lu_triangular_solve_in_place(
     }
 }
 
+/*
+ * Packed variants: `SLOTS = 64 / T` multibodies per 64-lane workgroup, each
+ * owning `T` lanes and a `T×T` shared tile. Shrinking the tile from
+ * `MAX_MB_DOFS² = 16 KB` to `64·T` floats lifts the shared-memory occupancy
+ * cap that made the one-multibody-per-workgroup layout latency-bound when
+ * every environment holds one small robot. All barriers stay at uniform
+ * points: every slot executes the same `max_n`-bounded loops in lock-step,
+ * inactive slots simply skip their stores.
+ */
+
+/// Index into the packed shared tile: slot-`slot`'s `T×T` column-major tile.
+#[inline]
+pub(super) fn sm_idx_packed<const T: u32>(slot: u32, r: u32, c: u32) -> usize {
+    (slot * T * T + c * T + r) as usize
+}
+
+/// Packed [`lu_factor_in_shared`]: factor each slot's `T×T` tile in place.
+/// `lane` is slot-relative (`0..T`).
+#[inline]
+#[allow(clippy::too_many_arguments)]
+pub(super) fn lu_factor_in_shared_packed<const T: u32, const MATN: usize, const SLOTS: usize>(
+    n: u32,
+    max_n: u32,
+    slot: u32,
+    lane: u32,
+    active_slot: bool,
+    mat: &mut [f32; MATN],
+    pivots_dst: &mut [u32],
+    piv: VSlice,
+    pivot_row_shared: &mut [u32; SLOTS],
+    inv_akk_shared: &mut [f32; SLOTS],
+) {
+    for k in 0..max_n {
+        let active = active_slot && k < n;
+        if active && lane == 0 {
+            let mut pivot_row = k;
+            let mut pivot_val = {
+                let v = mat.read(sm_idx_packed::<T>(slot, k, k));
+                if v >= 0.0 { v } else { -v }
+            };
+            for i in (k + 1)..n {
+                let v = mat.read(sm_idx_packed::<T>(slot, i, k));
+                let av = if v >= 0.0 { v } else { -v };
+                if av > pivot_val {
+                    pivot_val = av;
+                    pivot_row = i;
+                }
+            }
+            pivot_row_shared.write(slot as usize, pivot_row);
+            pivots_dst.write(piv.at(k), pivot_row);
+        }
+        workgroup_memory_barrier_with_group_sync();
+        let pivot_row = pivot_row_shared.read(slot as usize);
+
+        if active && pivot_row != k && lane < n {
+            let c = lane;
+            let a = mat.read(sm_idx_packed::<T>(slot, k, c));
+            let b = mat.read(sm_idx_packed::<T>(slot, pivot_row, c));
+            mat.write(sm_idx_packed::<T>(slot, k, c), b);
+            mat.write(sm_idx_packed::<T>(slot, pivot_row, c), a);
+        }
+        workgroup_memory_barrier_with_group_sync();
+
+        if active && lane == 0 {
+            let akk = mat.read(sm_idx_packed::<T>(slot, k, k));
+            inv_akk_shared.write(slot as usize, if akk != 0.0 { 1.0 / akk } else { 0.0 });
+        }
+        workgroup_memory_barrier_with_group_sync();
+        let inv_akk = inv_akk_shared.read(slot as usize);
+
+        if active {
+            let r = k + 1 + lane;
+            if r < n {
+                let v = mat.read(sm_idx_packed::<T>(slot, r, k)) * inv_akk;
+                mat.write(sm_idx_packed::<T>(slot, r, k), v);
+            }
+        }
+        workgroup_memory_barrier_with_group_sync();
+
+        if active {
+            let j = k + 1 + lane;
+            if j < n {
+                let akj = mat.read(sm_idx_packed::<T>(slot, k, j));
+                for r in (k + 1)..n {
+                    let lik = mat.read(sm_idx_packed::<T>(slot, r, k));
+                    let v = mat.read(sm_idx_packed::<T>(slot, r, j)) - lik * akj;
+                    mat.write(sm_idx_packed::<T>(slot, r, j), v);
+                }
+            }
+        }
+        workgroup_memory_barrier_with_group_sync();
+    }
+}
+
+/// Packed [`lu_triangular_solve_in_place`]: per-slot `x`/`partial` segments
+/// live at `slot·T`; the tree reduction runs `log2(T)` levels within each
+/// slot's segment, all slots in lock-step.
+#[inline]
+pub(super) fn lu_triangular_solve_in_place_packed<const T: u32, const MATN: usize>(
+    n: u32,
+    max_n: u32,
+    slot: u32,
+    lane: u32,
+    active_slot: bool,
+    mat: &[f32; MATN],
+    x: &mut [f32; 64],
+    partial: &mut [f32; 64],
+) {
+    let seg = (slot * T) as usize;
+    let log2_t = T.trailing_zeros();
+
+    for i in 0..max_n {
+        let active = active_slot && i < n;
+        let s = if active && lane < i {
+            mat.read(sm_idx_packed::<T>(slot, i, lane)) * x.read(seg + lane as usize)
+        } else {
+            0.0f32
+        };
+        partial.write(seg + lane as usize, s);
+        workgroup_memory_barrier_with_group_sync();
+        for step in 0..log2_t {
+            let stride = T >> (step + 1);
+            if lane < stride {
+                let v = partial.read(seg + lane as usize)
+                    + partial.read(seg + (lane + stride) as usize);
+                partial.write(seg + lane as usize, v);
+            }
+            workgroup_memory_barrier_with_group_sync();
+        }
+        if active && lane == 0 {
+            let cur = x.read(seg + i as usize);
+            x.write(seg + i as usize, cur - partial.read(seg));
+        }
+        workgroup_memory_barrier_with_group_sync();
+    }
+
+    for step in 0..max_n {
+        let active = active_slot && step < n;
+        // For dummy iterations (step >= n), `i` is not meaningful; guard
+        // every use of it behind `active`.
+        let i = if active { n - 1 - step } else { 0 };
+        let s = if active && lane > i && lane < n {
+            mat.read(sm_idx_packed::<T>(slot, i, lane)) * x.read(seg + lane as usize)
+        } else {
+            0.0f32
+        };
+        partial.write(seg + lane as usize, s);
+        workgroup_memory_barrier_with_group_sync();
+        for r in 0..log2_t {
+            let stride = T >> (r + 1);
+            if lane < stride {
+                let v = partial.read(seg + lane as usize)
+                    + partial.read(seg + (lane + stride) as usize);
+                partial.write(seg + lane as usize, v);
+            }
+            workgroup_memory_barrier_with_group_sync();
+        }
+        if active && lane == 0 {
+            let u = mat.read(sm_idx_packed::<T>(slot, i, i));
+            let cur = x.read(seg + i as usize) - partial.read(seg);
+            x.write(seg + i as usize, if u != 0.0 { cur / u } else { 0.0 });
+        }
+        workgroup_memory_barrier_with_group_sync();
+    }
+}
+
+/// Packed [`lu_apply_pivots`]: sequential on each slot's lane 0.
+#[inline]
+pub(super) fn lu_apply_pivots_packed<const T: u32>(
+    n: u32,
+    slot: u32,
+    lane: u32,
+    active_slot: bool,
+    buf_pivots: &[u32],
+    piv: VSlice,
+    x: &mut [f32; 64],
+) {
+    let seg = (slot * T) as usize;
+    if active_slot && lane == 0 {
+        for k in 0..n {
+            let p = buf_pivots.read(piv.at(k));
+            if p != k {
+                let a = x.read(seg + k as usize);
+                let b = x.read(seg + p as usize);
+                x.write(seg + k as usize, b);
+                x.write(seg + p as usize, a);
+            }
+        }
+    }
+    workgroup_memory_barrier_with_group_sync();
+}
+
 /// Apply the recorded pivots (sequential — lane 0 only). `n` is small so the
 /// extra parallelism wouldn't pay for the barrier.
 #[inline]
@@ -173,12 +361,12 @@ pub(super) fn lu_apply_pivots(
     n: u32,
     lane: u32,
     buf_pivots: &[u32],
-    pivots_offset: usize,
+    piv: VSlice,
     x: &mut [f32; MAX_MB_DOFS],
 ) {
     if lane == 0 {
         for k in 0..n {
-            let p = buf_pivots.read(pivots_offset + k as usize);
+            let p = buf_pivots.read(piv.at(k));
             if p != k {
                 let a = x.read(k as usize);
                 let b = x.read(p as usize);
diff --git a/src_rbd_shaders/dynamics/multibody/mod.rs b/src_rbd_shaders/dynamics/multibody/mod.rs
index 8b64d1a..45ed4e2 100644
--- a/src_rbd_shaders/dynamics/multibody/mod.rs
+++ b/src_rbd_shaders/dynamics/multibody/mod.rs
@@ -22,8 +22,10 @@ mod jacobian;
 mod joint_constraints;
 mod lu;
 mod mass_matrix;
+mod solve_constraints;
 mod types;
 mod utils;
+mod ws_soa;
 
 pub use compute_dynamics_pre::*;
 pub use contact_constraints::*;
@@ -31,5 +33,7 @@ pub use gravity_and_lu::*;
 pub use impulse_joint_constraints::*;
 pub use integrate::*;
 pub use joint_constraints::*;
+pub use solve_constraints::*;
 pub use types::*;
 pub use utils::*;
+pub use ws_soa::*;
diff --git a/src_rbd_shaders/dynamics/multibody/solve_constraints.rs b/src_rbd_shaders/dynamics/multibody/solve_constraints.rs
new file mode 100644
index 0000000..cdba815
--- /dev/null
+++ b/src_rbd_shaders/dynamics/multibody/solve_constraints.rs
@@ -0,0 +1,574 @@
+//! Fused multibody PGS sweep: joint limit/motor constraints followed by
+//! contact constraints, in one dispatch per substep phase.
+
+use khal_std::glamx::UVec3;
+use khal_std::index::MaybeIndexUnchecked;
+use khal_std::iter::StepRng;
+use khal_std::macros::{spirv, spirv_bindgen};
+use khal_std::sync::workgroup_memory_barrier_with_group_sync;
+
+use crate::dynamics::body::Velocity;
+use crate::gdot;
+use crate::utils::BatchIndices;
+use crate::utils::linalg::MAX_MB_DOFS;
+
+use super::types::{
+    MAX_MB_CONTACT_CONSTRAINTS_PER_MB, MB_CONTACT_KIND_TANGENT, MB_JOINT_KIND_COUPLING,
+    MB_JOINT_KIND_LIMIT, MB_JOINT_KIND_MOTOR, MultibodyContactConstraint, MultibodyInfo,
+    MultibodyJointConstraint,
+};
+
+const LANES: u32 = 64;
+
+/// One PGS sweep over a multibody's joint (limit/motor) constraints followed
+/// by its contact constraints.
+///
+/// Dispatch: one 64-lane workgroup per (multibody, batch).
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_mb_solve_constraints(
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    joint_constraints: &mut [MultibodyJointConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] joint_constraint_columns: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)]
+    contact_constraints: &mut [MultibodyContactConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] contact_constraint_jacs: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] contact_constraint_columns: &[f32],
+    #[spirv(uniform, descriptor_set = 0, binding = 6)] use_bias: &u32,
+    #[spirv(uniform, descriptor_set = 0, binding = 7)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 0)] dof_state: &mut [f32],
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 1)] solver_vels: &mut [Velocity],
+    #[spirv(workgroup)] dof_v: &mut [f32; MAX_MB_DOFS as usize],
+    #[spirv(workgroup)] scratch: &mut [f32; LANES as usize],
+    #[spirv(workgroup)] imp_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] delta_shared: &mut f32,
+) {
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
+    let num_mb = batch_ids.multibodies_len;
+    if mb_idx >= num_mb {
+        return;
+    }
+
+    let mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
+    let ndofs = mb.ndofs;
+    // Uniform per workgroup: every lane of this group returns together.
+    if ndofs == 0 {
+        return;
+    }
+    let use_bias = *use_bias != 0;
+
+    let v_base = mb.first_dof as usize;
+    let dofs_stride = batch_ids.dof_batch_capacity as usize;
+    let colliders_start = batch_ids.coll_start(batch_id);
+
+    let jcons_base =
+        batch_ids.mb_joint_constraints_start(batch_id) + mb.first_constraint as usize;
+    let jcol_base = batch_ids.mb_joint_constraint_columns_start(batch_id)
+        + (mb.first_constraint as usize) * dofs_stride;
+
+    let ccons_base = batch_ids.mb_contact_constraints_start(batch_id)
+        + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize);
+    let ccol_base = batch_ids.mb_contact_constraint_columns_start(batch_id)
+        + (mb_idx as usize) * (MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize) * dofs_stride;
+
+    let contact_count = mb.contact_constraint_count;
+    if mb.max_constraints == 0 && contact_count == 0 {
+        // Nothing to solve.
+        return;
+    }
+
+    // Load the generalized velocities and accumulated contact impulses into
+    // workgroup memory.
+    if lane < ndofs {
+        dof_v[lane as usize] = dof_state.read(batch_ids.mbi(batch_id, v_base + lane as usize));
+    }
+    for s in StepRng::new(lane..contact_count, LANES) {
+        imp_shared[s as usize] = contact_constraints.read(ccons_base + s as usize).impulse;
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+
+    // Joint limits/motors
+    for s in 0..mb.max_constraints {
+        let cons = joint_constraints.read(jcons_base + s as usize);
+        if cons.kind != MB_JOINT_KIND_LIMIT
+            && cons.kind != MB_JOINT_KIND_MOTOR
+            && cons.kind != MB_JOINT_KIND_COUPLING
+        {
+            // Unused slot or inactive limit.
+            continue;
+        }
+
+        let rhs = if use_bias { cons.rhs } else { cons.rhs_wo_bias };
+        // Generalized `J·v` for `J = e_{dof_id} - coupling_coeff*e_{dof2_id}`
+        // (coupling rows); collapses to `v[dof_id]` for limit / motor rows
+        // (their `coupling_coeff` is 0).
+        let v_d = dof_v[cons.dof_id as usize]
+            - cons.coupling_coeff * dof_v[cons.dof2_id as usize];
+        let rhs_total = v_d + rhs;
+        let raw_imp = cons.impulse + cons.inv_lhs * (rhs_total - cons.cfm_gain * cons.impulse);
+        let mut new_imp = raw_imp;
+        if new_imp < cons.impulse_lo {
+            new_imp = cons.impulse_lo;
+        }
+        if new_imp > cons.impulse_hi {
+            new_imp = cons.impulse_hi;
+        }
+        let delta = new_imp - cons.impulse;
+
+        if lane == 0 {
+            let mut cons = cons;
+            cons.impulse = new_imp;
+            joint_constraints.write(jcons_base + s as usize, cons);
+        }
+
+        // All lanes read `dof_v[dof_id]` above; sync before overwriting it.
+        workgroup_memory_barrier_with_group_sync();
+        if lane < ndofs {
+            let col = joint_constraint_columns
+                .read(jcol_base + (s as usize) * dofs_stride + lane as usize);
+            dof_v[lane as usize] -= delta * col;
+        }
+        workgroup_memory_barrier_with_group_sync();
+    }
+
+
+    // Contacts.
+    for s in 0..contact_count {
+        let cons = contact_constraints.read(ccons_base + s as usize);
+        // Friction is only solved during the relaxation phase.
+        if use_bias && cons.kind == MB_CONTACT_KIND_TANGENT {
+            continue;
+        }
+        let col_offset = ccol_base + (s as usize) * dofs_stride;
+        let is_self = cons.free_body_id == u32::MAX;
+
+        // Multibody side of J · u, one product per lane; lane 0 sums them in
+        // DOF order.
+        scratch[lane as usize] = if lane < ndofs {
+            contact_constraint_jacs.read(col_offset + lane as usize) * dof_v[lane as usize]
+        } else {
+            0.0
+        };
+        workgroup_memory_barrier_with_group_sync();
+
+        if lane == 0 {
+            let mut j_dot_v = 0.0f32;
+            for i in 0..ndofs {
+                j_dot_v += scratch[i as usize];
+            }
+            // Free-body side stays lane-0-local.
+            let free = if is_self {
+                Velocity::default()
+            } else {
+                solver_vels.read(colliders_start + cons.free_body_id as usize)
+            };
+            if !is_self {
+                j_dot_v += cons.lin_jac.dot(free.linear) + gdot(cons.ang_jac, free.angular);
+            }
+
+            let rhs = if use_bias { cons.rhs } else { cons.rhs_wo_bias };
+            let impulse = imp_shared[s as usize];
+            let rhs_total = j_dot_v + rhs;
+            let cfm_factor = if use_bias { cons.cfm_factor } else { 1.0 };
+            let raw_imp = cfm_factor * (impulse - cons.inv_lhs * rhs_total);
+
+            // Normal: clamp to ≥ 0. Friction tangent: clamp to
+            // `±μ · normal_impulse` (box friction), reading the paired normal
+            // slot's current impulse from shared memory.
+            let new_imp = if cons.kind == MB_CONTACT_KIND_TANGENT {
+                let limit =
+                    cons.friction_coeff * imp_shared[cons.normal_constraint_slot as usize];
+                if raw_imp > limit {
+                    limit
+                } else if raw_imp < -limit {
+                    -limit
+                } else {
+                    raw_imp
+                }
+            } else if raw_imp < 0.0 {
+                0.0
+            } else {
+                raw_imp
+            };
+            let delta = new_imp - impulse;
+            imp_shared[s as usize] = new_imp;
+            *delta_shared = delta;
+
+            if delta != 0.0 && !is_self {
+                let mut new_free = free;
+                new_free.linear += cons.lin_jac * (cons.free_body_im * delta);
+                new_free.angular += cons.ii_ang_jac * delta;
+                solver_vels.write(colliders_start + cons.free_body_id as usize, new_free);
+            }
+        }
+        workgroup_memory_barrier_with_group_sync();
+
+        // Per-lane `dof_v[lane]` update.
+        let delta = *delta_shared;
+        if delta != 0.0 && lane < ndofs {
+            let col = contact_constraint_columns.read(col_offset + lane as usize);
+            dof_v[lane as usize] += delta * col;
+        }
+    }
+
+    // Writeback
+    if lane < ndofs {
+        dof_state.write(
+            batch_ids.mbi(batch_id, v_base + lane as usize),
+            dof_v[lane as usize],
+        );
+    }
+    for s in StepRng::new(lane..contact_count, LANES) {
+        let mut cons = contact_constraints.read(ccons_base + s as usize);
+        cons.impulse = imp_shared[s as usize];
+        contact_constraints.write(ccons_base + s as usize, cons);
+    }
+}
+
+/// Joint-only PGS sweep (the joint half of [`gpu_mb_solve_constraints`]),
+/// used by the Delassus path where the contact half runs in constraint space
+/// as a separate dispatch (to avoid exceeding the 8-storage-buffer budget).
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_mb_solve_joints(
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    joint_constraints: &mut [MultibodyJointConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] joint_constraint_columns: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] dof_state: &mut [f32],
+    #[spirv(uniform, descriptor_set = 0, binding = 4)] use_bias: &u32,
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
+    #[spirv(workgroup)] dof_v: &mut [f32; MAX_MB_DOFS as usize],
+) {
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
+    let num_mb = batch_ids.multibodies_len;
+    if mb_idx >= num_mb {
+        return;
+    }
+
+    let mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
+    let ndofs = mb.ndofs;
+    // Uniform per workgroup: every lane of this group returns together.
+    if ndofs == 0 || mb.max_constraints == 0 {
+        return;
+    }
+    let use_bias = *use_bias != 0;
+
+    let v_base = mb.first_dof as usize;
+    let dofs_stride = batch_ids.dof_batch_capacity as usize;
+    let jcons_base =
+        batch_ids.mb_joint_constraints_start(batch_id) + mb.first_constraint as usize;
+    let jcol_base = batch_ids.mb_joint_constraint_columns_start(batch_id)
+        + (mb.first_constraint as usize) * dofs_stride;
+
+    if lane < ndofs {
+        dof_v[lane as usize] = dof_state.read(batch_ids.mbi(batch_id, v_base + lane as usize));
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    for s in 0..mb.max_constraints {
+        let cons = joint_constraints.read(jcons_base + s as usize);
+        if cons.kind != MB_JOINT_KIND_LIMIT
+            && cons.kind != MB_JOINT_KIND_MOTOR
+            && cons.kind != MB_JOINT_KIND_COUPLING
+        {
+            // Unused slot or inactive limit.
+            continue;
+        }
+
+        let rhs = if use_bias { cons.rhs } else { cons.rhs_wo_bias };
+        // Generalized `J·v` for `J = e_{dof_id} - coupling_coeff*e_{dof2_id}`
+        // (coupling rows); collapses to `v[dof_id]` for limit / motor rows
+        // (their `coupling_coeff` is 0).
+        let v_d = dof_v[cons.dof_id as usize]
+            - cons.coupling_coeff * dof_v[cons.dof2_id as usize];
+        let rhs_total = v_d + rhs;
+        let raw_imp = cons.impulse + cons.inv_lhs * (rhs_total - cons.cfm_gain * cons.impulse);
+        let mut new_imp = raw_imp;
+        if new_imp < cons.impulse_lo {
+            new_imp = cons.impulse_lo;
+        }
+        if new_imp > cons.impulse_hi {
+            new_imp = cons.impulse_hi;
+        }
+        let delta = new_imp - cons.impulse;
+
+        if lane == 0 {
+            let mut cons = cons;
+            cons.impulse = new_imp;
+            joint_constraints.write(jcons_base + s as usize, cons);
+        }
+
+        workgroup_memory_barrier_with_group_sync();
+        if lane < ndofs {
+            let col = joint_constraint_columns
+                .read(jcol_base + (s as usize) * dofs_stride + lane as usize);
+            dof_v[lane as usize] -= delta * col;
+        }
+        workgroup_memory_barrier_with_group_sync();
+    }
+
+    if lane < ndofs {
+        dof_state.write(
+            batch_ids.mbi(batch_id, v_base + lane as usize),
+            dof_v[lane as usize],
+        );
+    }
+}
+
+/// Fills the per-multibody Delassus block `D[s][j] = ∂a[j]/∂impulse[s]` (row
+/// `s` = the effect of constraint `s`, laid out row-contiguously so the solve
+/// kernel's per-iteration row update reads coalesced). Runs right after
+/// `gpu_mb_finalize_contact_constraints` (it consumes the M⁻¹Jᵀ columns).
+///
+/// One 64-lane workgroup per (multibody, batch).
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_mb_build_contact_delassus(
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    contact_constraints: &[MultibodyContactConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] contact_constraint_jacs: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] contact_constraint_columns: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] delassus: &mut [f32],
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
+) {
+    const MAXC: u32 = MAX_MB_CONTACT_CONSTRAINTS_PER_MB;
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
+    let num_mb = batch_ids.multibodies_len;
+    if mb_idx >= num_mb {
+        return;
+    }
+
+    let mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
+    let ndofs = mb.ndofs;
+    let count = mb.contact_constraint_count;
+    if ndofs == 0 || count == 0 {
+        return;
+    }
+
+    let cons_base = batch_ids.mb_contact_constraints_start(batch_id)
+        + (mb_idx as usize) * (MAXC as usize);
+    let dofs_stride = batch_ids.dof_batch_capacity as usize;
+    let col_base = batch_ids.mb_contact_constraint_columns_start(batch_id)
+        + (mb_idx as usize) * (MAXC as usize) * dofs_stride;
+    let d_base = ((batch_id * batch_ids.multibodies_batch_capacity + mb_idx) as usize)
+        * (MAXC as usize)
+        * (MAXC as usize);
+
+    // Pair `p = s · count + j`: consecutive lanes share the source row `s`
+    // and vary the target `j`, so the column reads of `s` broadcast and the
+    // `D` writes coalesce.
+    let num_pairs = count * count;
+    for p in StepRng::new(lane..num_pairs, LANES) {
+        let s = p / count;
+        let j = p % count;
+
+        // Multibody coupling: jac_j · (M⁻¹ jac_sᵀ).
+        let jac_j_off = col_base + (j as usize) * dofs_stride;
+        let col_s_off = col_base + (s as usize) * dofs_stride;
+        let mut v = 0.0f32;
+        for i in 0..ndofs {
+            let jj = contact_constraint_jacs.read(jac_j_off + i as usize);
+            let cs = contact_constraint_columns.read(col_s_off + i as usize);
+            v += jj * cs;
+        }
+
+        // Free-body coupling (impulse at `s` moves the shared free body,
+        // which feeds `a[j]`'s free-side term). Zero for self-contacts and
+        // static free bodies.
+        let cons_s = contact_constraints.read(cons_base + s as usize);
+        let cons_j = contact_constraints.read(cons_base + j as usize);
+        if cons_s.free_body_id != u32::MAX && cons_s.free_body_id == cons_j.free_body_id {
+            v += cons_s.free_body_im * cons_j.lin_jac.dot(cons_s.lin_jac)
+                + gdot(cons_j.ang_jac, cons_s.ii_ang_jac);
+        }
+
+        delassus.write(d_base + (s * MAXC + j) as usize, v);
+    }
+}
+
+/// Constraint-space contact sweep: tracks `a[s] = J_s · u` incrementally in
+/// workgroup memory using the precomputed Delassus rows, so each PGS
+/// iteration is a couple of shared-memory scalars plus one lane-parallel row
+/// update.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_mb_solve_contacts_delassus(
+    #[spirv(workgroup_id)] workgroup_id: UVec3,
+    #[spirv(local_invocation_id)] local_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] multibody_info: &[MultibodyInfo],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    contact_constraints: &mut [MultibodyContactConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] contact_constraint_jacs: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] contact_constraint_columns: &[f32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] delassus: &[f32],
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] use_bias: &u32,
+    #[spirv(uniform, descriptor_set = 0, binding = 6)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 0)] dof_state: &mut [f32],
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 1)] solver_vels: &mut [Velocity],
+    #[spirv(workgroup)] dof_v: &mut [f32; MAX_MB_DOFS as usize],
+    #[spirv(workgroup)] a_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] imp_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] rhs_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] inv_lhs_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] cfm_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] friction_shared: &mut [f32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+    #[spirv(workgroup)] meta_shared: &mut [u32; MAX_MB_CONTACT_CONSTRAINTS_PER_MB as usize],
+) {
+    const MAXC: u32 = MAX_MB_CONTACT_CONSTRAINTS_PER_MB;
+    let batch_id = workgroup_id.y;
+    let mb_idx = workgroup_id.x;
+    let lane = local_id.x;
+    let num_mb = batch_ids.multibodies_len;
+    if mb_idx >= num_mb {
+        return;
+    }
+
+    let mb = multibody_info.read(batch_ids.mbi(batch_id, mb_idx as usize));
+    let ndofs = mb.ndofs;
+    let count = mb.contact_constraint_count;
+    // Uniform per workgroup: every lane of this group returns together.
+    if ndofs == 0 || count == 0 {
+        return;
+    }
+    let use_bias = *use_bias != 0;
+
+    let v_base = mb.first_dof as usize;
+    let colliders_start = batch_ids.coll_start(batch_id);
+    let cons_base = batch_ids.mb_contact_constraints_start(batch_id)
+        + (mb_idx as usize) * (MAXC as usize);
+    let dofs_stride = batch_ids.dof_batch_capacity as usize;
+    let col_base = batch_ids.mb_contact_constraint_columns_start(batch_id)
+        + (mb_idx as usize) * (MAXC as usize) * dofs_stride;
+    let d_base = ((batch_id * batch_ids.multibodies_batch_capacity + mb_idx) as usize)
+        * (MAXC as usize)
+        * (MAXC as usize);
+
+    if lane < ndofs {
+        dof_v[lane as usize] = dof_state.read(batch_ids.mbi(batch_id, v_base + lane as usize));
+    }
+
+    // Preload the per-constraint solve scalars into shared SoA arrays so the
+    // serial recurrence below never touches storage on its critical path.
+    // `meta` packs the kind, the paired normal slot, and whether the
+    // free-body side needs the fire-and-forget storage velocity update.
+    for s in StepRng::new(lane..count, LANES) {
+        let cons = contact_constraints.read(cons_base + s as usize);
+        imp_shared[s as usize] = cons.impulse;
+        rhs_shared[s as usize] = if use_bias { cons.rhs } else { cons.rhs_wo_bias };
+        inv_lhs_shared[s as usize] = cons.inv_lhs;
+        cfm_shared[s as usize] = if use_bias { cons.cfm_factor } else { 1.0 };
+        friction_shared[s as usize] = cons.friction_coeff;
+        let is_self = cons.free_body_id == u32::MAX;
+        let free_active = !is_self
+            && (cons.free_body_im != 0.0 || gdot(cons.ii_ang_jac, cons.ii_ang_jac) != 0.0);
+        meta_shared[s as usize] = (cons.kind & 0xff)
+            | ((cons.normal_constraint_slot & 0xffff) << 8)
+            | (if free_active { 1 << 24 } else { 0 });
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    // Fresh `a[s] = J_s · u` under the current (post-joint-sweep, post-
+    // warmstart) velocities.
+    for s in StepRng::new(lane..count, LANES) {
+        let jac_off = col_base + (s as usize) * dofs_stride;
+        let mut dot = 0.0f32;
+        for i in 0..ndofs {
+            dot += contact_constraint_jacs.read(jac_off + i as usize) * dof_v[i as usize];
+        }
+        let cons = contact_constraints.read(cons_base + s as usize);
+        if cons.free_body_id != u32::MAX {
+            let free = solver_vels.read(colliders_start + cons.free_body_id as usize);
+            dot += cons.lin_jac.dot(free.linear) + gdot(cons.ang_jac, free.angular);
+        }
+        a_shared[s as usize] = dot;
+    }
+    workgroup_memory_barrier_with_group_sync();
+
+    for s in 0..count {
+        let meta = meta_shared[s as usize];
+        let kind = meta & 0xff;
+        let normal_slot = (meta >> 8) & 0xffff;
+        let free_active = (meta >> 24) != 0;
+
+        if use_bias && kind == MB_CONTACT_KIND_TANGENT {
+            // Friction is only solved during the stabilization sweep.
+            continue;
+        }
+
+        let impulse = imp_shared[s as usize];
+        let rhs_total = a_shared[s as usize] + rhs_shared[s as usize];
+        let raw_imp = cfm_shared[s as usize] * (impulse - inv_lhs_shared[s as usize] * rhs_total);
+
+        let new_imp = if kind == MB_CONTACT_KIND_TANGENT {
+            let limit = friction_shared[s as usize] * imp_shared[normal_slot as usize];
+            if raw_imp > limit {
+                limit
+            } else if raw_imp < -limit {
+                -limit
+            } else {
+                raw_imp
+            }
+        } else if raw_imp < 0.0 {
+            0.0
+        } else {
+            raw_imp
+        };
+        let delta = new_imp - impulse;
+
+        if delta != 0.0 {
+            if lane == 0 {
+                imp_shared[s as usize] = new_imp;
+
+                if free_active {
+                    let cons = contact_constraints.read(cons_base + s as usize);
+                    let mut free =
+                        solver_vels.read(colliders_start + cons.free_body_id as usize);
+                    free.linear += cons.lin_jac * (cons.free_body_im * delta);
+                    free.angular += cons.ii_ang_jac * delta;
+                    solver_vels.write(colliders_start + cons.free_body_id as usize, free);
+                }
+            }
+            // Lane-parallel Delassus row update (row `s` is contiguous), plus
+            // the off-path dof update (each lane owns its DOF).
+            let d_row = d_base + (s * MAXC) as usize;
+            for j in StepRng::new(lane..count, LANES) {
+                a_shared[j as usize] += delta * delassus.read(d_row + j as usize);
+            }
+            if lane < ndofs {
+                let col = contact_constraint_columns
+                    .read(col_base + (s as usize) * dofs_stride + lane as usize);
+                dof_v[lane as usize] += delta * col;
+            }
+            workgroup_memory_barrier_with_group_sync();
+        }
+    }
+
+    // Writeback.
+    if lane < ndofs {
+        dof_state.write(
+            batch_ids.mbi(batch_id, v_base + lane as usize),
+            dof_v[lane as usize],
+        );
+    }
+    for s in StepRng::new(lane..count, LANES) {
+        let mut cons = contact_constraints.read(cons_base + s as usize);
+        cons.impulse = imp_shared[s as usize];
+        contact_constraints.write(cons_base + s as usize, cons);
+    }
+}
diff --git a/src_rbd_shaders/dynamics/multibody/types.rs b/src_rbd_shaders/dynamics/multibody/types.rs
index f06e2ee..b5c9974 100644
--- a/src_rbd_shaders/dynamics/multibody/types.rs
+++ b/src_rbd_shaders/dynamics/multibody/types.rs
@@ -47,6 +47,23 @@ pub const MB_CONTACT_KIND_NORMAL: u32 = 1;
 /// `normal_constraint_slot` (relative to the multibody's `cons_base`).
 pub const MB_CONTACT_KIND_TANGENT: u32 = 2;
 
+/// Joint-constraint `kind`: unused slot.
+pub const MB_JOINT_KIND_INACTIVE: u32 = 0;
+/// Joint-constraint `kind`: active limit.
+pub const MB_JOINT_KIND_LIMIT: u32 = 1;
+/// Joint-constraint `kind`: active motor.
+pub const MB_JOINT_KIND_MOTOR: u32 = 2;
+/// Joint-constraint `kind`: limit slot that is inactive this substep (its
+/// joint coordinate is inside the limits). The solve skips it; the slot
+/// still gets a valid M⁻¹ column / `inv_lhs` / `cfm_gain` in case it needs
+/// to be enabled at a next substep.
+pub const MB_JOINT_KIND_LIMIT_INACTIVE: u32 = 3;
+/// Joint-constraint `kind`: holonomic DoF coupling `q2 = coeff·q1 + offset`
+/// (rapier's `MultibodyDofCoupling`): one bilateral constraint with jacobian
+/// `J = e_{dof_id} − coeff·e_{dof2_id}` and a bias pulling the position drift
+/// back to zero.
+pub const MB_JOINT_KIND_COUPLING: u32 = 4;
+
 /// Sentinel marking a link with no parent (the root).
 pub const MULTIBODY_ROOT: u32 = u32::MAX;
 
@@ -164,11 +181,17 @@ pub struct MultibodyJointConstraint {
     // TODO: rename to MultibodyUnitJointConstraint?
     /// Index of the constrained DOF, relative to the multibody's `first_dof`.
     pub dof_id: u32,
-    /// 0 = inactive (skipped by the solver), 1 = limit, 2 = motor.
+    /// See `MB_JOINT_KIND_*`: 0 = inactive (skipped by the solver), 1 = limit,
+    /// 2 = motor, 3 = inactive limit, 4 = DoF coupling.
     pub kind: u32,
-    /// Constraint kind extras (for future extensions). Currently, always 0.
+    /// Packed `(link_id | axis << 16)` of the constrained DOF, used by the
+    /// per-substep refresh to re-read the joint coordinate.
     pub _kind_extra: u32,
-    pub _pad0: u32,
+    /// Second constrained DOF for coupling rows (`J = e_{dof_id} −
+    /// coupling_coeff·e_{dof2_id}`). Zero (together with `coupling_coeff = 0`)
+    /// for limits / motors, making the solve's generalized `J·v` collapse to
+    /// the plain single-DOF form.
+    pub dof2_id: u32,
 
     /// `J·v` reference + bias velocity (rapier's `rhs`, includes positional bias).
     pub rhs: f32,
@@ -188,6 +211,16 @@ pub struct MultibodyJointConstraint {
     /// Matches rapier's `cfm_coeff` / `cfm_gain` fields.
     pub cfm_coeff: f32,
     pub cfm_gain: f32,
+
+    /// Coupling coefficient (`q2 − coeff·q1 − offset = 0`); zero for
+    /// limit / motor rows.
+    pub coupling_coeff: f32,
+    /// Coupling constant offset; zero for limit / motor rows.
+    pub coupling_offset: f32,
+    /// Packed `(link_id | axis << 16)` of the coupling's first joint
+    /// coordinate (`q1`), for the per-substep drift refresh.
+    pub _kind_extra2: u32,
+    pub _pad1: u32,
 }
 
 /// One normal-direction contact constraint between a free rigid body and a
@@ -351,4 +384,34 @@ pub struct MultibodyInfo {
     /// multibody. Written by `gpu_mb_init_contact_constraints`, read by the
     /// warmstart / finalize / solve / remove-bias contact kernels.
     pub contact_constraint_count: u32,
+    /// Per-step copy of `contacts_len[batch]` (to work around the web 8 storage
+    /// bindings count limit).
+    pub batch_contacts_len: u32,
+    /// First entry of this multibody's DoF couplings in the `dof_couplings`
+    /// buffer (relative to the batch's coupling slice).
+    pub first_coupling: u32,
+    /// Number of DoF couplings on this multibody.
+    pub num_couplings: u32,
+}
+
+/// One holonomic coupling `q2 = coeff·q1 + offset` between two generalized
+/// coordinates of the same multibody (rapier's `MultibodyDofCoupling`),
+/// converted to the GPU's re-numbered assembly ids at build time.
+#[derive(Clone, Copy, Default)]
+#[cfg_attr(not(target_arch_is_gpu), derive(bytemuck::Pod, bytemuck::Zeroable))]
+#[repr(C)]
+pub struct MbDofCoupling {
+    /// Absolute DOF index of `q1` (relative to the multibody's `first_dof`).
+    pub dof1: u32,
+    /// Absolute DOF index of `q2`.
+    pub dof2: u32,
+    /// Packed `(link_id | axis << 16)` locating `q1`'s joint coordinate.
+    pub link_axis1: u32,
+    /// Packed `(link_id | axis << 16)` locating `q2`'s joint coordinate.
+    pub link_axis2: u32,
+    /// Linear coupling coefficient.
+    pub coeff: f32,
+    /// Constant offset.
+    pub offset: f32,
+    pub _pad: [u32; 2],
 }
diff --git a/src_rbd_shaders/dynamics/multibody/ws_soa.rs b/src_rbd_shaders/dynamics/multibody/ws_soa.rs
new file mode 100644
index 0000000..1449ed4
--- /dev/null
+++ b/src_rbd_shaders/dynamics/multibody/ws_soa.rs
@@ -0,0 +1,373 @@
+//! SoA layout for the per-link multibody workspace.
+
+use glamx::Vec4;
+use khal_std::index::MaybeIndexUnchecked;
+
+#[cfg(feature = "dim2")]
+use glamx::Vec2;
+#[cfg(feature = "dim3")]
+use glamx::Vec3;
+
+use super::types::MultibodyLinkWorkspace;
+use crate::dynamics::body::Velocity;
+use crate::{Pose, Rotation, Vector};
+
+/*
+ * Per-link Vec4 offsets of each field.
+ */
+#[cfg(feature = "dim3")]
+mod layout {
+    /// Joint rotation quat (xyzw).
+    pub const WS_JOINT_ROT: u32 = 0;
+    /// Generalized coordinates c0..c3 | c4, c5, pad, pad.
+    pub const WS_COORDS: u32 = 1;
+    /// Local-to-parent: rot quat | trans xyz, pad.
+    pub const WS_LTP: u32 = 3;
+    /// Local-to-world: rot quat | trans xyz, pad.
+    pub const WS_LTW: u32 = 5;
+    /// shift02 xyz, pad.
+    pub const WS_SHIFT02: u32 = 7;
+    /// shift23 xyz, pad.
+    pub const WS_SHIFT23: u32 = 8;
+    /// Joint velocity: lin xyz, pad | ang xyz, pad.
+    pub const WS_JOINT_VEL: u32 = 9;
+    /// Rigid-body velocity: lin | ang.
+    pub const WS_RB_VELS: u32 = 11;
+    /// Kinematic acceleration: lin | ang.
+    pub const WS_KIN_ACC: u32 = 13;
+    /// Total quads per link (per-link stride, in quad units).
+    pub const WS_QUADS: u32 = 15;
+}
+
+/*
+ * Per-link QUAD offsets of each field (dim2): 9 quads / 144 B per link.
+ */
+#[cfg(feature = "dim2")]
+mod layout {
+    /// Joint rotation (re, im, pad, pad).
+    pub const WS_JOINT_ROT: u32 = 0;
+    /// Generalized coordinates c0..c2, pad.
+    pub const WS_COORDS: u32 = 1;
+    /// Local-to-parent: (rot.re, rot.im, trans.x, trans.y), one quad.
+    pub const WS_LTP: u32 = 2;
+    /// Local-to-world: (rot.re, rot.im, trans.x, trans.y), one quad.
+    pub const WS_LTW: u32 = 3;
+    /// shift02 x, y, pad, pad.
+    pub const WS_SHIFT02: u32 = 4;
+    /// shift23 x, y, pad, pad.
+    pub const WS_SHIFT23: u32 = 5;
+    /// Joint velocity: (lin.x, lin.y, ang, pad).
+    pub const WS_JOINT_VEL: u32 = 6;
+    /// Rigid-body velocity: (lin.x, lin.y, ang, pad).
+    pub const WS_RB_VELS: u32 = 7;
+    /// Kinematic acceleration: (lin.x, lin.y, ang, pad).
+    pub const WS_KIN_ACC: u32 = 8;
+    /// Total quads per link (per-link stride, in quad units).
+    pub const WS_QUADS: u32 = 9;
+}
+
+pub use layout::*;
+
+/// Addressing view over the SoA workspace buffer for one multibody of one
+/// batch: `base` is the multibody's first link (intra-batch), `stride` /
+/// `shift` the batch interleave (`num_batches` / `batch_id`).
+#[derive(Copy, Clone)]
+pub struct WsAddr {
+    pub base: usize,
+    pub stride: u32,
+    pub shift: u32,
+}
+
+impl WsAddr {
+    /// View of batch `shift`'s workspace, based at link `base`.
+    #[inline]
+    pub fn new(base: usize, stride: u32, shift: u32) -> Self {
+        Self {
+            base,
+            stride,
+            shift,
+        }
+    }
+
+    /// Re-based view (like `Slice::offset`).
+    #[inline]
+    pub fn offset(self, links: usize) -> Self {
+        Self {
+            base: self.base + links,
+            ..self
+        }
+    }
+
+    /// Flat index of quad `quad` (a `WS_*` field offset + quad index) of
+    /// link `k` (relative to `base`).
+    #[inline]
+    pub fn at(&self, k: u32, quad: u32) -> usize {
+        ((self.base + k as usize) * WS_QUADS as usize + quad as usize) * self.stride as usize
+            + self.shift as usize
+    }
+}
+
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_rot(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Rotation {
+    let q = buf.read(a.at(k, f));
+    Rotation::from_xyzw(q.x, q.y, q.z, q.w)
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_rot(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Rotation {
+    let q = buf.read(a.at(k, f));
+    Rotation::from_cos_sin_unchecked(q.x, q.y)
+}
+
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_set_rot(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, r: Rotation) {
+    buf.write(a.at(k, f), Vec4::new(r.x, r.y, r.z, r.w));
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_set_rot(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, r: Rotation) {
+    buf.write(a.at(k, f), Vec4::new(r.re, r.im, 0.0, 0.0));
+}
+
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_vec(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Vector {
+    let q = buf.read(a.at(k, f));
+    Vec3::new(q.x, q.y, q.z)
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_vec(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Vector {
+    let q = buf.read(a.at(k, f));
+    Vec2::new(q.x, q.y)
+}
+
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_set_vec(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, v: Vector) {
+    buf.write(a.at(k, f), Vec4::new(v.x, v.y, v.z, 0.0));
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_set_vec(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, v: Vector) {
+    buf.write(a.at(k, f), Vec4::new(v.x, v.y, 0.0, 0.0));
+}
+
+/// Pose accessors. 3D: rotation quad + translation quad. 2D: one quad
+/// `(rot.re, rot.im, trans.x, trans.y)`.
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_pose(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Pose {
+    let rot = ws_rot(buf, a, k, f);
+    let tr = ws_vec(buf, a, k, f + 1);
+    Pose::from_parts(tr, rot)
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_pose(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Pose {
+    let q = buf.read(a.at(k, f));
+    Pose::from_parts(
+        Vec2::new(q.z, q.w),
+        Rotation::from_cos_sin_unchecked(q.x, q.y),
+    )
+}
+
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_set_pose(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, p: Pose) {
+    ws_set_rot(buf, a, k, f, p.rotation);
+    ws_set_vec(buf, a, k, f + 1, p.translation);
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_set_pose(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, p: Pose) {
+    buf.write(
+        a.at(k, f),
+        Vec4::new(
+            p.rotation.re,
+            p.rotation.im,
+            p.translation.x,
+            p.translation.y,
+        ),
+    );
+}
+
+/// Velocity accessors. 3D: linear quad + angular quad. 2D: one quad
+/// `(lin.x, lin.y, ang, pad)`.
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_vel(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Velocity {
+    Velocity::new(ws_vec(buf, a, k, f), ws_vec(buf, a, k, f + 1))
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_vel(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> Velocity {
+    let q = buf.read(a.at(k, f));
+    Velocity::new(Vec2::new(q.x, q.y), q.z)
+}
+
+/// Angular part of a velocity field (3D loads only the angular quad instead
+/// of the whole velocity).
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_vel_ang(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> crate::AngVector {
+    ws_vec(buf, a, k, f + 1)
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_vel_ang(buf: &[Vec4], a: WsAddr, k: u32, f: u32) -> crate::AngVector {
+    buf.read(a.at(k, f)).z
+}
+
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_set_vel(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, v: Velocity) {
+    ws_set_vec(buf, a, k, f, v.linear);
+    ws_set_vec(buf, a, k, f + 1, v.angular);
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_set_vel(buf: &mut [Vec4], a: WsAddr, k: u32, f: u32, v: Velocity) {
+    buf.write(a.at(k, f), Vec4::new(v.linear.x, v.linear.y, v.angular, 0.0));
+}
+
+/// Extract component `i` (0..4) of a `Vec4` by value (no reference indexing,
+/// which would create SPIR-V pointer phis).
+#[inline]
+fn vec4_get(v: Vec4, i: u32) -> f32 {
+    if i == 0 {
+        v.x
+    } else if i == 1 {
+        v.y
+    } else if i == 2 {
+        v.z
+    } else {
+        v.w
+    }
+}
+
+#[inline]
+fn vec4_set(v: Vec4, i: u32, val: f32) -> Vec4 {
+    let mut out = v;
+    if i == 0 {
+        out.x = val;
+    } else if i == 1 {
+        out.y = val;
+    } else if i == 2 {
+        out.z = val;
+    } else {
+        out.w = val;
+    }
+    out
+}
+
+/// Single generalized-coordinate accessors (`i < MAX_JOINT_DOFS`).
+#[inline]
+pub fn ws_coord(buf: &[Vec4], a: WsAddr, k: u32, i: u32) -> f32 {
+    vec4_get(buf.read(a.at(k, WS_COORDS + i / 4)), i % 4)
+}
+
+/// Read-modify-write of one coordinate's quad.
+#[inline]
+pub fn ws_set_coord(buf: &mut [Vec4], a: WsAddr, k: u32, i: u32, v: f32) {
+    let idx = a.at(k, WS_COORDS + i / 4);
+    let q = buf.read(idx);
+    buf.write(idx, vec4_set(q, i % 4, v));
+}
+
+/// Load the whole coords array (for `body_to_parent`).
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_coords(buf: &[Vec4], a: WsAddr, k: u32) -> [f32; 6] {
+    let q0 = buf.read(a.at(k, WS_COORDS));
+    let q1 = buf.read(a.at(k, WS_COORDS + 1));
+    [q0.x, q0.y, q0.z, q0.w, q1.x, q1.y]
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_coords(buf: &[Vec4], a: WsAddr, k: u32) -> [f32; 3] {
+    let q0 = buf.read(a.at(k, WS_COORDS));
+    [q0.x, q0.y, q0.z]
+}
+
+/// World-space inertia of link `k`, the SoA counterpart of the former
+/// `MultibodyLinkWorkspace::link_world_inertia` (reads only the
+/// local-to-world rotation).
+#[cfg(feature = "dim3")]
+#[inline]
+pub fn ws_world_inertia(
+    buf: &[Vec4],
+    a: WsAddr,
+    k: u32,
+    lmp: &crate::dynamics::body::LocalMassProperties,
+) -> glamx::Mat3 {
+    use crate::rotation_to_matrix;
+    let ipi = lmp.inv_principal_inertia;
+    let px = if ipi.x != 0.0 { 1.0 / ipi.x } else { 0.0 };
+    let py = if ipi.y != 0.0 { 1.0 / ipi.y } else { 0.0 };
+    let pz = if ipi.z != 0.0 { 1.0 / ipi.z } else { 0.0 };
+    let rot = ws_rot(buf, a, k, WS_LTW);
+    let r = rotation_to_matrix(rot * lmp.inertia_ref_frame);
+    // M = r · diag(px, py, pz) (column-scale); I = M · rᵀ.
+    let m = glamx::Mat3::from_cols(r.x_axis * px, r.y_axis * py, r.z_axis * pz);
+    m * r.transpose()
+}
+
+#[cfg(feature = "dim2")]
+#[inline]
+pub fn ws_world_inertia(
+    _buf: &[Vec4],
+    _a: WsAddr,
+    _k: u32,
+    lmp: &crate::dynamics::body::LocalMassProperties,
+) -> f32 {
+    if lmp.inv_inertia != 0.0 {
+        1.0 / lmp.inv_inertia
+    } else {
+        0.0
+    }
+}
+
+/*
+ * Host-side conversion of the AoS structs into the SoA buffer. `data` is
+ * batch-major (`batch · links_cap + link`).
+ */
+#[cfg(not(target_arch_is_gpu))]
+pub fn ws_soa_from_structs(
+    data: &[MultibodyLinkWorkspace],
+    links_cap: u32,
+    num_batches: u32,
+) -> std::vec::Vec<Vec4> {
+    let mut out =
+        std::vec![Vec4::ZERO; links_cap as usize * WS_QUADS as usize * num_batches as usize];
+    for b in 0..num_batches {
+        let a = WsAddr::new(0, num_batches, b);
+        for k in 0..links_cap {
+            let ws = &data[(b * links_cap + k) as usize];
+            ws_set_rot(&mut out, a, k, WS_JOINT_ROT, ws.joint_rot);
+            for (i, &c) in ws.coords.iter().enumerate() {
+                ws_set_coord(&mut out, a, k, i as u32, c);
+            }
+            ws_set_pose(&mut out, a, k, WS_LTP, ws.local_to_parent);
+            ws_set_pose(&mut out, a, k, WS_LTW, ws.local_to_world);
+            ws_set_vec(&mut out, a, k, WS_SHIFT02, ws.shift02);
+            ws_set_vec(&mut out, a, k, WS_SHIFT23, ws.shift23);
+            ws_set_vel(&mut out, a, k, WS_JOINT_VEL, ws.joint_velocity);
+            ws_set_vel(&mut out, a, k, WS_RB_VELS, ws.rb_vels);
+            ws_set_vel(&mut out, a, k, WS_KIN_ACC, ws.kinematic_acc);
+        }
+    }
+    out
+}
diff --git a/src_rbd_shaders/dynamics/sim_params.rs b/src_rbd_shaders/dynamics/sim_params.rs
index b6de4a7..e5c7762 100644
--- a/src_rbd_shaders/dynamics/sim_params.rs
+++ b/src_rbd_shaders/dynamics/sim_params.rs
@@ -8,7 +8,7 @@ pub const TWO_PI: f32 = core::f32::consts::TAU;
 /// Precomputed soft-constraint coefficients (contact + joint), matching rapier's
 /// TGS-soft `SpringCoefficients`. Computed once per step on the host from
 /// [`RbdSimParams`] and passed to the multibody contact/joint kernels as a small
-/// uniform. Exactly 32 bytes (8 scalars) for std140 uniform layout.
+/// uniform. Exactly 48 bytes (12 scalars) for std140 uniform layout.
 #[derive(Clone, Copy, Default)]
 #[cfg_attr(not(target_arch_is_gpu), derive(bytemuck::Pod, bytemuck::Zeroable))]
 #[repr(C)]
@@ -20,7 +20,7 @@ pub struct ConstraintSoftness {
     /// Contact `1 / (1 + cfm_coeff)` — multiplies the contact impulse each PGS
     /// sweep for constraint-force-mixing compliance.
     pub cfm_factor: f32,
-    /// Penetration the solver won't try to correct.
+    /// Geometric slop distance.
     pub allowed_lin_err: f32,
     /// Max corrective velocity applied for penetration recovery.
     pub max_corr_velocity: f32,
@@ -36,6 +36,14 @@ pub struct ConstraintSoftness {
     pub joint_cfm_coeff: f32,
     /// Substep `dt`, needed by `motor_params` for joint motors.
     pub dt: f32,
+    /// [`Self::erp_inv_dt`] for contacts touching a fixed body.
+    pub static_erp_inv_dt: f32,
+    /// [`Self::cfm_factor`] for contacts touching a fixed body.
+    pub static_cfm_factor: f32,
+    /// Unused; keeps the uniform a 16-byte multiple.
+    pub _padding0: f32,
+    /// Unused; keeps the uniform a 16-byte multiple.
+    pub _padding1: f32,
 }
 
 #[cfg(not(target_arch_is_gpu))]
@@ -52,6 +60,10 @@ impl ConstraintSoftness {
             joint_erp_inv_dt: params.joint_erp_inv_dt(),
             joint_cfm_coeff: params.joint_cfm_coeff(),
             dt: params.dt,
+            static_erp_inv_dt: params.static_contact_erp_inv_dt(),
+            static_cfm_factor: params.static_contact_cfm_factor(),
+            _padding0: 0.0,
+            _padding1: 0.0,
         }
     }
 }
@@ -68,7 +80,7 @@ pub struct RbdSimParams {
     ///
     /// Larger values make the constraints more compliant (allowing more visible
     /// penetrations before stabilization).
-    /// (default `5.0`).
+    /// (default `10.0`).
     pub contact_damping_ratio: f32,
 
     /// > 0: the natural frequency used by the springs for contact constraint regularization.
@@ -80,6 +92,14 @@ pub struct RbdSimParams {
     /// (default: `30.0`).
     pub contact_natural_frequency: f32,
 
+    /// [`Self::contact_damping_ratio`] for contacts touching a fixed body.
+    /// (default `10.0`).
+    pub static_contact_damping_ratio: f32,
+
+    /// [`Self::contact_natural_frequency`] for contacts touching a fixed body.
+    /// (default: `60.0`).
+    pub static_contact_natural_frequency: f32,
+
     /// > 0: the natural frequency used by the springs for joint constraint regularization.
     ///
     /// Increasing this value will make it so that penetrations get fixed more quickly.
@@ -114,21 +134,29 @@ pub struct RbdSimParams {
     /// (default `1.0`).
     pub length_unit: f32,
 
-    /// Amount of penetration the engine won't attempt to correct (default: `0.001m`).
+    /// Geometric slop distance (default: `0.005m`).
     ///
     /// This value is implicitly scaled by `length_unit`.
     pub normalized_allowed_linear_error: f32,
 
-    /// Maximum amount of penetration the solver will attempt to resolve in one timestep (default: `10.0`).
+    /// Maximum speed at which contact penetration is pushed out by the biased solve (default: `3.0`).
     ///
+    /// Capping this recovery velocity keeps deep penetrations from being resolved explosively.
     /// This value is implicitly scaled by `length_unit`.
     pub normalized_max_corrective_velocity: f32,
 
-    /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002m`).
+    /// The maximal distance separating two objects that will generate predictive contacts
+    /// (default: `0.02m`)
     ///
     /// This value is implicitly scaled by `length_unit`.
     pub normalized_prediction_distance: f32,
 
+    /// Maximum linear velocity a body may have after each solver substep (default: `400.0` m/s).
+    /// Bounding per-step travel keeps speculative contacts reliable; set to `f32::MAX` to disable.
+    ///
+    /// This value is implicitly scaled by `length_unit`.
+    pub normalized_max_linear_velocity: f32,
+
     /// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
     pub num_solver_iterations: u32,
 }
@@ -142,14 +170,17 @@ impl RbdSimParams {
         Self {
             dt: 1.0 / 60.0,
             contact_natural_frequency: 30.0,
-            contact_damping_ratio: 5.0,
+            contact_damping_ratio: 10.0,
+            static_contact_natural_frequency: 60.0,
+            static_contact_damping_ratio: 10.0,
             joint_natural_frequency: 1.0e6,
             joint_damping_ratio: 1.0,
             warmstart_coefficient: 1.0,
             num_solver_iterations: 4,
-            normalized_allowed_linear_error: 0.001,
-            normalized_max_corrective_velocity: 10.0,
-            normalized_prediction_distance: 0.002,
+            normalized_allowed_linear_error: 0.005,
+            normalized_max_corrective_velocity: 3.0,
+            normalized_prediction_distance: 0.02,
+            normalized_max_linear_velocity: 400.0,
             length_unit: 1.0,
         }
     }
@@ -167,6 +198,28 @@ impl RbdSimParams {
         if self.dt == 0.0 { 0.0 } else { 1.0 / self.dt }
     }
 
+    /// The soft-constraint `erp / dt` for a spring with the given natural
+    /// frequency (Hz) and damping ratio.
+    fn spring_erp_inv_dt(&self, natural_frequency: f32, damping_ratio: f32) -> f32 {
+        let ang_freq = natural_frequency * TWO_PI;
+        ang_freq / (self.dt * ang_freq + 2.0 * damping_ratio)
+    }
+
+    /// The soft-constraint CFM factor `1 / (1 + cfm_coeff)` for a spring with
+    /// the given natural frequency (Hz) and damping ratio.
+    ///
+    /// See [`Self::contact_cfm_factor`] for the derivation.
+    fn spring_cfm_factor(&self, natural_frequency: f32, damping_ratio: f32) -> f32 {
+        let erp = self.dt * self.spring_erp_inv_dt(natural_frequency, damping_ratio);
+        if erp == 0.0 {
+            return 0.0;
+        }
+        let inv_erp_minus_one = 1.0 / erp - 1.0;
+        let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
+            / ((1.0 + inv_erp_minus_one) * 4.0 * damping_ratio * damping_ratio);
+        1.0 / (1.0 + cfm_coeff)
+    }
+
     /// Computes the contact constraint angular frequency (rad/s).
     pub fn contact_angular_frequency(&self) -> f32 {
         self.contact_natural_frequency * TWO_PI
@@ -174,8 +227,16 @@ impl RbdSimParams {
 
     /// The `contact_erp` coefficient, multiplied by the inverse timestep length.
     pub fn contact_erp_inv_dt(&self) -> f32 {
-        let ang_freq = self.contact_angular_frequency();
-        ang_freq / (self.dt * ang_freq + 2.0 * self.contact_damping_ratio)
+        self.spring_erp_inv_dt(self.contact_natural_frequency, self.contact_damping_ratio)
+    }
+
+    /// [`Self::contact_erp_inv_dt`] for contacts touching a fixed body
+    /// (rapier's `static_contact_softness`).
+    pub fn static_contact_erp_inv_dt(&self) -> f32 {
+        self.spring_erp_inv_dt(
+            self.static_contact_natural_frequency,
+            self.static_contact_damping_ratio,
+        )
     }
 
     /// The effective Error Reduction Parameter applied for calculating regularization forces
@@ -212,43 +273,16 @@ impl RbdSimParams {
     /// This parameter is computed automatically from `contact_natural_frequency`,
     /// `contact_damping_ratio` and the substep length.
     pub fn contact_cfm_factor(&self) -> f32 {
-        // Compute CFM assuming a critically damped spring multiplied by the damping ratio.
-        // The logic is similar to `joint_cfm_coeff`.
-        let contact_erp = self.contact_erp();
-        if contact_erp == 0.0 {
-            return 0.0;
-        }
-        let inv_erp_minus_one = 1.0 / contact_erp - 1.0;
-
-        // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
-        //     / (dt * dt * inv_erp_minus_one * inv_erp_minus_one);
-        // let damping = 4.0 * damping_ratio * damping_ratio * projected_mass
-        //     / (dt * inv_erp_minus_one);
-        // let cfm = 1.0 / (dt * dt * stiffness + dt * damping);
-        // NOTE: This simplifies to cfm = cfm_coeff / projected_mass:
-        let cfm_coeff = inv_erp_minus_one * inv_erp_minus_one
-            / ((1.0 + inv_erp_minus_one)
-                * 4.0
-                * self.contact_damping_ratio
-                * self.contact_damping_ratio);
-
-        // Furthermore, we use this coefficient inside of the impulse resolution.
-        // Surprisingly, several simplifications happen there.
-        // Let `m` the projected mass of the constraint.
-        // Let `m'` the projected mass that includes CFM: `m' = 1 / (1 / m + cfm_coeff / m) = m / (1 + cfm_coeff)`
-        // We have:
-        // new_impulse = old_impulse - m' (delta_vel - cfm * old_impulse)
-        //             = old_impulse - m / (1 + cfm_coeff) * (delta_vel - cfm_coeff / m * old_impulse)
-        //             = old_impulse * (1 - cfm_coeff / (1 + cfm_coeff)) - m / (1 + cfm_coeff) * delta_vel
-        //             = old_impulse / (1 + cfm_coeff) - m * delta_vel / (1 + cfm_coeff)
-        //             = 1 / (1 + cfm_coeff) * (old_impulse - m * delta_vel)
-        // So, setting cfm_factor = 1 / (1 + cfm_coeff).
-        // We obtain:
-        // new_impulse = cfm_factor * (old_impulse - m * delta_vel)
-        //
-        // The value returned by this function is this cfm_factor that can be used directly
-        // in the constraint solver.
-        1.0 / (1.0 + cfm_coeff)
+        self.spring_cfm_factor(self.contact_natural_frequency, self.contact_damping_ratio)
+    }
+
+    /// [`Self::contact_cfm_factor`] for contacts touching a fixed body
+    /// (rapier's `static_contact_softness`).
+    pub fn static_contact_cfm_factor(&self) -> f32 {
+        self.spring_cfm_factor(
+            self.static_contact_natural_frequency,
+            self.static_contact_damping_ratio,
+        )
     }
 
     /// The CFM (constraints force mixing) coefficient applied to all joints for constraints regularization.
@@ -270,8 +304,7 @@ impl RbdSimParams {
                 * self.joint_damping_ratio)
     }
 
-    /// Amount of penetration the engine won't attempt to correct (default: `0.001` multiplied by
-    /// `length_unit`).
+    /// Geometric slop distance (default: `0.005` multiplied by `length_unit`).
     pub fn allowed_linear_error(&self) -> f32 {
         self.normalized_allowed_linear_error * self.length_unit
     }
@@ -289,8 +322,27 @@ impl RbdSimParams {
     }
 
     /// The maximal distance separating two objects that will generate predictive contacts
-    /// (default: `0.002m` multiplied by `length_unit`).
+    /// (default: `0.02m` multiplied by `length_unit`).
     pub fn prediction_distance(&self) -> f32 {
         self.normalized_prediction_distance * self.length_unit
     }
+
+    /// Maximum linear velocity a body may have after each solver substep.
+    ///
+    /// This is equal to `normalized_max_linear_velocity` multiplied by
+    /// `length_unit`, or `f32::MAX` when the linear speed cap is disabled.
+    pub fn max_linear_velocity(&self) -> f32 {
+        if self.normalized_max_linear_velocity != MAX_FLT {
+            self.normalized_max_linear_velocity * self.length_unit
+        } else {
+            MAX_FLT
+        }
+    }
+
+    /// Per-substep angular speed cap: bounds the total rotation per step to `π/4`
+    /// regardless of the substep count.
+    pub fn max_angular_velocity(&self) -> f32 {
+        const MAX_ROTATION: f32 = core::f32::consts::FRAC_PI_4;
+        MAX_ROTATION * self.inv_dt() / self.num_solver_iterations.max(1) as f32
+    }
 }
diff --git a/src_rbd_shaders/dynamics/solver.rs b/src_rbd_shaders/dynamics/solver.rs
index 6dfbe62..3b2ddf3 100644
--- a/src_rbd_shaders/dynamics/solver.rs
+++ b/src_rbd_shaders/dynamics/solver.rs
@@ -6,7 +6,11 @@ use khal_std::glamx::UVec3;
 use khal_std::macros::{spirv, spirv_bindgen};
 
 use crate::{AngVector, Pose, Vector};
-use khal_std::{index::MaybeIndexUnchecked, iter::StepRng, sync::atomic_add_u32};
+use khal_std::{
+    index::MaybeIndexUnchecked,
+    iter::StepRng,
+    sync::{atomic_add_u32, control_barrier},
+};
 
 use super::body::{LocalMassProperties, Velocity, WorldMassProperties};
 use super::constraint::{TwoBodyConstraint, TwoBodyConstraintBuilder};
@@ -18,39 +22,6 @@ use crate::utils::{BatchIndices, Slice, SliceMut};
 
 const WORKGROUP_SIZE: u32 = 64;
 
-/// Resets the current color to 1 (for graph coloring).
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_solver_reset_color(
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] curr_color: &mut u32,
-) {
-    // NOTE: this `for` loop is silly. It doesn't do anything
-    //       more than a `*curr_color = 1` in a convoluted
-    //       way because otherwise rustgpu apparently does not generate
-    //       the spirv for this kernel (seems to happen if the kernel is
-    //       too trivial.
-    for k in 0..1 {
-        // NOTE: our first colors start at 1 instead of 0.
-        *curr_color = 1 + k;
-    }
-}
-
-/// Increments the current color.
-#[spirv_bindgen]
-#[spirv(compute(threads(1)))]
-pub fn gpu_solver_inc_color(
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] curr_color: &mut u32,
-) {
-    // NOTE: this `for` loop is silly. It doesn't do anything
-    //       more than a `*curr_color += 1` in a convoluted
-    //       way because otherwise rustgpu apparently does not generate
-    //       the spirv for this kernel (seems to happen if the kernel is
-    //       too trivial.
-    for k in 0..1 {
-        *curr_color += 1 + k;
-    }
-}
-
 /// Initializes constraints from contact manifolds.
 ///
 /// Split into two passes to stay within WebGPU's 8-storage-buffer per-stage
@@ -84,11 +55,7 @@ pub fn gpu_solver_init_constraints(
     let solver_body_poses = batch_ids.coll_batch(batch_id, solver_body_poses);
     let vels = batch_ids.coll_batch(batch_id, vels);
     let mprops = batch_ids.coll_batch(batch_id, mprops);
-    // Iterating to `cap` (instead of `contacts_len[batch]`) lets us drop the
-    // `contacts_len` storage binding. Empty / unused contact slots have
-    // `contact.len == 0` and are skipped — narrow-phase zero-initialises the
-    // buffer so the sentinel is reliable.
-    let cap = batch_ids.contacts_batch_capacity;
+    let cap = batch_ids.contacts_batch_capacity.min(num_threads);
 
     for i in StepRng::new(invocation_id.x..cap, num_threads) {
         let im = &contacts[i as usize];
@@ -118,7 +85,8 @@ pub fn gpu_solver_count_constraints(
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] body_constraint_counts: &mut [u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] body_group: &[u32],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] mprops: &[WorldMassProperties],
-    #[spirv(uniform, descriptor_set = 0, binding = 4)] batch_ids: &BatchIndices,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] contacts_len: &[u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
 ) {
     let num_threads = num_workgroups.x * WORKGROUP_SIZE;
     let batch_id = invocation_id.y;
@@ -127,14 +95,10 @@ pub fn gpu_solver_count_constraints(
     let mut body_constraint_counts = batch_ids.coll_batch_mut(batch_id, body_constraint_counts);
     let body_group = batch_ids.coll_batch(batch_id, body_group);
     let mprops = batch_ids.coll_batch(batch_id, mprops);
-    let cap = batch_ids.contacts_batch_capacity;
+    let len = contacts_len.read(batch_id as usize);
 
-    for i in StepRng::new(invocation_id.x..cap, num_threads) {
+    for i in StepRng::new(invocation_id.x..len, num_threads) {
         let im = &contacts[i as usize];
-        if im.contact.len == 0 {
-            continue;
-        }
-
         let body1 = im.bodies.x;
         let body2 = im.bodies.y;
         let group1 = body_group[body1 as usize];
@@ -189,6 +153,40 @@ pub fn gpu_solver_update_constraints(
     }
 }
 
+/// Relax-pass refresh: recomputes the unbiased normal rhs of every contact
+/// constraint from the current (post-integration) solver poses.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_solver_refresh_rhs_wo_bias(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(num_workgroups)] num_workgroups: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
+    constraints: &mut [TwoBodyConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)]
+    constraint_builders: &[TwoBodyConstraintBuilder],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] contacts_len: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 0)] solver_body_poses: &[Pose],
+    #[spirv(storage_buffer, descriptor_set = 1, binding = 1)] all_params: &[RbdSimParams],
+    #[spirv(uniform, descriptor_set = 1, binding = 2)] batch_ids: &BatchIndices,
+) {
+    let num_threads = num_workgroups.x * WORKGROUP_SIZE;
+    let batch_id = invocation_id.y;
+    let params = all_params.at(batch_id as usize);
+
+    let mut constraints = batch_ids.contact_batch_mut(batch_id, constraints);
+    let constraint_builders = batch_ids.contact_batch(batch_id, constraint_builders);
+    let solver_body_poses = batch_ids.coll_batch(batch_id, solver_body_poses);
+    let len = contacts_len.read(batch_id as usize);
+
+    for i in StepRng::new(invocation_id.x..len, num_threads) {
+        constraints[i as usize].refresh_rhs_wo_bias(
+            &constraint_builders[i as usize],
+            &solver_body_poses,
+            params,
+        );
+    }
+}
+
 #[spirv_bindgen]
 #[spirv(compute(threads(64)))]
 pub fn gpu_solver_sort_constraints(
@@ -368,22 +366,129 @@ pub fn gpu_warmstart(
     #[spirv(num_workgroups)] num_workgroups: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] constraints: &[TwoBodyConstraint],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] solver_vels: &mut [Velocity],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] constraints_colors: &[u32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] contacts_len: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_starts: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] color_sorted_ids: &[u32],
     #[spirv(uniform, descriptor_set = 0, binding = 4)] curr_color: &u32,
     #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
 ) {
     let num_threads = num_workgroups.x * WORKGROUP_SIZE;
     let batch_id = invocation_id.y;
+    let stride = batch_ids.solver_color_buckets_stride;
 
     let constraints = batch_ids.contact_batch(batch_id, constraints);
-    let constraints_colors = batch_ids.contact_batch(batch_id, constraints_colors);
+    let color_sorted_ids = batch_ids.contact_batch(batch_id, color_sorted_ids);
     let mut solver_vels = batch_ids.coll_batch_mut(batch_id, solver_vels);
-    let len = contacts_len.read(batch_id as usize);
     let color = *curr_color;
 
-    for i in StepRng::new(invocation_id.x..len, num_threads) {
-        if constraints_colors[i as usize] == color {
+    let bucket = (batch_id * stride + color) as usize;
+    let start = color_starts.read(bucket);
+    let end = color_starts.read(bucket + 1);
+
+    for k in StepRng::new(start + invocation_id.x..end, num_threads) {
+        let i = color_sorted_ids[k as usize];
+        let constraint = &constraints[i as usize];
+        let solver_id1 = constraint.solver_body_a as usize;
+        let solver_id2 = constraint.solver_body_b as usize;
+
+        let mut solver_vel1 = solver_vels[solver_id1];
+        let mut solver_vel2 = solver_vels[solver_id2];
+
+        constraint.warmstart_constraint(&mut solver_vel1, &mut solver_vel2);
+
+        solver_vels[solver_id1] = solver_vel1;
+        solver_vels[solver_id2] = solver_vel2;
+    }
+}
+
+/// Main constraint solver iteration kernel (Projected Gauss-Seidel).
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_step_gauss_seidel(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(num_workgroups)] num_workgroups: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
+    constraints: &mut [TwoBodyConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] solver_vels: &mut [Velocity],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_starts: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] color_sorted_ids: &[u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 4)] curr_color: &u32,
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
+    #[spirv(uniform, descriptor_set = 0, binding = 6)] use_bias: &u32,
+) {
+    let num_threads = num_workgroups.x * WORKGROUP_SIZE;
+    let batch_id = invocation_id.y;
+    let stride = batch_ids.solver_color_buckets_stride;
+
+    let mut constraints = batch_ids.contact_batch_mut(batch_id, constraints);
+    let color_sorted_ids = batch_ids.contact_batch(batch_id, color_sorted_ids);
+    let mut solver_vels = batch_ids.coll_batch_mut(batch_id, solver_vels);
+    let color = *curr_color;
+    let use_bias = *use_bias != 0;
+
+    let bucket = (batch_id * stride + color) as usize;
+    let start = color_starts.read(bucket);
+    let end = color_starts.read(bucket + 1);
+
+    for k in StepRng::new(start + invocation_id.x..end, num_threads) {
+        let i = color_sorted_ids[k as usize];
+        let solver_id1 = constraints[i as usize].solver_body_a as usize;
+        let solver_id2 = constraints[i as usize].solver_body_b as usize;
+
+        let mut solver_vel1 = solver_vels[solver_id1];
+        let mut solver_vel2 = solver_vels[solver_id2];
+
+        constraints[i as usize].solve_constraint_gauss_seidel(
+            &mut solver_vel1,
+            &mut solver_vel2,
+            use_bias,
+        );
+
+        solver_vels[solver_id1] = solver_vel1;
+        solver_vels[solver_id2] = solver_vel2;
+    }
+}
+
+/// Fused colored warmstart: only one 64-lane workgroup per batch walks every color
+/// bucket.
+///
+/// Used for small scenes where the contact count is small wrt. the environment count.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_warmstart_fused(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] constraints: &[TwoBodyConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] solver_vels: &mut [Velocity],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_starts: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] color_sorted_ids: &[u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 4)] num_colors: &u32,
+    #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
+) {
+    let lane = invocation_id.x;
+    let batch_id = invocation_id.y;
+    let stride = batch_ids.solver_color_buckets_stride;
+
+    let constraints = batch_ids.contact_batch(batch_id, constraints);
+    let color_sorted_ids = batch_ids.contact_batch(batch_id, color_sorted_ids);
+    let mut solver_vels = batch_ids.coll_batch_mut(batch_id, solver_vels);
+    let num_colors = *num_colors;
+
+    let base = (batch_id * stride) as usize;
+    if color_starts.read(base + 1) == color_starts.read(base + num_colors as usize + 1) {
+        // Every color bucket is empty.
+        return;
+    }
+
+    for color in 1..=num_colors {
+        let bucket = base + color as usize;
+        let start = color_starts.read(bucket);
+        let end = color_starts.read(bucket + 1);
+        if start == end {
+            // Empty color.
+            continue;
+        }
+
+        for k in StepRng::new(start + lane..end, WORKGROUP_SIZE) {
+            let i = color_sorted_ids[k as usize];
             let constraint = &constraints[i as usize];
             let solver_id1 = constraint.solver_body_a as usize;
             let solver_id2 = constraint.solver_body_b as usize;
@@ -396,47 +501,85 @@ pub fn gpu_warmstart(
             solver_vels[solver_id1] = solver_vel1;
             solver_vels[solver_id2] = solver_vel2;
         }
+
+        control_barrier::<
+            { khal_std::memory::Scope::Workgroup as u32 },
+            { khal_std::memory::Scope::QueueFamily as u32 },
+            {
+                khal_std::memory::Semantics::UNIFORM_MEMORY.bits()
+                    | khal_std::memory::Semantics::ACQUIRE_RELEASE.bits()
+            },
+        >();
     }
 }
 
-/// Main constraint solver iteration kernel (Projected Gauss-Seidel).
+/// Fused colored Gauss-Seidel sweep: only one 64-lane workgroup per batch walks
+/// every color bucket with a storage barrier between colors.
+///
+/// Used for small scenes where the contact count is small wrt. the environment count.
 #[spirv_bindgen]
 #[spirv(compute(threads(64)))]
-pub fn gpu_step_gauss_seidel(
+pub fn gpu_step_gauss_seidel_fused(
     #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(num_workgroups)] num_workgroups: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
     constraints: &mut [TwoBodyConstraint],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] solver_vels: &mut [Velocity],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] constraints_colors: &[u32],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] contacts_len: &[u32],
-    #[spirv(uniform, descriptor_set = 0, binding = 4)] curr_color: &u32,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] color_starts: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] color_sorted_ids: &[u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 4)] num_colors: &u32,
     #[spirv(uniform, descriptor_set = 0, binding = 5)] batch_ids: &BatchIndices,
+    #[spirv(uniform, descriptor_set = 0, binding = 6)] use_bias: &u32,
 ) {
-    let num_threads = num_workgroups.x * WORKGROUP_SIZE;
+    let lane = invocation_id.x;
     let batch_id = invocation_id.y;
+    let stride = batch_ids.solver_color_buckets_stride;
 
     let mut constraints = batch_ids.contact_batch_mut(batch_id, constraints);
-    let constraints_colors = batch_ids.contact_batch(batch_id, constraints_colors);
+    let color_sorted_ids = batch_ids.contact_batch(batch_id, color_sorted_ids);
     let mut solver_vels = batch_ids.coll_batch_mut(batch_id, solver_vels);
-    let len = contacts_len.read(batch_id as usize);
-    let color = *curr_color;
+    let num_colors = *num_colors;
+    let use_bias = *use_bias != 0;
 
-    for i in StepRng::new(invocation_id.x..len, num_threads) {
-        // Only process constraints of the current color (for parallelization)
-        if constraints_colors[i as usize] == color {
+    // Early-out / empty-color skip: see `gpu_warmstart_fused`.
+    let base = (batch_id * stride) as usize;
+    if color_starts.read(base + 1) == color_starts.read(base + num_colors as usize + 1) {
+        return;
+    }
+
+    for color in 1..=num_colors {
+        let bucket = base + color as usize;
+        let start = color_starts.read(bucket);
+        let end = color_starts.read(bucket + 1);
+        if start == end {
+            continue;
+        }
+
+        for k in StepRng::new(start + lane..end, WORKGROUP_SIZE) {
+            let i = color_sorted_ids[k as usize];
             let solver_id1 = constraints[i as usize].solver_body_a as usize;
             let solver_id2 = constraints[i as usize].solver_body_b as usize;
 
             let mut solver_vel1 = solver_vels[solver_id1];
             let mut solver_vel2 = solver_vels[solver_id2];
 
-            constraints[i as usize]
-                .solve_constraint_gauss_seidel(&mut solver_vel1, &mut solver_vel2);
+            constraints[i as usize].solve_constraint_gauss_seidel(
+                &mut solver_vel1,
+                &mut solver_vel2,
+                use_bias,
+            );
 
             solver_vels[solver_id1] = solver_vel1;
             solver_vels[solver_id2] = solver_vel2;
         }
+
+        control_barrier::<
+            { khal_std::memory::Scope::Workgroup as u32 },
+            { khal_std::memory::Scope::QueueFamily as u32 },
+            {
+                khal_std::memory::Semantics::UNIFORM_MEMORY.bits()
+                    | khal_std::memory::Semantics::ACQUIRE_RELEASE.bits()
+            },
+        >();
     }
 }
 
@@ -446,7 +589,7 @@ pub fn gpu_step_gauss_seidel(
 pub fn gpu_integrate_linearized(
     #[spirv(global_invocation_id)] invocation_id: UVec3,
     #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] poses: &mut [Pose],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] solver_vels: &[Velocity],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] solver_vels: &mut [Velocity],
     #[spirv(storage_buffer, descriptor_set = 0, binding = 2)] all_params: &[RbdSimParams],
     #[spirv(uniform, descriptor_set = 0, binding = 3)] batch_ids: &BatchIndices,
 ) {
@@ -456,11 +599,32 @@ pub fn gpu_integrate_linearized(
 
     let num_bodies = batch_ids.bodies_len;
     let mut poses = batch_ids.coll_batch_mut(batch_id, poses);
-    let solver_vels = batch_ids.coll_batch(batch_id, solver_vels);
+    let mut solver_vels = batch_ids.coll_batch_mut(batch_id, solver_vels);
 
     if i < num_bodies {
         let idx = i as usize;
-        let vels = &solver_vels[idx];
+        let mut vels = solver_vels[idx];
+
+        let max_lin = params.max_linear_velocity();
+        let lin_norm = vels.linear.length();
+        if lin_norm > max_lin {
+            vels.linear *= max_lin / lin_norm;
+        }
+
+        let max_ang = params.max_angular_velocity();
+        #[cfg(feature = "dim2")]
+        if vels.angular.abs() > max_ang {
+            vels.angular = vels.angular.signum() * max_ang;
+        }
+        #[cfg(feature = "dim3")]
+        {
+            let ang_norm = vels.angular.length();
+            if ang_norm > max_ang {
+                vels.angular *= max_ang / ang_norm;
+            }
+        }
+
+        solver_vels[idx] = vels;
         let pose = &mut poses[idx];
         vels.integrate_linearized(params.dt, &mut pose.translation, &mut pose.rotation);
     }
@@ -529,23 +693,3 @@ pub fn gpu_solver_finalize(
     }
 }
 
-/// Removes CFM and bias from constraints for the final substep iteration.
-#[spirv_bindgen]
-#[spirv(compute(threads(64)))]
-pub fn gpu_remove_cfm_and_bias_kernel(
-    #[spirv(global_invocation_id)] invocation_id: UVec3,
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)]
-    constraints: &mut [TwoBodyConstraint],
-    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] contacts_len: &[u32],
-    #[spirv(uniform, descriptor_set = 0, binding = 2)] batch_ids: &BatchIndices,
-) {
-    let batch_id = invocation_id.y;
-    let i = invocation_id.x;
-
-    let mut constraints = batch_ids.contact_batch_mut(batch_id, constraints);
-    let len = contacts_len.read(batch_id as usize);
-
-    if i < len {
-        constraints[i as usize].remove_cfm_and_bias();
-    }
-}
diff --git a/src_rbd_shaders/dynamics/solver_utils.rs b/src_rbd_shaders/dynamics/solver_utils.rs
index 2f65380..974379f 100644
--- a/src_rbd_shaders/dynamics/solver_utils.rs
+++ b/src_rbd_shaders/dynamics/solver_utils.rs
@@ -66,42 +66,17 @@ fn orthonormal_vector(vec: Vector) -> Vector {
 
 #[cfg(feature = "dim2")]
 /// Computes the tangent contact directions for friction (2D version).
-fn compute_tangent_contact_directions(
-    force_dir1: Vector,
-    _linvel1: Vector,
-    _linvel2: Vector,
-) -> [Vector; SUB_LEN] {
+fn compute_tangent_contact_directions(force_dir1: Vector) -> [Vector; SUB_LEN] {
     [orthonormal_vector(force_dir1)]
 }
 
 #[cfg(feature = "dim3")]
 /// Computes the tangent contact directions for friction (3D version).
-fn compute_tangent_contact_directions(
-    force_dir1: Vector,
-    linvel1: Vector,
-    linvel2: Vector,
-) -> [Vector; SUB_LEN] {
-    // Compute the tangent direction. Pick the direction of
-    // the linear relative velocity, if it is not too small.
-    // Otherwise use a fallback direction.
-    let relative_linvel = linvel1 - linvel2;
-    let mut tangent_relative_linvel =
-        relative_linvel - force_dir1 * force_dir1.dot(relative_linvel);
-
-    let tangent_linvel_norm = tangent_relative_linvel.length();
-    tangent_relative_linvel /= tangent_linvel_norm;
-
-    const THRESHOLD: f32 = 1.0e-4;
-    let use_fallback = tangent_linvel_norm < THRESHOLD;
-    let tangent_fallback = orthonormal_vector(force_dir1);
-
-    let tangent1 = if use_fallback {
-        tangent_fallback
-    } else {
-        tangent_relative_linvel
-    };
+///
+/// A deterministic basis derived from the contact normal.
+fn compute_tangent_contact_directions(force_dir1: Vector) -> [Vector; SUB_LEN] {
+    let tangent1 = orthonormal_vector(force_dir1);
     let bitangent1 = force_dir1.cross(tangent1);
-
     [tangent1, bitangent1]
 }
 
@@ -153,23 +128,19 @@ impl IndexedManifold {
 
         let force_dir1 = -(cpose1.rotation * contact.normal_a);
 
-        let cfm_factor = params.contact_cfm_factor();
-        let inv_dt_val = params.inv_dt();
-        let erp_inv_dt = params.contact_erp_inv_dt();
-        let allowed_lin_err = params.allowed_linear_error();
-        let max_corr_velocity = params.max_corrective_velocity();
-
         // Combined friction / restitution of the two colliders, resolved at
         // narrow-phase time (rapier's `CoefficientCombineRule`) and stored in the
         // manifold.
         let friction = self.friction;
         let restitution = self.restitution;
 
-        let tangents1 = compute_tangent_contact_directions(force_dir1, vel1.linear, vel2.linear);
+        let tangents1 = compute_tangent_contact_directions(force_dir1);
         constraint.dir_a = force_dir1;
         constraint.im_a = mprops1.inv_mass;
         constraint.im_b = mprops2.inv_mass;
-        constraint.cfm_factor = cfm_factor;
+        // Softness (cfm, erp) and the rhs are (re)computed by
+        // `update_constraint` before the first solve of every substep.
+        constraint.cfm_factor = params.contact_cfm_factor();
         constraint.limit = friction;
         constraint.solver_body_a = bid1;
         constraint.solver_body_b = bid2;
@@ -206,28 +177,24 @@ impl IndexedManifold {
 
             // TODO: handle is_bouncy?
             let dist = contact.points_a.at(k).dist;
-            let normal_rhs_wo_bias = restitution * (contact_vel1 - contact_vel2).dot(force_dir1)
-                + dist.max(0.0) * inv_dt_val;
-
-            let rhs_bias = (erp_inv_dt * (dist + allowed_lin_err)).clamp(-max_corr_velocity, 0.0);
+            let normal_rhs_wo_bias = restitution * (contact_vel1 - contact_vel2).dot(force_dir1);
 
             constraint.elements.at_mut(k).normal_part = TwoBodyConstraintNormalPart {
                 torque_dir_a: torque_dir1,
                 ii_torque_dir_a: ii_torque_dir1,
                 torque_dir_b: torque_dir2,
                 ii_torque_dir_b: ii_torque_dir2,
-                rhs: normal_rhs_wo_bias + rhs_bias,
+                rhs: normal_rhs_wo_bias,
                 rhs_wo_bias: normal_rhs_wo_bias,
                 impulse: 0.0,
                 r: projected_mass,
+                cfm_factor: 1.0,
                 #[cfg(feature = "dim3")]
                 _padding0: 0.0,
                 #[cfg(feature = "dim3")]
                 _padding1: 0.0,
                 #[cfg(feature = "dim3")]
                 _padding2: 0.0,
-                #[cfg(feature = "dim3")]
-                _padding3: 0.0,
             };
 
             //
@@ -322,10 +289,18 @@ impl TwoBodyConstraint {
         let body1 = self.solver_body_a as usize;
         let body2 = self.solver_body_b as usize;
 
-        let cfm_factor = params.contact_cfm_factor();
+        let is_static = self.im_a == Vector::ZERO || self.im_b == Vector::ZERO;
+        let cfm_factor = if is_static {
+            params.static_contact_cfm_factor()
+        } else {
+            params.contact_cfm_factor()
+        };
+        let erp_inv_dt = if is_static {
+            params.static_contact_erp_inv_dt()
+        } else {
+            params.contact_erp_inv_dt()
+        };
         let inv_dt_val = params.inv_dt();
-        let allowed_lin_err = params.allowed_linear_error();
-        let erp_inv_dt = params.contact_erp_inv_dt();
         let max_corr_velocity = params.max_corrective_velocity();
         let warmstart_coeff = params.warmstart_coefficient;
 
@@ -349,12 +324,14 @@ impl TwoBodyConstraint {
             // Normal part.
             {
                 let rhs_wo_bias = info.normal_vel + dist.max(0.0) * inv_dt_val;
-                let rhs_bias =
-                    ((dist + allowed_lin_err) * erp_inv_dt).clamp(-max_corr_velocity, 0.0);
+                let rhs_bias = (dist * erp_inv_dt).clamp(-max_corr_velocity, 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
 
                 self.elements.at_mut(j).normal_part.rhs_wo_bias = rhs_wo_bias;
                 self.elements.at_mut(j).normal_part.rhs = new_rhs;
+                // Separated (speculative) points are solved rigidly.
+                self.elements.at_mut(j).normal_part.cfm_factor =
+                    if dist <= 0.0 { cfm_factor } else { 1.0 };
                 self.elements.at_mut(j).normal_part.impulse *= warmstart_coeff;
             }
 
@@ -400,6 +377,33 @@ impl TwoBodyConstraint {
 
         self.cfm_factor = cfm_factor;
     }
+
+    /// Relax-pass refresh: recompute the unbiased normal rhs (speculative term
+    /// included) from the current solver poses.
+    #[inline(always)]
+    pub fn refresh_rhs_wo_bias(
+        &mut self,
+        builder: &TwoBodyConstraintBuilder,
+        poses: &Slice<Pose>,
+        params: &RbdSimParams,
+    ) {
+        let body1 = self.solver_body_a as usize;
+        let body2 = self.solver_body_b as usize;
+        let inv_dt_val = params.inv_dt();
+
+        let pose1 = poses[body1];
+        let pose2 = poses[body2];
+        let num_contacts = self.len as usize;
+
+        for j in 0..num_contacts {
+            let info = builder.infos.at(j);
+            let p1 = pose1 * info.local_pt_a;
+            let p2 = pose2 * info.local_pt_b;
+            let dist = info.dist + (p1 - p2).dot(self.dir_a);
+            self.elements.at_mut(j).normal_part.rhs_wo_bias =
+                info.normal_vel + dist.max(0.0) * inv_dt_val;
+        }
+    }
 }
 
 /// Applies warmstart impulses to a single body (gather-style, no graph coloring required).
@@ -538,137 +542,122 @@ impl TwoBodyConstraint {
         &mut self,
         solver_vel1: &mut Velocity,
         solver_vel2: &mut Velocity,
+        use_bias: bool,
     ) {
         let dir_a = self.dir_a;
         let friction_coeff = self.limit;
         let im_a = self.im_a;
         let im_b = self.im_b;
-        let cfm_factor = self.cfm_factor;
-
-        #[cfg(feature = "dim2")]
-        let tangent_a = Vec2::new(-dir_a.y, dir_a.x);
-        #[cfg(feature = "dim3")]
-        let tangent_a = self.tangent_a;
 
+        // Solve the normal parts of the constraint.
         for k in 0..(self.len as usize) {
-            // Solve the normal part of the constraint.
-            let limit = {
-                let c = &self.elements.at(k).normal_part;
-                // Copy values we need after the assignment
-                let ii_torque_dir_a = c.ii_torque_dir_a;
-                let ii_torque_dir_b = c.ii_torque_dir_b;
-
-                let dvel = dir_a.dot(solver_vel1.linear)
-                    + gdot(c.torque_dir_a, solver_vel1.angular)
-                    - dir_a.dot(solver_vel2.linear)
-                    + gdot(c.torque_dir_b, solver_vel2.angular)
-                    + c.rhs;
-                let new_impulse = cfm_factor * (c.impulse - c.r * dvel).max(0.0);
-                let delta_impulse = new_impulse - c.impulse;
-
-                self.elements.at_mut(k).normal_part.impulse = new_impulse;
+            let c = &self.elements.at(k).normal_part;
+            // Copy values we need after the assignment
+            let ii_torque_dir_a = c.ii_torque_dir_a;
+            let ii_torque_dir_b = c.ii_torque_dir_b;
+            let (rhs, cfm_factor) = if use_bias {
+                (c.rhs, c.cfm_factor)
+            } else {
+                (c.rhs_wo_bias, 1.0)
+            };
+            let dvel = dir_a.dot(solver_vel1.linear)
+                + gdot(c.torque_dir_a, solver_vel1.angular)
+                - dir_a.dot(solver_vel2.linear)
+                + gdot(c.torque_dir_b, solver_vel2.angular)
+                + rhs;
+            let new_impulse = cfm_factor * (c.impulse - c.r * dvel).max(0.0);
+            let delta_impulse = new_impulse - c.impulse;
 
-                solver_vel1.linear += dir_a * im_a * delta_impulse;
-                solver_vel1.angular += ii_torque_dir_a * delta_impulse;
+            self.elements.at_mut(k).normal_part.impulse = new_impulse;
 
-                solver_vel2.linear += dir_a * im_b * -delta_impulse;
-                solver_vel2.angular += ii_torque_dir_b * delta_impulse;
-                new_impulse * friction_coeff // Friction impulse limit.
-            };
+            solver_vel1.linear += dir_a * im_a * delta_impulse;
+            solver_vel1.angular += ii_torque_dir_a * delta_impulse;
 
-            // Solve the tangent parts of the constraint.
-            {
-                let c = &self.elements.at(k).tangent_part;
-                // Copy values we need after the assignment
-                let ii_torque_dir_a = c.ii_torque_dir_a;
-                let ii_torque_dir_b = c.ii_torque_dir_b;
+            solver_vel2.linear += dir_a * im_b * -delta_impulse;
+            solver_vel2.angular += ii_torque_dir_b * delta_impulse;
+        }
 
-                #[cfg(feature = "dim2")]
-                {
-                    let dvel = tangent_a.dot(solver_vel1.linear)
-                        + gdot(**c.torque_dir_a.at(0), solver_vel1.angular)
-                        - tangent_a.dot(solver_vel2.linear)
-                        + gdot(**c.torque_dir_b.at(0), solver_vel2.angular)
-                        + c.rhs.read(0);
-                    let new_impulse =
-                        cfm_factor * (c.impulse.read(0) - c.r.read(0) * dvel).clamp(-limit, limit);
-                    let delta_impulse = new_impulse - c.impulse.read(0);
+        // Friction is only solved during the stabilization sweep.
+        if use_bias {
+            return;
+        }
 
-                    self.elements
-                        .at_mut(k)
-                        .tangent_part
-                        .impulse
-                        .write(0, new_impulse);
+        #[cfg(feature = "dim2")]
+        let tangent_a = Vec2::new(-dir_a.y, dir_a.x);
+        #[cfg(feature = "dim3")]
+        let tangent_a = self.tangent_a;
 
-                    solver_vel1.linear += tangent_a * im_a * delta_impulse;
-                    solver_vel1.angular += **ii_torque_dir_a.at(0) * delta_impulse;
+        // Solve the tangent parts of the constraint.
+        for k in 0..(self.len as usize) {
+            let limit = friction_coeff * self.elements.at(k).normal_part.impulse;
+            let c = &self.elements.at(k).tangent_part;
+            // Copy values we need after the assignment
+            let ii_torque_dir_a = c.ii_torque_dir_a;
+            let ii_torque_dir_b = c.ii_torque_dir_b;
 
-                    solver_vel2.linear += tangent_a * im_b * -delta_impulse;
-                    solver_vel2.angular += **ii_torque_dir_b.at(0) * delta_impulse;
-                }
-                #[cfg(feature = "dim3")]
-                {
-                    let tangents_a = [tangent_a, dir_a.cross(tangent_a)];
-                    let dvel_0 = tangents_a.read(0).dot(solver_vel1.linear)
-                        + gdot(**c.torque_dir_a.at(0), solver_vel1.angular)
-                        - tangents_a.read(0).dot(solver_vel2.linear)
-                        + gdot(**c.torque_dir_b.at(0), solver_vel2.angular)
-                        + c.rhs.read(0);
-                    let dvel_1 = tangents_a.read(1).dot(solver_vel1.linear)
-                        + gdot(**c.torque_dir_a.at(1), solver_vel1.angular)
-                        - tangents_a.read(1).dot(solver_vel2.linear)
-                        + gdot(**c.torque_dir_b.at(1), solver_vel2.angular)
-                        + c.rhs.read(1);
-
-                    let dvel_00 = dvel_0 * dvel_0;
-                    let dvel_11 = dvel_1 * dvel_1;
-                    let dvel_01 = dvel_0 * dvel_1;
-                    let inv_lhs = (dvel_00 + dvel_11)
-                        * maybe_inv(
-                            dvel_00 * c.r.read(0) + dvel_11 * c.r.read(1) + dvel_01 * c.r.read(2),
-                        );
-                    let delta_impulse = Vec2::new(inv_lhs * dvel_0, inv_lhs * dvel_1);
-                    let mut new_impulse = c.impulse - delta_impulse;
-                    new_impulse = cap_magnitude(new_impulse, limit);
-
-                    let delta_impulse = new_impulse - c.impulse;
-                    self.elements.at_mut(k).tangent_part.impulse = new_impulse;
-
-                    solver_vel1.linear += (tangents_a.read(0) * delta_impulse.x
-                        + tangents_a.read(1) * delta_impulse.y)
-                        * im_a;
-                    solver_vel1.angular += **ii_torque_dir_a.at(0) * delta_impulse.x
-                        + **ii_torque_dir_a.at(1) * delta_impulse.y;
+            #[cfg(feature = "dim2")]
+            {
+                let dvel = tangent_a.dot(solver_vel1.linear)
+                    + gdot(**c.torque_dir_a.at(0), solver_vel1.angular)
+                    - tangent_a.dot(solver_vel2.linear)
+                    + gdot(**c.torque_dir_b.at(0), solver_vel2.angular)
+                    + c.rhs_wo_bias.read(0);
+                let new_impulse = (c.impulse.read(0) - c.r.read(0) * dvel).clamp(-limit, limit);
+                let delta_impulse = new_impulse - c.impulse.read(0);
+
+                self.elements
+                    .at_mut(k)
+                    .tangent_part
+                    .impulse
+                    .write(0, new_impulse);
+
+                solver_vel1.linear += tangent_a * im_a * delta_impulse;
+                solver_vel1.angular += **ii_torque_dir_a.at(0) * delta_impulse;
+
+                solver_vel2.linear += tangent_a * im_b * -delta_impulse;
+                solver_vel2.angular += **ii_torque_dir_b.at(0) * delta_impulse;
+            }
+            #[cfg(feature = "dim3")]
+            {
+                let tangents_a = [tangent_a, dir_a.cross(tangent_a)];
+                let dvel_0 = tangents_a.read(0).dot(solver_vel1.linear)
+                    + gdot(**c.torque_dir_a.at(0), solver_vel1.angular)
+                    - tangents_a.read(0).dot(solver_vel2.linear)
+                    + gdot(**c.torque_dir_b.at(0), solver_vel2.angular)
+                    + c.rhs_wo_bias.read(0);
+                let dvel_1 = tangents_a.read(1).dot(solver_vel1.linear)
+                    + gdot(**c.torque_dir_a.at(1), solver_vel1.angular)
+                    - tangents_a.read(1).dot(solver_vel2.linear)
+                    + gdot(**c.torque_dir_b.at(1), solver_vel2.angular)
+                    + c.rhs_wo_bias.read(1);
+
+                let k11 = c.r.read(0);
+                let k22 = c.r.read(1);
+                let k12 = c.r.read(2) * 0.5;
+                let inv_det = maybe_inv(k11 * k22 - k12 * k12);
+                let delta_impulse = Vec2::new(
+                    (k22 * dvel_0 - k12 * dvel_1) * inv_det,
+                    (k11 * dvel_1 - k12 * dvel_0) * inv_det,
+                );
+                let mut new_impulse = c.impulse - delta_impulse;
+                new_impulse = cap_magnitude(new_impulse, limit);
 
-                    solver_vel2.linear += (tangents_a.read(0) * -delta_impulse.x
-                        + tangents_a.read(1) * -delta_impulse.y)
-                        * im_b;
-                    solver_vel2.angular += **ii_torque_dir_b.at(0) * delta_impulse.x
-                        + **ii_torque_dir_b.at(1) * delta_impulse.y;
-                }
+                let delta_impulse = new_impulse - c.impulse;
+                self.elements.at_mut(k).tangent_part.impulse = new_impulse;
+
+                solver_vel1.linear += (tangents_a.read(0) * delta_impulse.x
+                    + tangents_a.read(1) * delta_impulse.y)
+                    * im_a;
+                solver_vel1.angular += **ii_torque_dir_a.at(0) * delta_impulse.x
+                    + **ii_torque_dir_a.at(1) * delta_impulse.y;
+
+                solver_vel2.linear += (tangents_a.read(0) * -delta_impulse.x
+                    + tangents_a.read(1) * -delta_impulse.y)
+                    * im_b;
+                solver_vel2.angular += **ii_torque_dir_b.at(0) * delta_impulse.x
+                    + **ii_torque_dir_b.at(1) * delta_impulse.y;
             }
         }
     }
 }
 
-impl TwoBodyConstraint {
-    /// Removes CFM and bias from constraints for the final substep iteration.
-    #[cfg(feature = "dim2")]
-    #[inline(always)]
-    pub fn remove_cfm_and_bias(&mut self) {
-        self.elements.at_mut(0).normal_part.rhs = self.elements.at(0).normal_part.rhs_wo_bias;
-        self.elements.at_mut(1).normal_part.rhs = self.elements.at(1).normal_part.rhs_wo_bias;
-        self.cfm_factor = 1.0;
-    }
-
-    /// Removes CFM and bias from constraints for the final substep iteration.
-    #[cfg(feature = "dim3")]
-    #[inline(always)]
-    pub fn remove_cfm_and_bias(&mut self) {
-        self.elements.at_mut(0).normal_part.rhs = self.elements.at(0).normal_part.rhs_wo_bias;
-        self.elements.at_mut(1).normal_part.rhs = self.elements.at(1).normal_part.rhs_wo_bias;
-        self.elements.at_mut(2).normal_part.rhs = self.elements.at(2).normal_part.rhs_wo_bias;
-        self.elements.at_mut(3).normal_part.rhs = self.elements.at(3).normal_part.rhs_wo_bias;
-        self.cfm_factor = 1.0;
-    }
-}
diff --git a/src_rbd_shaders/dynamics/warmstart.rs b/src_rbd_shaders/dynamics/warmstart.rs
index 6e68659..512eaf4 100644
--- a/src_rbd_shaders/dynamics/warmstart.rs
+++ b/src_rbd_shaders/dynamics/warmstart.rs
@@ -56,6 +56,88 @@ pub fn gpu_transfer_warmstart_impulses(
     }
 }
 
+/// Seeds the topo-gc coloring from the previous frame's colors (same
+/// body-pair matching as the warmstart impulse transfer), between the
+/// topo-gc reset and its iterations.
+///
+/// Contacts persist across frames, so most constraints can reuse last
+/// frame's color and the iterations only color the genuinely new ones; the
+/// fix-conflicts pass still validates every seed, so a stale seed is simply
+/// uncolored and recomputed.
+#[spirv_bindgen]
+#[spirv(compute(threads(64)))]
+pub fn gpu_seed_colors_from_warmstart(
+    #[spirv(global_invocation_id)] invocation_id: UVec3,
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 0)] old_body_constraint_counts: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 1)] old_body_constraint_ids: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 2)]
+    old_constraints: &[TwoBodyConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 3)] new_constraints: &[TwoBodyConstraint],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 4)] old_constraints_colors: &[u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 5)] constraints_colors: &mut [u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 6)] colored: &mut [u32],
+    #[spirv(storage_buffer, descriptor_set = 0, binding = 7)] contacts_len: &[u32],
+    #[spirv(uniform, descriptor_set = 0, binding = 8)] batch_ids: &BatchIndices,
+) {
+    let batch_id = invocation_id.y;
+    let contacts_start = batch_ids.contacts_start(batch_id);
+    let colliders_start = batch_ids.coll_start(batch_id);
+    let bci_start = batch_id as usize * 2 * batch_ids.contacts_batch_capacity as usize;
+
+    let old_body_constraint_counts = Slice(old_body_constraint_counts, colliders_start);
+    let old_body_constraint_ids = Slice(old_body_constraint_ids, bci_start);
+    let old_constraints = Slice(old_constraints, contacts_start);
+    let old_constraints_colors = Slice(old_constraints_colors, contacts_start);
+    let mut constraints_colors = SliceMut(constraints_colors, contacts_start);
+    let mut colored = SliceMut(colored, contacts_start);
+    let new_constraints = Slice(new_constraints, contacts_start);
+
+    let len = contacts_len.read(batch_id as usize);
+    let i = invocation_id.x as usize;
+
+    if (i as u32) < len {
+        let body_a = new_constraints[i].solver_body_a;
+        let body_b = new_constraints[i].solver_body_b;
+
+        let first_a = if body_a != 0 {
+            old_body_constraint_counts[body_a as usize - 1] as usize
+        } else {
+            0
+        };
+        let last_a = old_body_constraint_counts[body_a as usize] as usize;
+        let first_b = if body_b != 0 {
+            old_body_constraint_counts[body_b as usize - 1] as usize
+        } else {
+            0
+        };
+        let last_b = old_body_constraint_counts[body_b as usize] as usize;
+
+        let len_a = last_a - first_a;
+        let len_b = last_b - first_b;
+        let (first_ref, last_ref) = if len_a != 0 && len_a < len_b {
+            (first_a, last_a)
+        } else {
+            (first_b, last_b)
+        };
+
+        for j in first_ref..last_ref {
+            let cid_old = old_body_constraint_ids[j] as usize;
+            if old_constraints[cid_old].solver_body_a == body_a
+                && old_constraints[cid_old].solver_body_b == body_b
+            {
+                let old_color = old_constraints_colors[cid_old];
+                // Colors 1..64 are the valid topo-gc range; anything else
+                // (e.g. stale data after a buffer resize) stays uncolored.
+                if old_color > 0 && old_color < 64 {
+                    constraints_colors[i] = old_color;
+                    colored[i] = 1;
+                }
+                break;
+            }
+        }
+    }
+}
+
 /// Transfers warmstart impulses from previous frame to current frame.
 ///
 /// NOTE: this assumes that the solver body ids in the constraints match the index of the body itself.
@@ -141,8 +223,6 @@ pub fn transfer_warmstart_impulses(
                         // Contact point match found! Transfer the accumulated impulse.
                         // The impulse field contains the last substep's impulse, which serves
                         // as the warmstart value for this frame.
-                        // NOTE: we sum the impulse + impulse_accumulator since the accumulator contains the
-                        //       accumulated impulse for all the substeps except the last one.
                         // TODO: what if we have multiple matches? (currently uses first match)
                         new_constraints[i]
                             .elements
@@ -153,15 +233,42 @@ pub fn transfer_warmstart_impulses(
                             .at(k_old)
                             .normal_part
                             .impulse;
-                        new_constraints[i]
-                            .elements
-                            .at_mut(k_new)
-                            .tangent_part
-                            .impulse = old_constraints[cid_old]
-                            .elements
-                            .at(k_old)
-                            .tangent_part
-                            .impulse;
+
+                        // 2D: the tangent is derived deterministically from the
+                        // normal, so the scalar component carries over as-is.
+                        #[cfg(feature = "dim2")]
+                        {
+                            new_constraints[i]
+                                .elements
+                                .at_mut(k_new)
+                                .tangent_part
+                                .impulse = old_constraints[cid_old]
+                                .elements
+                                .at(k_old)
+                                .tangent_part
+                                .impulse;
+                        }
+
+                        // 3D: reproject the friction impulse through world space
+                        // onto the new constraint's tangent basis.
+                        #[cfg(feature = "dim3")]
+                        {
+                            let old_c = &old_constraints[cid_old];
+                            let old_t0 = old_c.tangent_a;
+                            let old_t1 = old_c.dir_a.cross(old_t0);
+                            let old_impulse =
+                                old_c.elements.at(k_old).tangent_part.impulse;
+                            let world = old_t0 * old_impulse.x + old_t1 * old_impulse.y;
+
+                            let new_t0 = new_constraints[i].tangent_a;
+                            let new_t1 = new_constraints[i].dir_a.cross(new_t0);
+                            new_constraints[i]
+                                .elements
+                                .at_mut(k_new)
+                                .tangent_part
+                                .impulse =
+                                khal_std::glamx::Vec2::new(world.dot(new_t0), world.dot(new_t1));
+                        }
                     }
                 }
             }
diff --git a/src_rbd_shaders/queries/contact.rs b/src_rbd_shaders/queries/contact.rs
index aa4aa0f..26b8678 100644
--- a/src_rbd_shaders/queries/contact.rs
+++ b/src_rbd_shaders/queries/contact.rs
@@ -226,17 +226,17 @@ pub fn cuboid_cuboid(
      */
     let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid1, cuboid2, pose12);
 
-    // TODO PERF: support the prediction early-exit.
-    // if sep1.separation > prediction {
-    //     return ContactManifold::default();
-    // }
+    // Early-exit: any contact point's distance is >= the separation along a
+    // separating axis, so the caller would drop the manifold anyway.
+    if sep1.separation > prediction {
+        return ContactManifold::default();
+    }
 
     let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cuboid2, cuboid1, pose21);
 
-    // TODO PERF: support the prediction early-exit.
-    // if sep2.separation > prediction {
-    //     return ContactManifold::default();
-    // }
+    if sep2.separation > prediction {
+        return ContactManifold::default();
+    }
 
     /*
      *
@@ -248,10 +248,9 @@ pub fn cuboid_cuboid(
     #[cfg(feature = "dim3")]
     let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cuboid1, cuboid2, pose12);
 
-    // TODO PERF: support the prediction early-exit.
-    // if sep3.separation > prediction {
-    //     return ContactManifold::default();
-    // }
+    if sep3.separation > prediction {
+        return ContactManifold::default();
+    }
 
     /*
      *
diff --git a/src_rbd_shaders/queries/sat.rs b/src_rbd_shaders/queries/sat.rs
index bcbbea6..c018ab7 100644
--- a/src_rbd_shaders/queries/sat.rs
+++ b/src_rbd_shaders/queries/sat.rs
@@ -40,6 +40,7 @@ pub const EPSILON: f32 = 1.1920929E-7;
 
 #[cfg(feature = "dim3")]
 /// Computes the separation of two cuboids along `axis1`.
+#[inline(always)]
 pub fn cuboid_cuboid_compute_separation_wrt_local_line(
     cuboid1: &Cuboid,
     cuboid2: &Cuboid,
@@ -65,6 +66,7 @@ pub fn cuboid_cuboid_compute_separation_wrt_local_line(
 ///
 /// All combinations of edges from both cuboids are taken into
 /// account.
+#[inline(always)]
 pub fn cuboid_cuboid_find_local_separating_edge_twoway(
     cuboid1: &Cuboid,
     cuboid2: &Cuboid,
@@ -116,6 +118,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway(
 /// Finds the best separating normal between two cuboids.
 ///
 /// Only the normals from `cuboid1` are tested.
+#[inline(always)]
 pub fn cuboid_cuboid_find_local_separating_normal_oneway(
     cuboid1: &Cuboid,
     cuboid2: &Cuboid,
diff --git a/src_rbd_shaders/utils/indices.rs b/src_rbd_shaders/utils/indices.rs
index df22253..bfd81a6 100644
--- a/src_rbd_shaders/utils/indices.rs
+++ b/src_rbd_shaders/utils/indices.rs
@@ -1,4 +1,5 @@
-use crate::utils::{Slice, SliceMut};
+use crate::utils::linalg::{MatSlice, VSlice};
+use crate::utils::{ISlice, ISliceMut, Slice, SliceMut};
 
 /// Per-batch capacities and packed-buffer section offsets, shared by every
 /// kernel that needs to slice a flat tensor into its batch's slot.
@@ -9,6 +10,9 @@ use crate::utils::{Slice, SliceMut};
 #[cfg_attr(not(target_arch_is_gpu), derive(bytemuck::Pod, bytemuck::Zeroable))]
 #[repr(C)]
 pub struct BatchIndices {
+    /// Total number of simulation batches (environments).
+    pub num_batches: u32,
+
     /*
      * RBD / collision-detection capacities.
      */
@@ -53,6 +57,18 @@ pub struct BatchIndices {
     /// contrast, are stored single-batch (identical coloring across batches)
     /// and read at offset 0.
     pub mb_imp_joint_color_groups_batch_capacity: u32,
+    /// Actual max `ndofs` across every multibody in every batch (often smaller
+    /// than the fixed `MAX_MB_DOFS` limit).
+    pub mb_max_ndofs: u32,
+    /// Actual max link count across every multibody in every batch.
+    pub mb_max_links: u32,
+    /// Lanes per multibody for the packed per-multibody workgroup kernels:
+    /// `next_power_of_two(mb_max_ndofs).clamp(8, 64)`.
+    pub mb_pack_lanes: u32,
+    /// Per-batch stride of the contact-solver color-bucket buffers
+    /// (`color_counts` / `color_starts` / `color_cursors`), = `max_colors + 3`
+    /// so that `starts[c + 1]` is in bounds for every swept color.
+    pub solver_color_buckets_stride: u32,
 
     /*
      * Intra-batch offsets for multi-purpose buffers.
@@ -62,6 +78,14 @@ pub struct BatchIndices {
     pub coriolis_w_section_offset: u32,
     pub i_coriolis_dt_section_offset: u32,
     pub dof_damping_section_offset: u32,
+    /// Offset (in f32 entries, within a batch's `mass_matrices` view) of the
+    /// section holding the coriolis-aware "acceleration" mass matrix
+    /// (rapier's `acc_augmented_mass`).
+    /// Non-zero = split-matrix.
+    /// Zero = single melded matrix for both (not recommended).
+    pub mass_matrix_acc_section_offset: u32,
+    /// Per-batch stride (capacity) of the multibody DoF-coupling buffer.
+    pub mb_dof_couplings_batch_capacity: u32,
 }
 
 impl BatchIndices {
@@ -76,54 +100,60 @@ impl BatchIndices {
         batch_id as usize * self.colliders_batch_capacity as usize
     }
 
+    /// Interleaved flat index for the multibody dynamics buffers.
     #[inline]
-    pub fn collision_pairs_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.collision_pairs_batch_capacity as usize
-    }
-
-    #[inline]
-    pub fn contacts_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.contacts_batch_capacity as usize
-    }
-
-    #[inline]
-    pub fn impulse_joints_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.impulse_joints_batch_capacity as usize
+    pub fn mbi(&self, batch_id: u32, intra: usize) -> usize {
+        intra * self.num_batches as usize + batch_id as usize
     }
 
+    /// Interleaved view of a multibody dynamics buffer for batch `batch_id`
+    /// (use `.offset(...)` for the intra-batch element offset).
     #[inline]
-    pub fn mb_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.multibodies_batch_capacity as usize
+    pub fn ib<'s, T>(&self, batch_id: u32, slice: &'s [T]) -> ISlice<'s, T> {
+        ISlice {
+            buf: slice,
+            base: 0,
+            stride: self.num_batches,
+            shift: batch_id,
+        }
     }
 
+    /// Mutable interleaved view.
     #[inline]
-    pub fn links_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.links_batch_capacity as usize
+    pub fn ib_mut<'s, T>(&self, batch_id: u32, slice: &'s mut [T]) -> ISliceMut<'s, T> {
+        ISliceMut {
+            buf: slice,
+            base: 0,
+            stride: self.num_batches,
+            shift: batch_id,
+        }
     }
 
+    /// Interleaved dense matrix view at intra-batch element offset `offset`.
     #[inline]
-    pub fn jac_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.jacobians_batch_capacity as usize
+    pub fn imat(&self, batch_id: u32, offset: usize, rows: u32, cols: u32) -> MatSlice {
+        MatSlice::interleaved(offset, rows, cols, self.num_batches, batch_id)
     }
 
+    /// Interleaved vector view at intra-batch element offset `offset`.
     #[inline]
-    pub fn mm_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.mass_matrix_batch_capacity as usize
+    pub fn ivec(&self, batch_id: u32, offset: usize) -> VSlice {
+        VSlice::interleaved(offset, self.num_batches, batch_id)
     }
 
     #[inline]
-    pub fn cor_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.coriolis_batch_capacity as usize
+    pub fn collision_pairs_start(&self, batch_id: u32) -> usize {
+        batch_id as usize * self.collision_pairs_batch_capacity as usize
     }
 
     #[inline]
-    pub fn icdt_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.i_coriolis_dt_batch_capacity as usize
+    pub fn contacts_start(&self, batch_id: u32) -> usize {
+        batch_id as usize * self.contacts_batch_capacity as usize
     }
 
     #[inline]
-    pub fn dof_start(&self, batch_id: u32) -> usize {
-        batch_id as usize * self.dof_batch_capacity as usize
+    pub fn impulse_joints_start(&self, batch_id: u32) -> usize {
+        batch_id as usize * self.impulse_joints_batch_capacity as usize
     }
 
     #[inline]
@@ -146,6 +176,11 @@ impl BatchIndices {
         batch_id as usize * self.mb_contact_constraint_columns_batch_capacity as usize
     }
 
+    #[inline]
+    pub fn mb_dof_couplings_start(&self, batch_id: u32) -> usize {
+        batch_id as usize * self.mb_dof_couplings_batch_capacity as usize
+    }
+
     #[inline]
     pub fn mb_imp_joints_start(&self, batch_id: u32) -> usize {
         batch_id as usize * self.mb_imp_joints_batch_capacity as usize
@@ -218,36 +253,6 @@ impl BatchIndices {
         SliceMut(slice, self.impulse_joints_start(batch_id))
     }
 
-    #[inline]
-    pub fn mb_batch<'s, T>(&self, batch_id: u32, slice: &'s [T]) -> Slice<'s, T> {
-        Slice(slice, self.mb_start(batch_id))
-    }
-
-    #[inline]
-    pub fn mb_batch_mut<'s, T>(&self, batch_id: u32, slice: &'s mut [T]) -> SliceMut<'s, T> {
-        SliceMut(slice, self.mb_start(batch_id))
-    }
-
-    #[inline]
-    pub fn mb_links_batch<'s, T>(&self, batch_id: u32, slice: &'s [T]) -> Slice<'s, T> {
-        Slice(slice, self.links_start(batch_id))
-    }
-
-    #[inline]
-    pub fn mb_links_batch_mut<'s, T>(&self, batch_id: u32, slice: &'s mut [T]) -> SliceMut<'s, T> {
-        SliceMut(slice, self.links_start(batch_id))
-    }
-
-    #[inline]
-    pub fn dof_batch<'s, T>(&self, batch_id: u32, slice: &'s [T]) -> Slice<'s, T> {
-        Slice(slice, self.dof_start(batch_id))
-    }
-
-    #[inline]
-    pub fn dof_batch_mut<'s, T>(&self, batch_id: u32, slice: &'s mut [T]) -> SliceMut<'s, T> {
-        SliceMut(slice, self.dof_start(batch_id))
-    }
-
     #[inline]
     pub fn mb_joint_constraints_batch<'s, T>(&self, batch_id: u32, slice: &'s [T]) -> Slice<'s, T> {
         Slice(slice, self.mb_joint_constraints_start(batch_id))
diff --git a/src_rbd_shaders/utils/linalg.rs b/src_rbd_shaders/utils/linalg.rs
index 7918699..ef8133f 100644
--- a/src_rbd_shaders/utils/linalg.rs
+++ b/src_rbd_shaders/utils/linalg.rs
@@ -22,14 +22,22 @@ pub const MAX_MB_DOFS: usize = 64;
 /// A column-major matrix view into a flat f32 buffer.
 #[derive(Copy, Clone)]
 pub struct MatSlice {
-    /// Offset (in f32 entries) of the (0, 0) element inside the backing buffer.
+    /// Offset (in element entries, multiplied by `stride`) of the (0, 0)
+    /// element inside the backing buffer.
     pub offset: usize,
     /// Number of rows.
     pub rows: u32,
     /// Number of columns.
     pub cols: u32,
-    /// Leading dimension — distance between columns, in f32 entries.
+    /// Leading dimension: distance between columns, in element entries.
     pub lead: u32,
+    /// Element stride: flat index = `(offset + c·lead + r) · stride + shift`.
+    /// `1` for plain dense storage; `num_batches` for the batch-interleaved
+    /// multibody dynamics buffers (with `shift = batch_id`).
+    pub stride: u32,
+    /// Additive shift applied after the stride (the batch id for
+    /// batch-interleaved buffers).
+    pub shift: u32,
 }
 
 impl MatSlice {
@@ -41,16 +49,34 @@ impl MatSlice {
             rows,
             cols,
             lead: rows,
+            stride: 1,
+            shift: 0,
+        }
+    }
+
+    /// Batch-interleaved dense view: element `k` (intra-batch offset
+    /// `offset + k`) of batch `shift` lives at `(offset + k) · stride +
+    /// shift`.
+    #[inline]
+    pub fn interleaved(offset: usize, rows: u32, cols: u32, stride: u32, shift: u32) -> Self {
+        Self {
+            offset,
+            rows,
+            cols,
+            lead: rows,
+            stride,
+            shift,
         }
     }
 
     /// Flat index of element `(r, c)`.
     #[inline]
     pub fn idx(&self, r: u32, c: u32) -> usize {
-        self.offset + (c * self.lead + r) as usize
+        (self.offset + (c * self.lead + r) as usize) * self.stride as usize + self.shift as usize
     }
 
-    /// Sub-view starting at `(r0, c0)` with shape `(nr × nc)`. Inherits `lead`.
+    /// Sub-view starting at `(r0, c0)` with shape `(nr × nc)`. Inherits
+    /// `lead`, `stride` and `shift`.
     #[inline]
     pub fn view(&self, r0: u32, c0: u32, nr: u32, nc: u32) -> Self {
         Self {
@@ -58,6 +84,8 @@ impl MatSlice {
             rows: nr,
             cols: nc,
             lead: self.lead,
+            stride: self.stride,
+            shift: self.shift,
         }
     }
 
@@ -80,6 +108,51 @@ impl MatSlice {
     }
 }
 
+/// Strided vector view: element `i` lives at `(offset + i) · stride + shift`.
+/// The vector counterpart of [`MatSlice`], used for the LU pivot / rhs
+/// buffers which may be batch-interleaved (`stride = num_batches`, `shift =
+/// batch_id`) or plain dense (`stride = 1, shift = 0`).
+#[derive(Copy, Clone)]
+pub struct VSlice {
+    pub offset: usize,
+    pub stride: u32,
+    pub shift: u32,
+}
+
+impl VSlice {
+    /// Plain dense view at `offset`.
+    #[inline]
+    pub fn dense(offset: usize) -> Self {
+        Self {
+            offset,
+            stride: 1,
+            shift: 0,
+        }
+    }
+
+    /// Batch-interleaved view.
+    #[inline]
+    pub fn interleaved(offset: usize, stride: u32, shift: u32) -> Self {
+        Self {
+            offset,
+            stride,
+            shift,
+        }
+    }
+
+    /// Flat index of element `i`.
+    #[inline]
+    pub fn at(&self, i: u32) -> usize {
+        (self.offset + i as usize) * self.stride as usize + self.shift as usize
+    }
+
+    /// Flat index of element `i` (usize form).
+    #[inline]
+    pub fn atz(&self, i: usize) -> usize {
+        (self.offset + i) * self.stride as usize + self.shift as usize
+    }
+}
+
 /// `m := val` (element-wise).
 #[inline]
 pub fn fill(buf: &mut [f32], m: MatSlice, val: f32) {
@@ -136,8 +209,31 @@ pub fn gemm_mat3_lhs(buf: &mut [f32], c: MatSlice, alpha: f32, a: Mat3, b: MatSl
     }
 }
 
-/// `dst += alpha * src`. `src` lives in a separate buffer (stack scratch or another tensor).
+/// Lane-parallel `dst += alpha * src` over two disjoint views of the same
+/// buffer: each lane owns the columns `lane, lane + lanes, …`.
 #[inline]
+pub fn axpy_mat_par(
+    buf: &mut [f32],
+    dst: MatSlice,
+    alpha: f32,
+    src: MatSlice,
+    lane: u32,
+    lanes: u32,
+) {
+    let mut c = lane;
+    // NOTE: `for`-style loop (rust-gpu miscompiles `while`).
+    for _ in 0..dst.cols.div_ceil(lanes) {
+        if c < dst.cols {
+            for r in 0..dst.rows {
+                let cur = buf.read(dst.idx(r, c));
+                let s = buf.read(src.idx(r, c));
+                buf.write(dst.idx(r, c), cur + alpha * s);
+            }
+        }
+        c += lanes;
+    }
+}
+
 pub fn axpy_mat(buf_dst: &mut [f32], dst: MatSlice, alpha: f32, buf_src: &[f32], src: MatSlice) {
     for c in 0..dst.cols {
         for r in 0..dst.rows {
@@ -266,7 +362,7 @@ pub fn gemv_tr_spatial<const XDIM: usize>(
 #[inline]
 pub fn gemv_tr_spatial_split(
     buf_y: &mut [f32],
-    y_offset: usize,
+    y: VSlice,
     alpha: f32,
     buf_a: &[f32],
     a: MatSlice,
@@ -281,7 +377,7 @@ pub fn gemv_tr_spatial_split(
             + buf_a.read(a.idx(3, c)) * x_ang.x
             + buf_a.read(a.idx(4, c)) * x_ang.y
             + buf_a.read(a.idx(5, c)) * x_ang.z;
-        let idx = y_offset + c as usize;
+        let idx = y.at(c);
         let cur = buf_y.read(idx);
         buf_y.write(idx, beta * cur + alpha * s);
     }
@@ -291,7 +387,7 @@ pub fn gemv_tr_spatial_split(
 #[inline]
 pub fn gemv_tr_spatial_split(
     buf_y: &mut [f32],
-    y_offset: usize,
+    y: VSlice,
     alpha: f32,
     buf_a: &[f32],
     a: MatSlice,
@@ -303,7 +399,7 @@ pub fn gemv_tr_spatial_split(
         let s = buf_a.read(a.idx(0, c)) * x_lin.x
             + buf_a.read(a.idx(1, c)) * x_lin.y
             + buf_a.read(a.idx(2, c)) * x_ang;
-        let idx = y_offset + c as usize;
+        let idx = y.at(c);
         let cur = buf_y.read(idx);
         buf_y.write(idx, beta * cur + alpha * s);
     }
@@ -622,7 +718,7 @@ pub fn gemm_tr(
 /// `pivots[k]` is the row that was swapped with row `k` during elimination step
 /// `k`. `pivots_offset` is where this multibody's pivot slot starts in `buf_pivots`.
 #[inline]
-pub fn lu_decompose(buf_m: &mut [f32], m: MatSlice, buf_pivots: &mut [u32], pivots_offset: usize) {
+pub fn lu_decompose(buf_m: &mut [f32], m: MatSlice, buf_pivots: &mut [u32], piv: VSlice) {
     let n = m.rows;
     for k in 0..n {
         // Partial pivot: find max |M[i, k]| for i in k..n.
@@ -639,7 +735,7 @@ pub fn lu_decompose(buf_m: &mut [f32], m: MatSlice, buf_pivots: &mut [u32], pivo
                 pivot_row = i;
             }
         }
-        buf_pivots.write(pivots_offset + k as usize, pivot_row);
+        buf_pivots.write(piv.at(k), pivot_row);
 
         // Row swap k ↔ pivot_row (full row since we haven't computed past col k).
         if pivot_row != k {
@@ -681,18 +777,18 @@ pub fn lu_solve_in_place(
     buf_m: &[f32],
     m: MatSlice,
     buf_pivots: &[u32],
-    pivots_offset: usize,
+    piv: VSlice,
     buf_rhs: &mut [f32],
-    rhs_offset: usize,
+    rhs: VSlice,
 ) {
     let n = m.rows;
 
     // Permute rhs in place according to the recorded pivots.
     for k in 0..n {
-        let p = buf_pivots.read(pivots_offset + k as usize);
+        let p = buf_pivots.read(piv.at(k));
         if p != k {
-            let ki = rhs_offset + k as usize;
-            let pi = rhs_offset + p as usize;
+            let ki = rhs.at(k);
+            let pi = rhs.at(p);
             let a = buf_rhs.read(ki);
             let b = buf_rhs.read(pi);
             buf_rhs.write(ki, b);
@@ -702,22 +798,22 @@ pub fn lu_solve_in_place(
 
     // Forward substitution: L · y = P · rhs (L is unit-lower — implicit diag = 1).
     for i in 0..n {
-        let mut s = buf_rhs.read(rhs_offset + i as usize);
+        let mut s = buf_rhs.read(rhs.at(i));
         for j in 0..i {
-            s -= buf_m.read(m.idx(i, j)) * buf_rhs.read(rhs_offset + j as usize);
+            s -= buf_m.read(m.idx(i, j)) * buf_rhs.read(rhs.at(j));
         }
-        buf_rhs.write(rhs_offset + i as usize, s);
+        buf_rhs.write(rhs.at(i), s);
     }
 
     // Back substitution: U · x = y (reverse iteration — equivalent to `for ii in (0..n).rev()`).
     for step in 0..n {
         let ii = n - 1 - step;
-        let mut s = buf_rhs.read(rhs_offset + ii as usize);
+        let mut s = buf_rhs.read(rhs.at(ii));
         for j in (ii + 1)..n {
-            s -= buf_m.read(m.idx(ii, j)) * buf_rhs.read(rhs_offset + j as usize);
+            s -= buf_m.read(m.idx(ii, j)) * buf_rhs.read(rhs.at(j));
         }
         let u = buf_m.read(m.idx(ii, ii));
-        buf_rhs.write(rhs_offset + ii as usize, if u != 0.0 { s / u } else { 0.0 });
+        buf_rhs.write(rhs.at(ii), if u != 0.0 { s / u } else { 0.0 });
     }
 }
 
@@ -1189,7 +1285,7 @@ pub fn gemm_omega_skew_tr_cross_buf_par(
 #[inline]
 pub fn gemv_tr_spatial_split_par(
     buf_y: &mut [f32],
-    y_offset: usize,
+    y: VSlice,
     alpha: f32,
     buf_a: &[f32],
     a: MatSlice,
@@ -1207,7 +1303,7 @@ pub fn gemv_tr_spatial_split_par(
             + buf_a.read(a.idx(3, c)) * x_ang.x
             + buf_a.read(a.idx(4, c)) * x_ang.y
             + buf_a.read(a.idx(5, c)) * x_ang.z;
-        let idx = y_offset + c as usize;
+        let idx = y.at(c);
         let cur = buf_y.read(idx);
         buf_y.write(idx, beta * cur + alpha * s);
     }
@@ -1217,7 +1313,7 @@ pub fn gemv_tr_spatial_split_par(
 #[inline]
 pub fn gemv_tr_spatial_split_par(
     buf_y: &mut [f32],
-    y_offset: usize,
+    y: VSlice,
     alpha: f32,
     buf_a: &[f32],
     a: MatSlice,
@@ -1232,7 +1328,7 @@ pub fn gemv_tr_spatial_split_par(
         let s = buf_a.read(a.idx(0, c)) * x_lin.x
             + buf_a.read(a.idx(1, c)) * x_lin.y
             + buf_a.read(a.idx(2, c)) * x_ang;
-        let idx = y_offset + c as usize;
+        let idx = y.at(c);
         let cur = buf_y.read(idx);
         buf_y.write(idx, beta * cur + alpha * s);
     }
diff --git a/src_rbd_shaders/utils/mod.rs b/src_rbd_shaders/utils/mod.rs
index e28d2f8..a9916b5 100644
--- a/src_rbd_shaders/utils/mod.rs
+++ b/src_rbd_shaders/utils/mod.rs
@@ -9,7 +9,7 @@ mod slice;
 
 pub use basis::orthonormal_basis3;
 pub use indices::BatchIndices;
-pub use slice::{Slice, SliceMut};
+pub use slice::{ISlice, ISliceMut, Slice, SliceMut};
 
 /// Division with ceiling (signed).
 pub fn div_ceil(x: i32, y: i32) -> i32 {
diff --git a/src_rbd_shaders/utils/slice.rs b/src_rbd_shaders/utils/slice.rs
index 4c0b2cd..121e226 100644
--- a/src_rbd_shaders/utils/slice.rs
+++ b/src_rbd_shaders/utils/slice.rs
@@ -80,3 +80,115 @@ impl<T: Copy> IndexMut<usize> for SliceMut<'_, T> {
         self.0.at_mut(self.1 + i)
     }
 }
+
+// Batch-interleaved slice: element `i` of the view lives at
+// `(base + i) · stride + shift` in the backing buffer.
+pub struct ISlice<'a, T> {
+    pub buf: &'a [T],
+    pub base: usize,
+    pub stride: u32,
+    pub shift: u32,
+}
+
+impl<'a, T: Copy> ISlice<'a, T> {
+    #[inline(always)]
+    fn flat(&self, i: usize) -> usize {
+        (self.base + i) * self.stride as usize + self.shift as usize
+    }
+
+    #[inline]
+    pub fn at(&self, i: usize) -> &'a T {
+        self.buf.at(self.flat(i))
+    }
+
+    #[inline]
+    pub fn read(&self, i: usize) -> T {
+        self.buf.read(self.flat(i))
+    }
+
+    #[inline]
+    pub fn offset(self, offset: usize) -> Self {
+        ISlice {
+            base: self.base + offset,
+            ..self
+        }
+    }
+}
+
+pub struct ISliceMut<'a, T> {
+    pub buf: &'a mut [T],
+    pub base: usize,
+    pub stride: u32,
+    pub shift: u32,
+}
+
+impl<'a, T: Copy> ISliceMut<'a, T> {
+    #[inline(always)]
+    fn flat(&self, i: usize) -> usize {
+        (self.base + i) * self.stride as usize + self.shift as usize
+    }
+
+    #[inline]
+    pub fn as_ref(&self) -> ISlice<'_, T> {
+        ISlice {
+            buf: &*self.buf,
+            base: self.base,
+            stride: self.stride,
+            shift: self.shift,
+        }
+    }
+
+    #[inline]
+    pub fn at(&self, i: usize) -> &T {
+        self.buf.at(self.flat(i))
+    }
+
+    #[inline]
+    pub fn read(&self, i: usize) -> T {
+        self.buf.read(self.flat(i))
+    }
+
+    #[inline]
+    pub fn at_mut(&mut self, i: usize) -> &mut T {
+        let idx = self.flat(i);
+        self.buf.at_mut(idx)
+    }
+
+    #[inline]
+    pub fn write(&mut self, i: usize, value: T) {
+        let idx = self.flat(i);
+        self.buf.write(idx, value)
+    }
+
+    #[inline]
+    pub fn offset(self, offset: usize) -> Self {
+        ISliceMut {
+            base: self.base + offset,
+            ..self
+        }
+    }
+}
+
+impl<T: Copy> Index<usize> for ISlice<'_, T> {
+    type Output = T;
+    #[inline(always)]
+    fn index(&self, i: usize) -> &T {
+        self.buf.at(self.flat(i))
+    }
+}
+
+impl<T: Copy> Index<usize> for ISliceMut<'_, T> {
+    type Output = T;
+    #[inline(always)]
+    fn index(&self, i: usize) -> &T {
+        self.buf.at(self.flat(i))
+    }
+}
+
+impl<T: Copy> IndexMut<usize> for ISliceMut<'_, T> {
+    #[inline(always)]
+    fn index_mut(&mut self, i: usize) -> &mut T {
+        let idx = self.flat(i);
+        self.buf.at_mut(idx)
+    }
+}