Narrow-phase micro-optimizations: EPA reuse, manifold drain, loop hoists

src/query/contact/contact_support_map_support_map.rs

@@ -63,9 +63,26 @@ where
     }
 
     // The point is inside of the CSO: use the fallback algorithm
-    let mut epa = EPA::new();
-    if let Some((p1, p2, n)) = epa.closest_points(pos12, g1, g2, simplex) {
-        return GJKResult::ClosestPoints(p1, p2, n);
+    #[cfg(feature = "std")]
+    {
+        // Reuse a thread-local EPA so repeated penetrating contacts don't
+        // re-grow its internal buffers on every call.
+        std::thread_local! {
+            static EPA_WORKSPACE: core::cell::RefCell<EPA> =
+                core::cell::RefCell::new(EPA::new());
+        }
+        if let Some((p1, p2, n)) =
+            EPA_WORKSPACE.with(|epa| epa.borrow_mut().closest_points(pos12, g1, g2, simplex))
+        {
+            return GJKResult::ClosestPoints(p1, p2, n);
+        }
+    }
+    #[cfg(not(feature = "std"))]
+    {
+        let mut epa = EPA::new();
+        if let Some((p1, p2, n)) = epa.closest_points(pos12, g1, g2, simplex) {
+            return GJKResult::ClosestPoints(p1, p2, n);
+        }
     }
 
     // Everything failed

src/query/contact_manifolds/contact_manifolds_capsule_capsule.rs

@@ -39,10 +39,10 @@ pub fn contact_manifold_capsule_capsule<'a, ManifoldData, ContactData>(
         (&seg2_1.a, &seg2_1.b),
     );
 
-    // We do this clone to perform contact tracking and transfer impulses.
-    // TODO: find a more efficient way of doing this.
-    let old_manifold_points = manifold.points.clone();
-    manifold.clear();
+    // Drain the previous points into a stack-first buffer to perform contact
+    // tracking and impulse transfer without a per-call heap allocation (the
+    // face/face case can emit up to 8 contacts).
+    let old_manifold_points: smallvec::SmallVec<[_; 8]> = manifold.points.drain(..).collect();
 
     let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 {
         v1 * 2

src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs

@@ -174,13 +174,13 @@ pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, Conta
                 );
             };
 
-            for leaf_id in composite2.bvh().intersect_aabb(&ls_part_aabb1_2) {
+            for leaf_id in bvh2.intersect_aabb(&ls_part_aabb1_2) {
                 leaf_fn2(leaf_id);
             }
         });
     };
 
-    for leaf_id in composite1.bvh().intersect_aabb(&ls_aabb2_1) {
+    for leaf_id in bvh1.intersect_aabb(&ls_aabb2_1) {
         leaf_fn1(leaf_id);
     }
 

src/query/contact_manifolds/contact_manifolds_cuboid_capsule.rs

@@ -122,10 +122,10 @@ pub fn contact_manifold_cuboid_capsule<'a, ManifoldData, ContactData>(
         feature2 = PolygonalFeature::from(segment2);
     }
 
-    // We do this clone to perform contact tracking and transfer impulses.
-    // TODO: find a more efficient way of doing this.
-    let old_manifold_points = manifold.points.clone();
-    manifold.clear();
+    // Drain the previous points into a stack-first buffer to perform contact
+    // tracking and impulse transfer without a per-call heap allocation (the
+    // face/face case can emit up to 8 contacts).
+    let old_manifold_points: smallvec::SmallVec<[_; 8]> = manifold.points.drain(..).collect();
 
     #[cfg(feature = "dim2")]
     CuboidFeature::face_face_contacts(

src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs

@@ -76,10 +76,10 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C
         best_sep = sep3;
     }
 
-    // We do this clone to perform contact tracking and transfer impulses.
-    // TODO: find a more efficient way of doing this.
-    let old_manifold_points = manifold.points.clone();
-    manifold.clear();
+    // Drain the previous points into a stack-first buffer to perform contact
+    // tracking and impulse transfer without a per-call heap allocation (the
+    // face/face case can emit up to 8 contacts).
+    let old_manifold_points: smallvec::SmallVec<[_; 8]> = manifold.points.drain(..).collect();
 
     let local_n2 = pos21.rotation * -best_sep.1;
 

src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs

@@ -140,10 +140,10 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>(
         feature2 = PolygonalFeature::from(*triangle2);
     }
 
-    // We do this clone to perform contact tracking and transfer impulses.
-    // TODO: find a more efficient way of doing this.
-    let old_manifold_points = manifold.points.clone();
-    manifold.clear();
+    // Drain the previous points into a stack-first buffer to perform contact
+    // tracking and impulse transfer without a per-call heap allocation (the
+    // face/face case can emit up to 8 contacts).
+    let old_manifold_points: smallvec::SmallVec<[_; 8]> = manifold.points.drain(..).collect();
 
     PolygonalFeature::contacts(
         pos12, pos21, normal1, normal2, &feature1, &feature2, manifold, flipped,

src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs

@@ -117,6 +117,7 @@ pub fn contact_manifolds_heightfield_shape<ManifoldData, ContactData>(
     // TODO: somehow precompute the Aabb and reuse it?
     let ls_aabb2 = shape2.compute_aabb(pos12).loosened(prediction);
     let mut old_manifolds = core::mem::take(manifolds);
+    let pos21 = pos12.inverse();
 
     heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1| {
         #[cfg(feature = "dim2")]
@@ -161,7 +162,7 @@ pub fn contact_manifolds_heightfield_shape<ManifoldData, ContactData>(
 
         if flipped {
             let _ = dispatcher.contact_manifold_convex_convex(
-                &pos12.inverse(),
+                &pos21,
                 shape2,
                 &sub_shape1,
                 None,

src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs

@@ -74,8 +74,10 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>(
         init_dir,
     );
 
-    let old_manifold_points = manifold.points.clone();
-    manifold.clear();
+    // Drain the previous points into a stack-first buffer to perform contact
+    // tracking and impulse transfer without a per-call heap allocation (the
+    // face/face case can emit up to 8 contacts).
+    let old_manifold_points: smallvec::SmallVec<[_; 8]> = manifold.points.drain(..).collect();
 
     match contact {
         GJKResult::ClosestPoints(p1, p2_1, dir) => {

src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs

@@ -149,6 +149,8 @@ pub fn contact_manifolds_trimesh_shape<ManifoldData, ContactData>(
     let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
     let mut old_manifolds_it = old_manifolds.drain(..);
 
+    let pos21 = pos12.inverse();
+
     // TODO: don't redispatch at each frame (we should probably do the same as
     // the heightfield).
     for (i, triangle_id) in new_interferences.iter().enumerate() {
@@ -194,7 +196,7 @@ pub fn contact_manifolds_trimesh_shape<ManifoldData, ContactData>(
 
         if flipped {
             let _ = dispatcher.contact_manifold_convex_convex(
-                &pos12.inverse(),
+                &pos21,
                 shape2,
                 &triangle1,
                 None,

src/query/contact_manifolds/contact_manifolds_voxels_shape.rs

@@ -270,6 +270,11 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
              * Filter-out points that don’t belong to this block.
              */
             let test_voxel = Cuboid::new(radius1 + Vector::splat(1.0e-2));
+            let cuboid1 = Cuboid::new(radius1);
+            // Resolve the support-map once instead of once per contact point
+            // (the `expect` below preserves the original panic timing: it only
+            // fires if a penetrating point actually needs the support-map).
+            let sm2_opt = shape2.as_support_map();
             let penetration_dir1 = if flipped {
                 manifold.local_n2
             } else {
@@ -281,11 +286,8 @@ pub fn contact_manifolds_voxels_shape<ManifoldData, ContactData>(
                     // If this is a penetration, double-check that we are not hitting the
                     // interior of the infinitely expanded canonical shape by checking if
                     // the opposite normal had led to a better vector.
-                    let cuboid1 = Cuboid::new(radius1);
                     let sp1 = cuboid1.local_support_point(-penetration_dir1) + vox1.center;
-                    let sm2 = shape2
-                        .as_support_map()
-                        .expect("Unsupported collision pair.");
+                    let sm2 = sm2_opt.expect("Unsupported collision pair.");
                     let sp2 = sm2.support_point(pos12, penetration_dir1);
                     let test_dist = (sp2 - sp1).dot(-penetration_dir1);
                     let keep = test_dist < pt.dist;

src/query/epa/epa2.rs

@@ -198,7 +198,13 @@ impl EPA {
     /// # }
     /// ```
     pub fn new() -> Self {
-        EPA::default()
+        // Pre-reserve the internal buffers so a fresh EPA doesn't pay a series
+        // of reallocations on its first penetrating contact.
+        Self {
+            vertices: Vec::with_capacity(32),
+            faces: Vec::with_capacity(64),
+            heap: BinaryHeap::with_capacity(32),
+        }
     }
 
     fn reset(&mut self) {

src/query/epa/epa3.rs

@@ -275,7 +275,15 @@ impl EPA {
     /// # }
     /// ```
     pub fn new() -> Self {
-        Self::default()
+        // Pre-reserve the internal buffers: EPA typically visits a few dozen
+        // vertices/faces per query, and starting from empty buffers would pay
+        // a dozen-plus reallocations on every penetrating contact.
+        Self {
+            vertices: Vec::with_capacity(64),
+            faces: Vec::with_capacity(128),
+            silhouette: Vec::with_capacity(32),
+            heap: BinaryHeap::with_capacity(64),
+        }
     }
 
     fn reset(&mut self) {

src/query/gjk/gjk.rs

@@ -138,7 +138,8 @@ pub enum GJKResult {
 /// # Returns
 ///
 /// The absolute tolerance value (10 * DEFAULT_EPSILON)
-pub fn eps_tol() -> Real {
+#[inline]
+pub const fn eps_tol() -> Real {
     let _eps = crate::math::DEFAULT_EPSILON;
     _eps * 10.0
 }
@@ -386,7 +387,9 @@ where
     // `< 0` bounds the penetration depth.
     let mut best_min_bound = -Real::max_value();
     // Largest CSO support magnitude seen, used to scale the tolerance below.
-    let mut support_scale: Real = 0.0;
+    // Squared while accumulated; max commutes with the monotonic sqrt, so the
+    // root is only taken at the (at most one) exit that consumes it.
+    let mut support_scale_sq: Real = 0.0;
     let mut nb_perturbs = 0usize;
     const MAX_PERTURBATIONS: usize = 2 * DIM;
 
@@ -412,7 +415,7 @@ where
                 } else {
                     return GJKResult::Proximity(old_dir);
                 }
-            } else if exact_dist && best_min_bound >= -_eps_rel * support_scale {
+            } else if exact_dist && best_min_bound >= -_eps_rel * support_scale_sq.sqrt() {
                 // Origin on the CSO boundary: the pair is touching, so the witnesses
                 // describe the contact.
                 let (p1, p2) = result(simplex, true);
@@ -433,7 +436,7 @@ where
         assert!(min_bound.is_finite());
 
         best_min_bound = best_min_bound.max(min_bound);
-        support_scale = support_scale.max(cso_point.point.length());
+        support_scale_sq = support_scale_sq.max(cso_point.point.length_squared());
 
         if min_bound > max_dist {
             return GJKResult::NoIntersection(dir);
@@ -457,7 +460,7 @@ where
                 } else {
                     return GJKResult::Proximity(dir);
                 }
-            } else if exact_dist && best_min_bound >= -_eps_rel * support_scale {
+            } else if exact_dist && best_min_bound >= -_eps_rel * support_scale_sq.sqrt() {
                 let (p1, p2) = result(simplex, false);
                 return GJKResult::ClosestPoints(p1, p2, dir);
             } else if nb_perturbs < MAX_PERTURBATIONS {