Reuse ShapeSupportData in CastHullShape to avoid per-call allocation

CastHullShape::computeShapeSupport called getSupport(shape, dir, hint)
which created a fresh ShapeSupportData every call, forcing
getShapeSupportLog to reallocate its visited vector on every support
query. Store two persistent ShapeSupportData members (one per pose) and
pass them to the new getSupport overload. The visited vector is now
allocated once and reused via std::fill on subsequent calls.

Measured: getShapeSupportLog self-time dropped from 2.65% to 2.04%.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>

Author: rjoomen

tesseract/collision/coal/include/tesseract/collision/coal/coal_casthullshape.h

@@ -106,14 +106,17 @@ class CastHullShape : public coal::ShapeBase
   coal::Transform3s castTransform_;
   coal::Transform3s castTransformInv_;
 
-  /// Separate support function vertex hints for pose 0 and pose 1.
+  /// Separate support function vertex hints and data for pose 0 and pose 1.
   /// Each getSupport call uses hill-climbing from the hint, so sharing a single
   /// hint between the two poses (which query different directions) would cause
-  /// each to corrupt the other's warm-start.
+  /// each to corrupt the other's warm-start. The ShapeSupportData holds the
+  /// visited-vertex buffer reused across calls to avoid per-call allocation.
   /// @note Not thread-safe: each thread must use its own CastHullShape instance.
   /// The collision managers ensure this via per-thread clone().
   mutable int hint0_{ 0 };
   mutable int hint1_{ 0 };
+  mutable coal::details::ShapeSupportData support_data0_;
+  mutable coal::details::ShapeSupportData support_data1_;
 };
 
 }  // namespace tesseract::collision::tesseract_collision_coal

tesseract/collision/coal/src/coal_casthullshape.cpp

@@ -143,17 +143,20 @@ void CastHullShape::computeShapeSupport(const coal::Vec3s& dir,
   // Use WithSweptSphere so that shapes with intrinsic radii (Sphere, Capsule)
   // include that radius in the support point — necessary for correct swept-hull
   // geometry and matching the behavior of the former castHullGetSupportFunc.
-  // Each pose gets its own vertex hint so the two hill-climbing searches
-  // warm-start independently.
+  // Each pose gets its own vertex hint and ShapeSupportData so the two
+  // hill-climbing searches warm-start independently and reuse their visited
+  // buffers across calls.
   const coal::Vec3s s0 =
-      coal::details::getSupport<coal::details::SupportOptions::WithSweptSphere>(shape_.get(), dir, hint0_);
+      coal::details::getSupport<coal::details::SupportOptions::WithSweptSphere>(
+          shape_.get(), dir, hint0_, support_data0_);
 
   // Support at pose 1 (shape at castTransform_).
   // Rotate the query direction into the local frame of pose 1, compute support,
   // then transform the result back to the local frame of pose 0.
   const coal::Vec3s dir_local1 = castTransformInv_.getRotation() * dir;
   const coal::Vec3s s1_local =
-      coal::details::getSupport<coal::details::SupportOptions::WithSweptSphere>(shape_.get(), dir_local1, hint1_);
+      coal::details::getSupport<coal::details::SupportOptions::WithSweptSphere>(
+          shape_.get(), dir_local1, hint1_, support_data1_);
   const coal::Vec3s s1 = castTransform_.transform(s1_local);
 
   // Return the support of the convex hull of both poses (Schulman et al. 2013).