add bbox checks for pathfinding

src/pathfind.js

@@ -83,11 +83,17 @@ export default class Pathfind {
 
     if (radius > 0) {
       for (const waypoint of waypoints) {
-        const collider = Entity.collider(waypoint[0], waypoint[1], radius)
-        const waypointAvailable = !colliders.some((it) => SATX.collideObject(collider, it))
-        if (waypointAvailable) {
-          filteredWaypoints.push(waypoint)
+        const bbox = Entity.bbox(waypoint[0], waypoint[1], radius) // TODO: duplicate bbox calculation logic for speed
+        const bboxCheckedObstacles = colliders.filter((it) => SATX.bboxCheck(bbox, it[0]))
+        if (bboxCheckedObstacles.length > 0) {
+          const collider = Entity.collider(waypoint[0], waypoint[1], radius)
+          const colliding = bboxCheckedObstacles.some((it) => it[1].some((c) => SATX.collideObject(collider, c)))
+          if (colliding) {
+            continue
+          }
         }
+
+        filteredWaypoints.push(waypoint)
       }
     }
 
@@ -111,31 +117,38 @@ export default class Pathfind {
         }
 
         const key = Pathfind.floatKey4(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1])
-        if (!checked.has(key)) {
-          checked.add(key)
-          checked.add(Pathfind.floatKey4(mergedWaypoints[j], mergedWaypoints[j + 1], mergedWaypoints[i], mergedWaypoints[i + 1]))
+        if (checked.has(key)) {
+          continue
+        }
 
-          // TODO: optimize tunnelCollider using bounding boxes
+        checked.add(key)
+        checked.add(Pathfind.floatKey4(mergedWaypoints[j], mergedWaypoints[j + 1], mergedWaypoints[i], mergedWaypoints[i + 1]))
+
+        const bbox = Entity.tunnelBbox(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius) // TODO: duplicate bbox calculation logic for speed
+        const bboxCheckedObstacles = colliders.filter((it) => SATX.bboxCheck(bbox, it[0]))
+        if (bboxCheckedObstacles.length > 0) {
           const tunnel = Entity.tunnelCollider(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius)
-
-          if (!colliders.some((it) => SATX.collideObject(tunnel, it))) {
-            const node = new Float32Array(5)
-            node[0] = mergedWaypoints[i]
-            node[1] = mergedWaypoints[i + 1]
-            node[2] = mergedWaypoints[j]
-            node[3] = mergedWaypoints[j + 1]
-            node[4] = Math.hypot(mergedWaypoints[j] - mergedWaypoints[i], mergedWaypoints[j + 1] - mergedWaypoints[i + 1])
-            nodes.push(node)
-
-            const reverseNode = new Float32Array(5)
-            reverseNode[0] = mergedWaypoints[j]
-            reverseNode[1] = mergedWaypoints[j + 1]
-            reverseNode[2] = mergedWaypoints[i]
-            reverseNode[3] = mergedWaypoints[i + 1]
-            reverseNode[4] = node[4] // distance is the same, copying is less expensive
-            nodes.push(reverseNode)
+          const colliding = bboxCheckedObstacles.some((it) => it[1].some((c) => SATX.collideObject(tunnel, c)))
+          if (colliding) {
+            continue
           }
         }
+
+        const node = new Float32Array(5)
+        node[0] = mergedWaypoints[i]
+        node[1] = mergedWaypoints[i + 1]
+        node[2] = mergedWaypoints[j]
+        node[3] = mergedWaypoints[j + 1]
+        node[4] = Math.hypot(mergedWaypoints[j] - mergedWaypoints[i], mergedWaypoints[j + 1] - mergedWaypoints[i + 1])
+        nodes.push(node)
+
+        const reverseNode = new Float32Array(5)
+        reverseNode[0] = mergedWaypoints[j]
+        reverseNode[1] = mergedWaypoints[j + 1]
+        reverseNode[2] = mergedWaypoints[i]
+        reverseNode[3] = mergedWaypoints[i + 1]
+        reverseNode[4] = node[4] // distance is the same, copying is less expensive
+        nodes.push(reverseNode)
       }
     }