Per-step allocation and inner-loop cleanups in the step pipeline

src/dynamics/solver/staged_island_solver/helpers.rs

@@ -12,7 +12,6 @@ use crate::dynamics::solver::solver_contact_graph::SolverContactGraph;
 #[cfg(feature = "dim3")]
 use crate::dynamics::solver::velocity_solver::GyroParams;
 use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodyHandle, RigidBodySet};
-use crate::math::DVector;
 use parry::math::SIMD_WIDTH;
 
 impl ContactConstraintsSet {
@@ -95,8 +94,16 @@ impl VelocitySolver {
             }
         }
 
-        self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id as usize);
-        self.generic_solver_vels = DVector::zeros(multibody_solver_id as usize);
+        // Resize in place and zero the rows instead of allocating two fresh
+        // vectors every step (reallocation only happens when the total DoF
+        // count grows).
+        let total_ndofs = multibody_solver_id as usize;
+        self.generic_solver_vels_increment
+            .resize_vertically_mut(total_ndofs, 0.0);
+        self.generic_solver_vels_increment.fill(0.0);
+        self.generic_solver_vels
+            .resize_vertically_mut(total_ndofs, 0.0);
+        self.generic_solver_vels.fill(0.0);
 
         for link in &self.multibody_roots {
             let multibody = multibodies

src/geometry/contact_clustering.rs

@@ -147,14 +147,17 @@ pub(crate) fn carry_warmstart_data(
                     continue;
                 }
 
+                // Manifold-level, invariant across the point loop (and always
+                // `None` for cluster manifolds).
+                let subshape_pos1 = target.subshape_pos1();
+
                 for (pt_id, pt) in target.points.iter().enumerate() {
                     if has_warmstart_data(&pt.data) {
                         // Already claimed by a previous point.
                         continue;
                     }
 
-                    let p1 = target
-                        .subshape_pos1()
+                    let p1 = subshape_pos1
                         .map(|pos| pos * pt.local_p1)
                         .unwrap_or(pt.local_p1);
                     let dist_sq = (p1 - prev_pt.local_p1).length_squared();

src/geometry/narrow_phase/contacts.rs

@@ -79,9 +79,14 @@ impl NarrowPhase {
         );
 
         // Begin/end-touch transitions detected during the update; applied by the
-        // sorted post-loop pass (see `PairTransition`).
+        // sorted post-loop pass (see `PairTransition`). Taken from a persistent
+        // scratch field so its capacity is reused across steps.
         #[cfg(not(feature = "parallel"))]
-        let mut transitions: Vec<PairTransition> = Vec::new();
+        let mut transitions: Vec<PairTransition> = {
+            let mut t = core::mem::take(&mut self.pair_transitions);
+            t.clear();
+            t
+        };
         #[cfg(not(feature = "parallel"))]
         let process_pair = |edge: &mut crate::data::graph::Edge<ContactPair>, edge_id: u32| {
             pair_update::process_pair(
@@ -162,7 +167,10 @@ impl NarrowPhase {
         }
 
         #[cfg(not(feature = "parallel"))]
-        self.apply_pair_transitions(&mut transitions, islands, bodies, colliders, events);
+        {
+            self.apply_pair_transitions(&mut transitions, islands, bodies, colliders, events);
+            self.pair_transitions = transitions;
+        }
 
         #[cfg(feature = "parallel")]
         {

src/geometry/narrow_phase/mod.rs

@@ -351,6 +351,10 @@ pub struct NarrowPhase {
     /// narrow-phase's it-may-have-moved signal for pair updates.
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     awake_body_mask: Vec<bool>,
+    /// Scratch: begin/end-touch transitions recorded during the serial pair update,
+    /// kept as a field so its capacity is reused across steps.
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pair_transitions: Vec<pair_update::PairTransition>,
     /// Per-pair solver-qualification hints (contact-graph edge index): bit 15 = has a
     /// dynamic body, low bits = qualified solver-manifold count. Maintained incrementally
     /// (count-cleared on sleep, mirrored on removals) so selection never re-walks every pair.
@@ -429,6 +433,7 @@ impl NarrowPhase {
             intersection_graph: InteractionGraph::new(),
             graph_indices: Coarena::new(),
             update_candidates: Vec::new(),
+            pair_transitions: Vec::new(),
             retired_pairs: Vec::new(),
             body_solver_color_masks: Vec::new(),
             body_qualify_info: Vec::new(),

src/pipeline/physics_pipeline/substep.rs

@@ -1,8 +1,6 @@
 //! The pipeline's inner step loop: CCD substepping and motion clamping, plus
 //! end-of-step advancement of bodies, colliders and broad-phase AABBs.
 
-use crate::alloc_prelude::*;
-
 use crate::dynamics::{
     CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet,
     RigidBodyChanges, RigidBodySet, RigidBodyType,
@@ -356,10 +354,13 @@ impl PhysicsPipeline {
         }
 
         // Persistent islands: apply the joint connectivity edits (in order).
-        let joint_island_events: Vec<_> = impulse_joints.island_events.drain(..).collect();
-        for event in joint_island_events {
+        // Take + restore instead of drain().collect() so no per-substep Vec is
+        // allocated (same idiom as the multibody chain events below).
+        let mut joint_island_events = core::mem::take(&mut impulse_joints.island_events);
+        for event in joint_island_events.drain(..) {
             islands.apply_impulse_joint_island_event(bodies, event);
         }
+        impulse_joints.island_events = joint_island_events;
         let mut mb_chain_events = core::mem::take(&mut multibody_joints.island_chain_events);
         for mb_id in &mb_chain_events {
             islands.refresh_multibody_chain(bodies, multibody_joints, *mb_id);