From db385113995a2fe7f47438d2f5c7973324485c1d Mon Sep 17 00:00:00 2001 From: Mark Nefedov Date: Fri, 14 Aug 2026 23:08:24 +0300 Subject: [PATCH] Per-step allocation and inner-loop cleanups in the step pipeline - carry_warmstart_data: hoist the manifold-level subshape_pos1 lookup out of the per-point matching loop (it is invariant there, and always None for cluster manifolds). - Narrow phase: keep the serial pair-transition buffer as a persistent NarrowPhase scratch field (mem::take + restore) so its capacity is reused across steps instead of reallocating each step. - Substep: replace the joint island event drain().collect() with the take + restore idiom already used for multibody chain events, removing a per-substep Vec allocation. - Staged island solver: resize the generic multibody solver velocity vectors in place and zero them instead of allocating two fresh DVectors every step. rapier3d test suite passes. --- .../solver/staged_island_solver/helpers.rs | 13 ++++++++++--- src/geometry/contact_clustering.rs | 7 +++++-- src/geometry/narrow_phase/contacts.rs | 14 +++++++++++--- src/geometry/narrow_phase/mod.rs | 5 +++++ src/pipeline/physics_pipeline/substep.rs | 9 +++++---- 5 files changed, 36 insertions(+), 12 deletions(-) diff --git a/src/dynamics/solver/staged_island_solver/helpers.rs b/src/dynamics/solver/staged_island_solver/helpers.rs index 1db1fa7bf..0227bf49d 100644 --- a/src/dynamics/solver/staged_island_solver/helpers.rs +++ b/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 diff --git a/src/geometry/contact_clustering.rs b/src/geometry/contact_clustering.rs index 04e0e2fe2..182c71f20 100644 --- a/src/geometry/contact_clustering.rs +++ b/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(); diff --git a/src/geometry/narrow_phase/contacts.rs b/src/geometry/narrow_phase/contacts.rs index bb870c612..476c7838c 100644 --- a/src/geometry/narrow_phase/contacts.rs +++ b/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 = Vec::new(); + let mut transitions: Vec = { + 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, 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")] { diff --git a/src/geometry/narrow_phase/mod.rs b/src/geometry/narrow_phase/mod.rs index 65ae91da2..3bb93593b 100644 --- a/src/geometry/narrow_phase/mod.rs +++ b/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, + /// 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, /// 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(), diff --git a/src/pipeline/physics_pipeline/substep.rs b/src/pipeline/physics_pipeline/substep.rs index 75b86d03e..500c4d05d 100644 --- a/src/pipeline/physics_pipeline/substep.rs +++ b/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);