Swept-AABB broadphase for the SDF collision sequence error function (#1527)

Summary:
Pull Request resolved: #1527

Add a conservative broadphase to `SDFCollisionSequenceErrorFunctionT` that skips any (vertex, collider) pair whose swept segment cannot reach the collider. The segment is already mapped into the collider's local SDF frame for the sweep; if the segment's local axis-aligned bounding box does not overlap the collider's SDF `bounds()`, the pair cannot penetrate and is skipped before the more expensive sphere-trace and deepest-point search. Because both the cull and the detailed test operate in the same local frame, the cull is exact — it only ever skips pairs whose detailed test would have found no penetration — so the error, gradient, and Jacobian are unchanged.

This replaces running the sphere-trace for every (vertex, collider) pair regardless of proximity, which matters once the participating-vertex set or collider count is large. It adds `bounds()` to the (already duck-typed) collider interface this function relies on.

Reviewed By: yutingye

Differential Revision: D108807344

fbshipit-source-id: 1de7688f8e86807d1e5074102c466569848d9661

Author: cdtwigg

momentum/character_sequence_solver/sdf_collision_sequence_error_function.h

@@ -66,8 +66,11 @@ namespace momentum {
 /// gradient is mapped to world space including the world-to-collider scale factor, which the
 /// derivatives require whenever the collider's joint carries a non-unit scale.
 ///
+/// A conservative broadphase skips any (vertex, collider) pair whose swept segment cannot reach the
+/// collider's SDF bounds, so distant pairs cost only an axis-aligned-bounding-box test.
+///
 /// The `SdfColliderType` is duck-typed; see `SDFCollisionErrorFunctionT` for the required
-/// interface.
+/// interface (this function additionally uses `bounds()`).
 template <typename T, typename SdfColliderType = SDFColliderT<float>>
 class SDFCollisionSequenceErrorFunctionT : public SequenceErrorFunctionT<T> {
  public:
@@ -170,6 +173,15 @@ class SDFCollisionSequenceErrorFunctionT : public SequenceErrorFunctionT<T> {
       const SweptIntervalT& interval,
       const SdfColliderType& collider) const;
 
+  /// Conservative broadphase: returns false when the swept segment's local-frame AABB cannot reach
+  /// the collider's SDF bounds, so the pair cannot penetrate and can be skipped. Because the sweep
+  /// is evaluated in the same local frame, this is exact — it only skips pairs whose detailed test
+  /// would have found no penetration.
+  [[nodiscard]] bool sweptSegmentMayCollide(
+      const Eigen::Vector3<T>& localPos0,
+      const Eigen::Vector3<T>& localPos1,
+      const SdfColliderType& collider) const;
+
   // Sphere-trace step budget for crossing detection. Generous: the |sdf|-clamped step cannot skip a
   // surface, so this only bounds work, it does not affect accuracy.
   static constexpr int kMaxMarchSteps = 64;
@@ -325,6 +337,20 @@ T SDFCollisionSequenceErrorFunctionT<T, SdfColliderType>::findDeepestPoint(
   return T(0.5) * (lo + hi);
 }
 
+template <typename T, typename SdfColliderType>
+bool SDFCollisionSequenceErrorFunctionT<T, SdfColliderType>::sweptSegmentMayCollide(
+    const Eigen::Vector3<T>& localPos0,
+    const Eigen::Vector3<T>& localPos1,
+    const SdfColliderType& collider) const {
+  const auto bounds = collider.bounds();
+  const Eigen::Vector3<T> boundsMin = bounds.min().template cast<T>();
+  const Eigen::Vector3<T> boundsMax = bounds.max().template cast<T>();
+  const Eigen::Vector3<T> segMin = localPos0.cwiseMin(localPos1);
+  const Eigen::Vector3<T> segMax = localPos0.cwiseMax(localPos1);
+  return !(
+      (segMin.array() > boundsMax.array()).any() || (segMax.array() < boundsMin.array()).any());
+}
+
 template <typename T, typename SdfColliderType>
 double SDFCollisionSequenceErrorFunctionT<T, SdfColliderType>::getError(
     std::span<const ModelParametersT<T>> /* modelParameters */,
@@ -359,6 +385,9 @@ double SDFCollisionSequenceErrorFunctionT<T, SdfColliderType>::getError(
       if (segLen < std::numeric_limits<T>::epsilon()) {
         continue; // no motion: static penetration is the per-frame function's job
       }
+      if (!sweptSegmentMayCollide(localPos0, localPos1, collider)) {
+        continue;
+      }
 
       const auto intervals = findPenetratingIntervals(localPos0, delta, segLen, collider);
       for (const auto& interval : intervals) {
@@ -418,6 +447,9 @@ double SDFCollisionSequenceErrorFunctionT<T, SdfColliderType>::getGradient(
       if (segLen < std::numeric_limits<T>::epsilon()) {
         continue;
       }
+      if (!sweptSegmentMayCollide(localPos0, localPos1, collider)) {
+        continue;
+      }
 
       const size_t commonAncestor = detail_sdf_collision::findCommonAncestorForVertex(
           *character_.skinWeights, this->skeleton_, vertexIndex, collider.parentJoint());
@@ -532,6 +564,9 @@ double SDFCollisionSequenceErrorFunctionT<T, SdfColliderType>::getJacobian(
       if (segLen < std::numeric_limits<T>::epsilon()) {
         continue;
       }
+      if (!sweptSegmentMayCollide(localPos0, localPos1, collider)) {
+        continue;
+      }
 
       const size_t commonAncestor = detail_sdf_collision::findCommonAncestorForVertex(
           *character_.skinWeights, this->skeleton_, vertexIndex, collider.parentJoint());

momentum/test/character_sequence_solver/sequence_test.cpp

@@ -69,6 +69,9 @@ struct AnalyticSphereCollider {
         dist > T(0) ? Eigen::Vector3<T>(localPos / dist) : Eigen::Vector3<T>::UnitX();
     return {dist - T(radius), grad};
   }
+  [[nodiscard]] axel::BoundingBox<float> bounds() const {
+    return axel::BoundingBox<float>(Eigen::Vector3f::Zero(), radius);
+  }
   [[nodiscard]] size_t parentJoint() const {
     return parent;
   }