@series-inc/rundot-syncplay 6.0.0-rc.7 → 6.0.0-rc.9

package/core/native/physics/physics.cpp

@@ -2686,6 +2686,8 @@ BroadphaseBounds bodyBounds(SolverOperation& operation, const Body& body) {
      (geometry.maxZ - geometry.minZ) / 2.0});
 }
 
+BroadphaseBounds queryProxyBounds(SolverOperation& operation, const Body& body);
+
 bool boundsOverlap(const BroadphaseBounds& left, const BroadphaseBounds& right) {
   return left.minX <= right.maxX && right.minX <= left.maxX
     && left.minY <= right.maxY && right.minY <= left.maxY
@@ -2861,9 +2863,10 @@ public:
   }
 
   template <typename Callback>
-  void forEachPair(const Body*, Callback&& callback) {
+  void forEachPair(const Body* bodies, Callback&& callback) {
     if (root == -1) return;
     for (std::uint32_t bodyIndex = 0; bodyIndex < bodyCount; bodyIndex += 1) {
+      const bool bodyStatic = bodies && !bodies[bodyIndex].dynamic && !bodies[bodyIndex].kinematic;
       const BroadphaseBounds actualLeafBounds = actualBounds[bodyIndex];
       std::uint32_t matchCount = 0;
       queryStackCount = 1;
@@ -2874,6 +2877,7 @@ public:
         if (!boundsOverlap(actualLeafBounds, node.bounds)) continue;
         if (node.leaf) {
           if (node.bodyIndex > bodyIndex
+            && !(bodyStatic && bodies && !bodies[node.bodyIndex].dynamic && !bodies[node.bodyIndex].kinematic)
             && boundsOverlap(actualLeafBounds, actualBounds[node.bodyIndex])) {
             queryMatches[matchCount++] = node.bodyIndex;
           }
@@ -2887,12 +2891,13 @@ public:
     }
   }
 
-  void collectPairs() {
+  void collectPairs(const Body* bodies = nullptr) {
     pairCount = 0;
     pairOverflow = false;
     pairsReady = true;
     if (root == -1) return;
     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;
       const std::uint32_t pairStart = pairCount;
       queryStackCount = 1;
@@ -2903,6 +2908,7 @@ public:
         if (!boundsOverlap(actualLeafBounds, node.bounds)) continue;
         if (node.leaf) {
           if (node.bodyIndex > bodyIndex
+            && !(bodyStatic && bodies && !bodies[node.bodyIndex].dynamic && !bodies[node.bodyIndex].kinematic)
             && boundsOverlap(actualLeafBounds, nodes[leafNodes[node.bodyIndex]].bounds)) {
             if (pairCount >= pairCapacity) {
               pairOverflow = true;
@@ -3342,9 +3348,11 @@ void forEachSweepPair(SolverOperation& operation, const Body* bodies, std::uint3
   prepareBroadphase(operation, bodies, bodyCount);
   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;
     for (std::uint32_t rightOrder = leftOrder + 1; rightOrder < bodyCount; rightOrder += 1) {
       const std::uint32_t right = operation.storage.broadphaseOrder[rightOrder];
       if (operation.storage.broadphaseBounds[right].minX > operation.storage.broadphaseBounds[left].maxX) break;
+      if (leftStatic && !bodies[right].dynamic && !bodies[right].kinematic) continue;
       if (boundsOverlap(operation.storage.broadphaseBounds[left], operation.storage.broadphaseBounds[right])) {
         callback(std::min(left, right), std::max(left, right));
       }
@@ -3358,9 +3366,11 @@ void collectSweepPairs(SolverOperation& operation, const Body* bodies, std::uint
   operation.storage.broadphasePairOverflow = false;
   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;
     for (std::uint32_t rightOrder = leftOrder + 1; rightOrder < bodyCount; rightOrder += 1) {
       const std::uint32_t right = operation.storage.broadphaseOrder[rightOrder];
       if (operation.storage.broadphaseBounds[right].minX > operation.storage.broadphaseBounds[left].maxX) break;
+      if (leftStatic && !bodies[right].dynamic && !bodies[right].kinematic) continue;
       if (!boundsOverlap(operation.storage.broadphaseBounds[left], operation.storage.broadphaseBounds[right])) continue;
       if (operation.storage.broadphasePairCount >= operation.storage.broadphasePairFirst.size()) {
         operation.storage.broadphasePairOverflow = true;
@@ -3684,9 +3694,15 @@ Collision collide(SolverOperation& operation, const Body& a, const Body& b) {
     return boxBox(a, b);
   }
   if (a.shape == ShapeKind::Compound && (a.geometryFormat == 4 || a.geometryFormat == 5)) {
+    const BroadphaseBounds boundsA = queryProxyBounds(operation, a);
+    const BroadphaseBounds boundsB = queryProxyBounds(operation, b);
+    if (!boundsOverlap(boundsA, boundsB)) return {};
+
     Collision best;
     for (std::uint32_t child = 0; child < compoundChildCount(operation, a); child += 1) {
       const Body childBody = compoundChildBody(operation, a, child);
+      const BroadphaseBounds childBounds = queryProxyBounds(operation, childBody);
+      if (!boundsOverlap(childBounds, boundsB)) continue;
       const Collision candidate = collide(operation, childBody, b);
       if (candidate.hit && (!best.hit || candidate.penetration > best.penetration)) best = candidate;
     }
@@ -6891,10 +6907,25 @@ std::int32_t runOverlapQuery(SolverOperation& operation,
   const double length = solverSqrt(dotVec3(rawDirection, rawDirection));
   const Vec3 direction = scaleVec3(rawDirection, 1.0 / length);
   const double maxDistance = queryData[17];
+  const BroadphaseBounds baseQueryBounds = queryProxyBounds(operation, queryBody);
+  BroadphaseBounds queryBounds = baseQueryBounds;
+  if (cast && maxDistance > 0.0) {
+    const Vec3 sweepDelta = scaleVec3(direction, maxDistance);
+    if (sweepDelta.x < 0.0) queryBounds.minX += sweepDelta.x; else queryBounds.maxX += sweepDelta.x;
+    if (sweepDelta.y < 0.0) queryBounds.minY += sweepDelta.y; else queryBounds.maxY += sweepDelta.y;
+    if (sweepDelta.z < 0.0) queryBounds.minZ += sweepDelta.z; else queryBounds.maxZ += sweepDelta.z;
+  }
   for (std::uint32_t index = 0; index < bodyCount; index += 1) {
     if (static_cast<std::int32_t>(queryData[20]) == static_cast<std::int32_t>(index)
       || !queryBodyAllowed(operation.storage.soaBodies[index], queryData)) continue;
     Body& target = operation.storage.soaBodies[index];
+    // Mesh, hull, and heightfield queries charge geometry work before a miss.
+    // A body cull would hide that cost. Compounds and primitive shapes still cull.
+    if (target.shape != ShapeKind::Mesh && target.shape != ShapeKind::Hull
+      && target.shape != ShapeKind::Heightfield) {
+      const BroadphaseBounds targetBounds = queryProxyBounds(operation, target);
+      if (!boundsOverlap(queryBounds, targetBounds)) continue;
+    }
     RayHit result;
     const bool useGeometryProxy = (isGeometryShape(queryBody.shape) && isGeometryShape(target.shape))
       || (cast && (target.shape == ShapeKind::Mesh && target.geometryCount % 3 != 0))
@@ -7435,6 +7466,7 @@ void resolveSoAContacts(SolverOperation& operation, Body* bodies, std::uint32_t
                              std::uint32_t preparedBegin,
                              std::uint32_t preparedEnd) {
     if (initialize && collisionDisabled(operation, first, second)) return;
+    if (initialize && !bodies[first].dynamic && !bodies[first].kinematic && !bodies[second].dynamic && !bodies[second].kinematic) return;
     if (!solveVelocity) {
       if (resolveContactPair(operation, bodies[first], bodies[second], options,
         nullptr, nullptr, nullptr, nullptr, nullptr, true,
@@ -7683,7 +7715,7 @@ 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();
+      operation.storage.broadphaseTree->collectPairs(bodies);
     }
   }
   for (std::uint32_t iteration = 0; iteration < velocityIterations; iteration += 1) {
@@ -7839,12 +7871,15 @@ void updateSleepingStates(SolverOperation& operation,
       continue;
     }
     const double threshold = body.sleepThreshold;
-    const double speedSquared = body.vx * body.vx + body.vy * body.vy + body.vz * body.vz
-      + body.angularVelX * body.angularVelX
+    const double linearSpeedSquared = body.vx * body.vx + body.vy * body.vy + body.vz * body.vz;
+    const double angularSpeedSquared = body.angularVelX * body.angularVelX
       + body.angularVelY * body.angularVelY
       + body.angularVelZ * body.angularVelZ;
+    const double angularThreshold = threshold * 2.0;
     if (!body.sleepEnabled || threshold <= 0.0
-      || speedSquared > threshold * threshold || !finite(speedSquared)) {
+      || linearSpeedSquared > threshold * threshold
+      || angularSpeedSquared > angularThreshold * angularThreshold
+      || !finite(linearSpeedSquared) || !finite(angularSpeedSquared)) {
       body.sleepTime = 0.0;
       operation.storage.sleepCanSleep[root] = false;
       continue;