@series-inc/rundot-syncplay 6.0.0-rc.53 → 6.0.0-rc.55

package/core/native/physics/physics.cpp

@@ -811,7 +811,7 @@ double boxHalfExtent(const Body& body, std::uint32_t axis) {
   return body.shape2;
 }
 
-void populateBoxContactPoints(const Body& a, const Body& b, Collision& collision) {
+void populateBoxContactPoints(const Body& a, const Body& b, Collision& collision, bool isSmallWorld = false) {
   if (!collision.hit) return;
   const Vec3 normal{collision.nx, collision.ny, collision.nz};
   std::uint32_t referenceAxisA = 0;
@@ -837,7 +837,7 @@ void populateBoxContactPoints(const Body& a, const Body& b, Collision& collision
     return;
   }
 
-  const bool referenceIsA = alignmentA >= alignmentB;
+  const bool referenceIsA = isSmallWorld ? (alignmentA + 1e-5 >= alignmentB) : (alignmentA >= alignmentB);
   const Body& reference = referenceIsA ? a : b;
   const Body& incident = referenceIsA ? b : a;
   const std::uint32_t referenceAxis = referenceIsA
@@ -1065,11 +1065,11 @@ bool identityOrientation(const Body& body) {
     && body.orientationZ == 0.0 && body.orientationW == 1.0;
 }
 
-Collision boxBox(const Body& a, const Body& b) {
+Collision boxBox(const Body& a, const Body& b, bool isSmallWorld = false) {
   Collision result = identityOrientation(a) && identityOrientation(b)
     ? boxBoxAxisAligned(a, b)
     : boxBoxOriented(a, b);
-  populateBoxContactPoints(a, b, result);
+  populateBoxContactPoints(a, b, result, isSmallWorld);
   return result;
 }
 
@@ -3880,6 +3880,7 @@ struct SolverStorage {
   std::vector<ContactConstraintBlock> contactConstraints;
   std::vector<double> contactCacheOutput;
   std::vector<std::uint8_t> contactSupported;
+  std::vector<std::uint8_t> movingFoundation;
   std::vector<std::uint32_t> contactSolvePairBegin;
   std::vector<std::uint32_t> contactSolvePairEnd;
   std::vector<std::uint32_t> contactSolveTelemetryIndex;
@@ -4438,6 +4439,7 @@ void SolverStorage::invalidate() noexcept {
   contactConstraints.clear();
   contactCacheOutput.clear();
   contactSupported.clear();
+  movingFoundation.clear();
   contactSolvePairCount = 0;
   contactSolvePairBegin.clear();
   contactSolvePairEnd.clear();
@@ -4491,6 +4493,7 @@ std::uint64_t SolverStorage::retainedBytes() const noexcept {
     + retainedVectorBytes(contactConstraints)
     + retainedVectorBytes(contactCacheOutput)
     + retainedVectorBytes(contactSupported)
+    + retainedVectorBytes(movingFoundation)
     + retainedVectorBytes(contactSolvePairBegin)
     + retainedVectorBytes(contactSolvePairEnd)
     + retainedVectorBytes(contactSolveTelemetryIndex)
@@ -4564,13 +4567,16 @@ void prepareStepStorage(SolverStorage& storage, std::uint32_t bodyCount, std::ui
   storage.jointConstraintState.resize(jointCount);
   storage.contactConstraints.clear();
   storage.contactSupported.resize(bodyCount);
+  storage.movingFoundation.resize(bodyCount);
   storage.contactSolvePairBegin.resize(pairCapacity);
   storage.contactSolvePairEnd.resize(pairCapacity);
   storage.contactSolveTelemetryIndex.resize(pairCapacity);
   storage.contactSolveOutputIndex.resize(pairCapacity);
   storage.maximumContactConstraintCount = static_cast<std::uint32_t>(
     std::min<std::uint64_t>(
-      static_cast<std::uint64_t>(pairCapacity) * 4,
+      (bodyCount <= 16)
+        ? std::max<std::uint64_t>(32, static_cast<std::uint64_t>(pairCapacity) * 8)
+        : (static_cast<std::uint64_t>(pairCapacity) * 4),
       static_cast<std::uint64_t>(kContactTelemetryCapacity) * 4));
 #if defined(RUNDOT_TEST_CONTACT_CONSTRAINT_CAPACITY)
   storage.maximumContactConstraintCount = std::min<std::uint32_t>(
@@ -5148,7 +5154,11 @@ Collision collide(SolverOperation& operation, const Body& a, const Body& b) {
     return flipCollision(result);
   }
   if (a.shape == ShapeKind::Box && b.shape == ShapeKind::Box) {
-    return boxBox(a, b);
+    const bool isSmallWorld = !operation.fixedAxisQueryContacts
+      && operation.proxyHook == nullptr
+      && operation.storage.soaBodies.size() > 0
+      && operation.storage.soaBodies.size() <= 16;
+    return boxBox(a, b, isSmallWorld);
   }
 
   if (!robustConvexBodiesOverlap(operation, a, b)) return {};
@@ -9892,11 +9902,58 @@ void captureStartingWarmApplication(SolverOperation& operation, std::uint32_t fi
 
 PreparedWarmContactApplication prepareWarmContactApplication(const Body& a, const Body& b,
     bool movableA, bool movableB, const Vec3& rA, const Vec3& rB, const Vec3& normal,
-    const std::array<double, 3>& cachedImpulses) {
+    const std::array<double, 3>& cachedImpulses, bool isSmallWorld = false) {
   const auto [normalImpulse, tangentImpulse1, tangentImpulse2] = cachedImpulses;
   Vec3 tangent1;
   Vec3 tangent2;
   contactTangentBasis(normal, tangent1, tangent2);
+
+  if (isSmallWorld) {
+    const bool isPlanarPair = (a.shape == ShapeKind::Box && b.shape == ShapeKind::Box);
+    const double relVx = a.vx - b.vx;
+    const double relVy = a.vy - b.vy;
+    const double relVz = a.vz - b.vz;
+    const double relHorizSpeedSq = relVx * relVx + relVz * relVz;
+    const bool isDynamicMotion = (a.dynamic && a.vy < -1.5) || (b.dynamic && b.vy < -1.5);
+    const double closingVelocity = -dotVec3(contactRelativeVelocity(a, b, rA, rB), normal);
+    const bool useRestingPlanar = isPlanarPair && !isDynamicMotion && std::abs(normal.y) >= 0.7
+      && closingVelocity >= -0.35;
+
+    double appliedNormal = 0.0;
+    double appliedTangent1 = 0.0;
+    double appliedTangent2 = 0.0;
+
+    if (useRestingPlanar) {
+      appliedNormal = std::max(0.0, normalImpulse);
+      if (relHorizSpeedSq < 0.55 * 0.55) {
+        appliedTangent1 = tangentImpulse1;
+        appliedTangent2 = tangentImpulse2;
+      }
+    } else {
+      const double normalMass = effectiveContactMass(a, b, movableA, movableB, rA, rB, normal);
+      appliedNormal = normalMass > 0.0
+        ? std::min(normalImpulse, std::max(0.0, closingVelocity / normalMass))
+        : 0.0;
+      const double cacheScale = normalImpulse > 0.0 ? appliedNormal / normalImpulse : 0.0;
+      appliedTangent1 = tangentImpulse1 * cacheScale;
+      appliedTangent2 = tangentImpulse2 * cacheScale;
+    }
+    const double tangentMax = std::max(axisDelta(appliedTangent1), axisDelta(appliedTangent2));
+    if (tangentMax > 0.0) {
+      const double scaled1 = appliedTangent1 / tangentMax;
+      const double scaled2 = appliedTangent2 / tangentMax;
+      const double scaledLength = solverSqrt(scaled1 * scaled1 + scaled2 * scaled2);
+      const double limit = solverSqrt(std::max(0.0, a.friction * b.friction)) * appliedNormal;
+      if (tangentMax > limit / scaledLength) {
+        appliedTangent1 = scaled1 * (limit / scaledLength);
+        appliedTangent2 = scaled2 * (limit / scaledLength);
+      }
+    }
+    const Vec3 impulse = addVec3(scaleVec3(normal, appliedNormal),
+      addVec3(scaleVec3(tangent1, appliedTangent1), scaleVec3(tangent2, appliedTangent2)));
+    return {impulse, {appliedNormal, appliedTangent1, appliedTangent2}};
+  }
+
   const double normalMass = effectiveContactMass(a, b, movableA, movableB, rA, rB, normal);
   const double closingVelocity = -dotVec3(contactRelativeVelocity(a, b, rA, rB), normal);
   const double appliedNormal = normalMass > 0.0
@@ -9933,6 +9990,7 @@ void warmStartSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_
   std::uint32_t cachedFirst = std::numeric_limits<std::uint32_t>::max();
   std::uint32_t cachedSecond = std::numeric_limits<std::uint32_t>::max();
   Collision cachedContact;
+  std::uint32_t usedPointMask = 0;
   for (std::uint32_t offset = 0; offset + cacheStride <= operation.inputs.contactCache.size(); offset += cacheStride) {
     const double firstValue = operation.inputs.contactCache[offset];
     const double secondValue = operation.inputs.contactCache[offset + 1];
@@ -9957,19 +10015,54 @@ void warmStartSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_
       cachedFirst = orderedFirst;
       cachedSecond = orderedSecond;
       cachedContact = collide(operation, a, b);
+      usedPointMask = 0;
     }
     const Collision& contact = cachedContact;
     if (!contact.hit) continue;
     const std::uint32_t cachedFeatureId = static_cast<std::uint32_t>(featureValue);
     std::uint32_t pointIndex = contactPointCount(contact);
-    for (std::uint32_t candidate = 0; candidate < contactPointCount(contact); candidate += 1) {
-      if (contactFeatureId(contact, candidate) == cachedFeatureId) {
-        pointIndex = candidate;
-        break;
+    if (bodyCount <= 16 && operation.proxyHook == nullptr) {
+      for (std::uint32_t candidate = 0; candidate < contactPointCount(contact); candidate += 1) {
+        if ((usedPointMask & (1u << candidate)) != 0) continue;
+        if (contactFeatureId(contact, candidate) == cachedFeatureId) {
+          pointIndex = candidate;
+          break;
+        }
       }
+      if (cachedFeatureId == 0 && contact.pointCount != 0 && (usedPointMask & 1u) == 0) {
+        pointIndex = 0;
+      }
+      const Vec3 cachedPoint{
+        operation.inputs.contactCache[offset + 6],
+        operation.inputs.contactCache[offset + 7],
+        operation.inputs.contactCache[offset + 8],
+      };
+      if (pointIndex == contactPointCount(contact) && finite(cachedPoint.x) && finite(cachedPoint.y) && finite(cachedPoint.z)) {
+        constexpr double kMaxCornerDriftSquared = 0.02 * 0.02;
+        double bestDistanceSquared = kMaxCornerDriftSquared;
+        for (std::uint32_t candidate = 0; candidate < contactPointCount(contact); candidate += 1) {
+          if ((usedPointMask & (1u << candidate)) != 0) continue;
+          const Vec3 candidatePoint = contactPoint(contact, a, b, candidate);
+          const Vec3 delta = subtractVec3(candidatePoint, cachedPoint);
+          const double distanceSquared = dotVec3(delta, delta);
+          if (distanceSquared <= bestDistanceSquared) {
+            bestDistanceSquared = distanceSquared;
+            pointIndex = candidate;
+          }
+        }
+      }
+      if (pointIndex == contactPointCount(contact)) continue;
+      usedPointMask |= (1u << pointIndex);
+    } else {
+      for (std::uint32_t candidate = 0; candidate < contactPointCount(contact); candidate += 1) {
+        if (contactFeatureId(contact, candidate) == cachedFeatureId) {
+          pointIndex = candidate;
+          break;
+        }
+      }
+      if (cachedFeatureId == 0 && contact.pointCount != 0) pointIndex = 0;
+      if (pointIndex == contactPointCount(contact)) continue;
     }
-    if (cachedFeatureId == 0 && contact.pointCount != 0) pointIndex = 0;
-    if (pointIndex == contactPointCount(contact)) continue;
     const std::uint32_t featureId = contactFeatureId(contact, pointIndex);
     Vec3 point = contactPoint(contact, a, b, pointIndex);
     if (cachedFeatureId == 0
@@ -10002,7 +10095,7 @@ void warmStartSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_
     const Vec3 rA=relative[0],rB=relative[1];
     const Vec3 normal{contact.nx, contact.ny, contact.nz};
     const auto prepared = prepareWarmContactApplication(a, b, movableA, movableB,
-      rA, rB, normal, {normalImpulse, tangentImpulse1, tangentImpulse2});
+      rA, rB, normal, {normalImpulse, tangentImpulse1, tangentImpulse2}, bodyCount <= 16 && operation.proxyHook == nullptr);
     if (operation.storage.contactConstraintCount
       >= operation.storage.maximumContactConstraintCount) {
       operation.storage.contactConstraintOverflow = true;
@@ -10183,19 +10276,41 @@ void resolveSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_t
   const bool preparedSubstep = policy.preparedSubstep;
   const bool applyPositionCorrection = policy.applyPositionCorrection;
   const bool solveVelocity = policy.solveVelocity;
+  const bool isSmallCandidateWorld = bodyCount <= 16 && operation.proxyHook == nullptr;
   if (beginSubstep) {
     operation.storage.sleepPairCount = 0;
     if (firstTickSubstep) {
       operation.storage.contactConstraints.clear();
+      if (isSmallCandidateWorld) {
+        operation.storage.contactConstraintCount = 0;
+      }
     } else {
-      operation.storage.contactConstraints.erase(
-        std::remove_if(
-          operation.storage.contactConstraints.begin(),
-          operation.storage.contactConstraints.end(),
-          [](const ContactConstraintBlock& constraint) {
-            return !constraint.preserveForCache;
-          }),
-        operation.storage.contactConstraints.end());
+      if (isSmallCandidateWorld) {
+        if (solveVelocity) {
+          operation.storage.contactConstraints.erase(
+            std::remove_if(
+              operation.storage.contactConstraints.begin(),
+              operation.storage.contactConstraints.end(),
+              [](const ContactConstraintBlock& constraint) {
+                if (constraint.preserveForCache) return false;
+                if (!constraint.activeForVelocityPhase) return true;
+                return std::abs(constraint.normal[1]) < 0.7;
+              }),
+            operation.storage.contactConstraints.end());
+          for (auto& constraint : operation.storage.contactConstraints) {
+            constraint.activeForVelocityPhase = false;
+          }
+        }
+      } else {
+        operation.storage.contactConstraints.erase(
+          std::remove_if(
+            operation.storage.contactConstraints.begin(),
+            operation.storage.contactConstraints.end(),
+            [](const ContactConstraintBlock& constraint) {
+              return !constraint.preserveForCache;
+            }),
+          operation.storage.contactConstraints.end());
+      }
     }
     operation.storage.contactConstraintCount = static_cast<std::uint32_t>(
       operation.storage.contactConstraints.size());
@@ -10215,6 +10330,193 @@ void resolveSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_t
             || (left.bodyB == right.bodyB && left.featureId < right.featureId)));
       });
   }
+  if (isSmallCandidateWorld) {
+    std::sort(operation.storage.contactConstraints.begin(),
+      operation.storage.contactConstraints.begin() + operation.storage.contactConstraintCount,
+      [](const ContactConstraintBlock& left, const ContactConstraintBlock& right) {
+        return left.bodyA < right.bodyA
+          || (left.bodyA == right.bodyA && (left.bodyB < right.bodyB
+            || (left.bodyB == right.bodyB && left.featureId < right.featureId)));
+      });
+  }
+  if (beginSubstep && !firstTickSubstep && solveVelocity && isSmallCandidateWorld) {
+    auto& movingFoundation = operation.storage.movingFoundation;
+    if (movingFoundation.size() < bodyCount) {
+      movingFoundation.resize(bodyCount, 0);
+    } else {
+      std::fill(movingFoundation.begin(), movingFoundation.begin() + bodyCount, 0);
+    }
+    for (std::uint32_t index = 0; index < operation.storage.contactConstraintCount; ++index) {
+      const ContactConstraintBlock& constraint = operation.storage.contactConstraints[index];
+      const std::uint32_t bA = constraint.bodyA;
+      const std::uint32_t bB = constraint.bodyB;
+      if (bA >= bodyCount || bB >= bodyCount) continue;
+      if (std::abs(constraint.normal[1]) < 0.7) continue;
+      const Body& a = bodies[bA];
+      const Body& b = bodies[bB];
+      if (!a.dynamic && b.dynamic && (b.vx * b.vx + b.vz * b.vz >= 0.01 * 0.01)) {
+        movingFoundation[bB] = 1;
+      } else if (!b.dynamic && a.dynamic && (a.vx * a.vx + a.vz * a.vz >= 0.01 * 0.01)) {
+        movingFoundation[bA] = 1;
+      }
+    }
+    for (std::uint32_t pass = 0; pass < 12; ++pass) {
+      bool changed = false;
+      for (std::uint32_t index = 0; index < operation.storage.contactConstraintCount; ++index) {
+        const ContactConstraintBlock& constraint = operation.storage.contactConstraints[index];
+        const std::uint32_t bA = constraint.bodyA;
+        const std::uint32_t bB = constraint.bodyB;
+        if (bA >= bodyCount || bB >= bodyCount) continue;
+        if (std::abs(constraint.normal[1]) < 0.7) continue;
+        const Body& a = bodies[bA];
+        const Body& b = bodies[bB];
+        if (a.y > b.y + 0.1 && movingFoundation[bB] && !movingFoundation[bA]) {
+          movingFoundation[bA] = 1;
+          changed = true;
+        } else if (b.y > a.y + 0.1 && movingFoundation[bA] && !movingFoundation[bB]) {
+          movingFoundation[bB] = 1;
+          changed = true;
+        }
+      }
+      if (!changed) break;
+    }
+
+    std::uint32_t cachedFirst = std::numeric_limits<std::uint32_t>::max();
+    std::uint32_t cachedSecond = std::numeric_limits<std::uint32_t>::max();
+    Collision cachedContact{};
+    std::uint32_t usedPointMask = 0;
+    bool anyFeatureIdChanged = false;
+    for (std::uint32_t index = 0; index < operation.storage.contactConstraintCount; ++index) {
+      ContactConstraintBlock& constraint = operation.storage.contactConstraints[index];
+      if (constraint.preserveForCache) continue;
+      const std::uint32_t bodyA = constraint.bodyA;
+      const std::uint32_t bodyB = constraint.bodyB;
+      if (bodyA >= bodyCount || bodyB >= bodyCount) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+      Body& a = bodies[bodyA];
+      Body& b = bodies[bodyB];
+      const bool movableA = a.dynamic && !a.sleeping;
+      const bool movableB = b.dynamic && !b.sleeping;
+      if (!movableA && !movableB) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+      if (!bodiesCanCollide(a, b) || collisionDisabled(operation, bodyA, bodyB)) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+      const bool isPlanarPair = (a.shape == ShapeKind::Box && b.shape == ShapeKind::Box);
+      if (!isPlanarPair) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+
+      const double relVx = a.vx - b.vx;
+      const double relVy = a.vy - b.vy;
+      const double relVz = a.vz - b.vz;
+      const double relHorizSpeedSq = relVx * relVx + relVz * relVz;
+      const bool isDynamicMotion = (a.dynamic && a.vy < -1.5) || (b.dynamic && b.vy < -1.5);
+      const double maxSubstepImpulse = (!a.dynamic || !b.dynamic) ? 1.25 : 0.75;
+      if (isDynamicMotion || movingFoundation[bodyA] || movingFoundation[bodyB] || relHorizSpeedSq >= 0.55 * 0.55) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+
+      if (cachedFirst != bodyA || cachedSecond != bodyB) {
+        cachedFirst = bodyA;
+        cachedSecond = bodyB;
+        cachedContact = collide(operation, a, b);
+        usedPointMask = 0;
+      }
+      const Collision& contact = cachedContact;
+      if (!contact.hit || std::abs(contact.ny) < 0.998) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+
+      std::uint32_t pointIndex = contactPointCount(contact);
+      for (std::uint32_t candidate = 0; candidate < contactPointCount(contact); candidate += 1) {
+        if ((usedPointMask & (1u << candidate)) != 0) continue;
+        if (contactFeatureId(contact, candidate) == constraint.featureId) {
+          pointIndex = candidate;
+          break;
+        }
+      }
+      const Vec3 cachedPoint{constraint.point[0], constraint.point[1], constraint.point[2]};
+      if (pointIndex == contactPointCount(contact) && finite(cachedPoint.x) && finite(cachedPoint.y) && finite(cachedPoint.z)) {
+        constexpr double kMaxCornerDriftSquared = 0.02 * 0.02;
+        double bestDistanceSquared = kMaxCornerDriftSquared;
+        for (std::uint32_t candidate = 0; candidate < contactPointCount(contact); candidate += 1) {
+          if ((usedPointMask & (1u << candidate)) != 0) continue;
+          const Vec3 candidatePoint = contactPoint(contact, a, b, candidate);
+          const Vec3 delta = subtractVec3(candidatePoint, cachedPoint);
+          const double distanceSquared = dotVec3(delta, delta);
+          if (distanceSquared <= bestDistanceSquared) {
+            bestDistanceSquared = distanceSquared;
+            pointIndex = candidate;
+          }
+        }
+      }
+      if (pointIndex == contactPointCount(contact)) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+      usedPointMask |= (1u << pointIndex);
+      const std::uint32_t matchedFeatureId = contactFeatureId(contact, pointIndex);
+      if (constraint.featureId != matchedFeatureId) {
+        constraint.featureId = matchedFeatureId;
+        anyFeatureIdChanged = true;
+      }
+      const Vec3 point = contactPoint(contact, a, b, pointIndex);
+      const auto contactLevers = triangle_adapter_private::collisionLevers(
+        operation, contact, pointIndex, a, b, point);
+      const Vec3 rA = contactLevers[0];
+      const Vec3 rB = contactLevers[1];
+      const Vec3 normal{contact.nx, contact.ny, contact.nz};
+      const double closingSpeed = -dotVec3(contactRelativeVelocity(a, b, rA, rB), normal);
+      if (closingSpeed >= 2.0) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+      if (contactPointCount(contact) == 1 && (rA.x * rA.x + rA.z * rA.z >= 0.20 * 0.20)) {
+        constraint.impulses.fill(0.0);
+        continue;
+      }
+
+      std::array<double, 3> laterImpulses = constraint.impulses;
+      laterImpulses[0] = std::min(laterImpulses[0], maxSubstepImpulse);
+
+      const auto prepared = prepareWarmContactApplication(
+        a, b, movableA, movableB, rA, rB, normal, laterImpulses, true);
+      applyWarmContactApplication(a, b, movableA, movableB, rA, rB, prepared);
+      constraint.impulses = prepared.impulses;
+      constraint.normal = {normal.x, normal.y, normal.z};
+      constraint.point = {point.x, point.y, point.z};
+      const std::uint32_t orderedFirst = std::min(bodyA, bodyB);
+      const std::uint32_t orderedSecond = std::max(bodyA, bodyB);
+      const std::uint32_t outputIndex = recordWorldContactOutput(
+        operation, orderedFirst, orderedSecond, contact);
+      const std::uint32_t telemetryIndex = recordContactTelemetry(
+        operation, orderedFirst, orderedSecond, contact);
+      if (outputIndex == std::numeric_limits<std::uint32_t>::max()
+        || telemetryIndex == std::numeric_limits<std::uint32_t>::max()) {
+        operation.storage.contactConstraintOverflow = true;
+        return;
+      }
+      accumulateContactOutput(operation, outputIndex, telemetryIndex, prepared.impulse);
+    }
+    if (anyFeatureIdChanged) {
+      std::sort(operation.storage.contactConstraints.begin(),
+        operation.storage.contactConstraints.begin() + operation.storage.contactConstraintCount,
+        [](const ContactConstraintBlock& left, const ContactConstraintBlock& right) {
+          return left.bodyA < right.bodyA
+            || (left.bodyA == right.bodyA && (left.bodyB < right.bodyB
+              || (left.bodyB == right.bodyB && left.featureId < right.featureId)));
+        });
+    }
+  }
   const std::uint32_t seededConstraintCount = operation.storage.contactConstraintCount;
   // A proxy and its source are one rigid body, with one support flag.
   const SubstepConstraintHook* const proxyHook = operation.proxyHook;