@series-inc/rundot-syncplay 6.0.0-rc.45 → 6.0.0-rc.47

package/core/native/physics/physics.cpp

@@ -11,7 +11,14 @@
 #include <array>
 #include <bit>
 #include <cmath>
+#include <cstdlib>
 #include <cstring>
+
+#if defined(RUNDOT_PHYSICS_DEBUG_INVARIANTS)
+  #define RUNDOT_PHYSICS_DEBUG_ASSERT(expr) do { if (!(expr)) { std::abort(); } } while (0)
+#else
+  #define RUNDOT_PHYSICS_DEBUG_ASSERT(expr) ((void)0)
+#endif
 #include <exception>
 #include <functional>
 #include <limits>
@@ -3813,6 +3820,7 @@ BroadphaseBounds orientedBounds(const Body& body, const Vec3& localCenter, const
 }
 
 constexpr std::uint32_t kBroadphasePairCapacity = 1u << 20;
+enum class BroadphasePairOwner : std::uint8_t { None, Sweep, Tree };
 constexpr std::uint32_t kJointCapacity = 10000;
 static_assert(kJointCapacity <= std::numeric_limits<std::uint16_t>::max());
 constexpr std::uint32_t kContactTelemetryTableCapacity = 1u << 18;
@@ -3835,6 +3843,7 @@ struct SolverStorage {
   std::uint32_t broadphaseOrderCount = 0;
   std::uint32_t broadphasePairCount = 0;
   bool broadphasePairOverflow = false;
+  BroadphasePairOwner broadphasePairOwner = BroadphasePairOwner::None;
   std::vector<Body> soaBodies;
   std::vector<BodyMotionEpoch> motionEpochs;
   std::vector<BodyPose> stagedMotionPoses;
@@ -4024,15 +4033,14 @@ struct DynamicAabbTreeNode {
 
 class FixedDynamicAabbTree {
 public:
-  void prepare(std::uint32_t count, std::uint32_t maximumPairs) {
-    actualBounds.resize(count);
-    queryMatches.resize(count);
-    nodes.resize(static_cast<std::size_t>(count) * 2);
-    leafNodes.resize(count);
-    queryStack.resize(static_cast<std::size_t>(count) * 2);
-    pairFirst.resize(maximumPairs);
-    pairSecond.resize(maximumPairs);
-    pairCapacity = maximumPairs;
+  void prepare(std::uint32_t count, std::span<std::uint32_t> first, std::span<std::uint32_t> second) {
+    if (actualBounds.size() < count) actualBounds.resize(count);
+    if (queryMatches.size() < count) queryMatches.resize(count);
+    if (nodes.size() < static_cast<std::size_t>(count) * 2) nodes.resize(static_cast<std::size_t>(count) * 2);
+    if (leafNodes.size() < count) leafNodes.resize(count);
+    if (queryStack.size() < static_cast<std::size_t>(count) * 2) queryStack.resize(static_cast<std::size_t>(count) * 2);
+    pairFirst = first;
+    pairSecond = second;
   }
 
   void reset(std::uint32_t count) {
@@ -4045,7 +4053,7 @@ public:
   }
 
   void rebuild(std::span<const BroadphaseBounds> bounds) {
-    prepare(static_cast<std::uint32_t>(bounds.size()), 0);
+    prepare(static_cast<std::uint32_t>(bounds.size()), {}, {});
     reset(static_cast<std::uint32_t>(bounds.size()));
     std::copy(bounds.begin(), bounds.end(), actualBounds.begin());
     for (std::uint32_t index = 0; index < bounds.size(); index += 1) {
@@ -4179,6 +4187,7 @@ public:
     pairOverflow = false;
     pairsReady = true;
     if (root == -1) return;
+    const std::uint32_t pairCapacity = static_cast<std::uint32_t>(std::min(pairFirst.size(), pairSecond.size()));
     for (std::uint32_t bodyIndex = 0; bodyIndex < bodyCount; bodyIndex += 1) {
       const bool bodyStatic = bodies && !bodies[bodyIndex].dynamic && !bodies[bodyIndex].kinematic;
       const BroadphaseBounds actualLeafBounds = nodes[leafNodes[bodyIndex]].bounds;
@@ -4190,9 +4199,9 @@ public:
         const DynamicAabbTreeNode& node = nodes[index];
         if (!boundsOverlap(actualLeafBounds, node.bounds)) continue;
         if (node.leaf) {
+          RUNDOT_PHYSICS_DEBUG_ASSERT(leafNodes[node.bodyIndex] == index);
           if (node.bodyIndex > bodyIndex
-            && !(bodyStatic && bodies && !bodies[node.bodyIndex].dynamic && !bodies[node.bodyIndex].kinematic)
-            && boundsOverlap(actualLeafBounds, nodes[leafNodes[node.bodyIndex]].bounds)) {
+            && !(bodyStatic && bodies && !bodies[node.bodyIndex].dynamic && !bodies[node.bodyIndex].kinematic)) {
             if (pairCount >= pairCapacity) {
               pairOverflow = true;
               return;
@@ -4249,9 +4258,7 @@ public:
       + static_cast<std::uint64_t>(queryMatches.capacity()) * sizeof(std::uint32_t)
       + static_cast<std::uint64_t>(nodes.capacity()) * sizeof(DynamicAabbTreeNode)
       + static_cast<std::uint64_t>(leafNodes.capacity()) * sizeof(std::int32_t)
-      + static_cast<std::uint64_t>(queryStack.capacity()) * sizeof(std::int32_t)
-      + static_cast<std::uint64_t>(pairFirst.capacity()) * sizeof(std::uint32_t)
-      + static_cast<std::uint64_t>(pairSecond.capacity()) * sizeof(std::uint32_t);
+      + static_cast<std::uint64_t>(queryStack.capacity()) * sizeof(std::int32_t);
   }
 
 private:
@@ -4392,9 +4399,8 @@ private:
   std::vector<DynamicAabbTreeNode> nodes;
   std::vector<std::int32_t> leafNodes;
   std::vector<std::int32_t> queryStack;
-  std::vector<std::uint32_t> pairFirst;
-  std::vector<std::uint32_t> pairSecond;
-  std::uint32_t pairCapacity = 0;
+  std::span<std::uint32_t> pairFirst;
+  std::span<std::uint32_t> pairSecond;
   std::uint32_t bodyCount = 0;
   std::uint32_t nodeCount = 0;
   std::uint32_t queryStackCount = 0;
@@ -4419,6 +4425,7 @@ void SolverStorage::invalidate() noexcept {
   broadphaseOrderCount = 0;
   broadphasePairCount = 0;
   broadphasePairOverflow = false;
+  broadphasePairOwner = BroadphasePairOwner::None;
   contactTelemetryCount = 0;
   worldContactOutputCount = 0;
   contactConstraintCount = 0;
@@ -4582,7 +4589,10 @@ void prepareStepStorage(SolverStorage& storage, std::uint32_t bodyCount, std::ui
   storage.sleepHasKinematic.resize(bodyCount);
   storage.sleepHasStaticAnchor.resize(bodyCount);
   storage.bodyJointAffected.assign(bodyCount, 0);
-  storage.broadphaseTree->prepare(bodyCount, pairCapacity);
+  storage.broadphaseTree->prepare(
+    bodyCount,
+    std::span<std::uint32_t>(storage.broadphasePairFirst),
+    std::span<std::uint32_t>(storage.broadphasePairSecond));
 }
 
 void prepareQueryStorage(SolverStorage& storage, std::uint32_t bodyCount) {
@@ -4676,6 +4686,7 @@ void collectSweepPairs(SolverOperation& operation, const Body* bodies, std::uint
   prepareBroadphase(operation, bodies, bodyCount);
   operation.storage.broadphasePairCount = 0;
   operation.storage.broadphasePairOverflow = false;
+  operation.storage.broadphasePairOwner = BroadphasePairOwner::Sweep;
   for (std::uint32_t leftOrder = 0; leftOrder < bodyCount; leftOrder += 1) {
     const std::uint32_t left = operation.storage.broadphaseOrder[leftOrder];
     const bool leftStatic = !bodies[left].dynamic && !bodies[left].kinematic;
@@ -4697,6 +4708,7 @@ void collectSweepPairs(SolverOperation& operation, const Body* bodies, std::uint
 
 template <typename Callback>
 void forEachCollectedSweepPair(SolverOperation& operation, Callback&& callback) {
+  RUNDOT_PHYSICS_DEBUG_ASSERT(operation.storage.broadphasePairOwner == BroadphasePairOwner::Sweep);
   for (std::uint32_t index = 0; index < operation.storage.broadphasePairCount; index += 1) {
     callback(operation.storage.broadphasePairFirst[index], operation.storage.broadphasePairSecond[index]);
   }
@@ -9808,6 +9820,16 @@ void finalizeContactCache(SolverOperation& operation) {
 }
 
 
+bool treePairsReady(const SolverOperation& operation) {
+  return operation.storage.broadphasePairOwner == BroadphasePairOwner::Tree
+    && operation.storage.broadphaseTree->pairsAreReady();
+}
+
+void collectTreePairs(SolverOperation& operation, const Body* bodies) {
+  operation.storage.broadphaseTree->collectPairs(bodies);
+  operation.storage.broadphasePairOwner = BroadphasePairOwner::Tree;
+}
+
 void discoverContactWakes(
     SolverOperation& operation, Body* bodies, std::uint32_t bodyCount) {
   const auto visit = [&](std::uint32_t first, std::uint32_t second) {
@@ -9841,6 +9863,7 @@ void discoverContactWakes(
   if (operation.storage.broadphaseTree->pairsOverflowed()) {
     operation.storage.broadphaseTree->forEachPair(bodies, visit);
   } else {
+    RUNDOT_PHYSICS_DEBUG_ASSERT(operation.storage.broadphasePairOwner == BroadphasePairOwner::Tree);
     operation.storage.broadphaseTree->forEachCollectedPair(visit);
   }
 }
@@ -10241,8 +10264,8 @@ void resolveSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_t
   }
   if (beginSubstep) {
     operation.storage.broadphaseTree->ensure(operation, bodies, bodyCount);
-    if (!operation.storage.broadphaseTree->pairsAreReady()) {
-      operation.storage.broadphaseTree->collectPairs(bodies);
+    if (!treePairsReady(operation)) {
+      collectTreePairs(operation, bodies);
     }
   }
   for (std::uint32_t iteration = 0; iteration < velocityIterations; iteration += 1) {
@@ -10255,6 +10278,7 @@ void resolveSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_t
       if (operation.storage.broadphaseTree->pairsOverflowed()) {
         operation.storage.broadphaseTree->forEachPair(bodies, solveCollectedPair);
       } else {
+        RUNDOT_PHYSICS_DEBUG_ASSERT(operation.storage.broadphasePairOwner == BroadphasePairOwner::Tree);
         operation.storage.broadphaseTree->forEachCollectedPair(solveCollectedPair);
       }
     } else {
@@ -10569,8 +10593,8 @@ void updateJointIslandMembership(SolverOperation& operation,
     collectSweepPairs(operation, bodies, bodyCount);
   } else {
     operation.storage.broadphaseTree->ensure(operation, bodies, bodyCount);
-    if (!operation.storage.broadphaseTree->pairsAreReady()) {
-      operation.storage.broadphaseTree->collectPairs(bodies);
+    if (!treePairsReady(operation)) {
+      collectTreePairs(operation, bodies);
     }
   }
 
@@ -10619,6 +10643,7 @@ void updateJointIslandMembership(SolverOperation& operation,
     if (operation.storage.broadphaseTree->pairsOverflowed()) {
       operation.storage.broadphaseTree->forEachPair(bodies, unionDynamicPair);
     } else {
+      RUNDOT_PHYSICS_DEBUG_ASSERT(operation.storage.broadphasePairOwner == BroadphasePairOwner::Tree);
       operation.storage.broadphaseTree->forEachCollectedPair(unionDynamicPair);
     }
   }
@@ -10653,6 +10678,7 @@ void updateJointIslandMembership(SolverOperation& operation,
     if (operation.storage.broadphaseTree->pairsOverflowed()) {
       operation.storage.broadphaseTree->forEachPair(bodies, markMovingNeighbor);
     } else {
+      RUNDOT_PHYSICS_DEBUG_ASSERT(operation.storage.broadphasePairOwner == BroadphasePairOwner::Tree);
       operation.storage.broadphaseTree->forEachCollectedPair(markMovingNeighbor);
     }
   }