check bi-directional paths in graph building

src/pathfind.js

@@ -3,8 +3,11 @@ import PriorityQueue from "./priority-queue.js"
 import SATX from "./satx.js"
 
 export default class Pathfind {
+  static key(pos) {
+    return `${pos.x},${pos.y}`
+  }
+
   static shortestPath(graph, start, goal) {
-    const key = (pos) => `${pos.x},${pos.y}`
     const queue = new PriorityQueue((a, b) => a[1] < b[1])
     const visited = new Map()
 
@@ -19,11 +22,13 @@ export default class Pathfind {
         return path
       }
 
-      if (!visited.has(key(waypoint)) || visited.get(key(waypoint)) > cost) {
-        visited.set(key(waypoint), cost)
+      const waypointKey = this.key(waypoint)
+      if (!visited.has(waypointKey) || visited.get(waypointKey) > cost) {
+        visited.set(waypointKey, cost)
 
         for (const { to, distance } of graph.filter(e => e.from.equals(waypoint))) {
-          if (!visited.has(key(to)) || visited.get(key(to)) > cost + distance) {
+          const toKey = this.key(to)
+          if (!visited.has(toKey) || visited.get(toKey) > cost + distance) {
             queue.push([[...path, to], cost + distance])
           }
         }
@@ -35,6 +40,7 @@ export default class Pathfind {
 
   static buildGraph(waypoints = [], colliders = [], radius = 0) {
     const graph = []
+    const calculated = new Set()
 
     for (const from of waypoints) {
       for (const to of waypoints) {
@@ -42,15 +48,22 @@ export default class Pathfind {
           continue
         }
 
-        const tunnel = SATX.entityTunnel(from, to, radius)
-        const collider = Entity.collider(from.x, from.y, radius)
+        const key = `${from.x},${from.y};${to.x},${to.y}`
+        if (!calculated.has(key)) {
+          calculated.add(key)
+          calculated.add(`${to.x},${to.y};${from.x},${from.y}`)
 
-        const tunnelClear = !SATX.collideObjects(tunnel, colliders)
-        const waypointAvailable = !SATX.collideObjects(collider, colliders)
+          const tunnel = SATX.entityTunnel(from, to, radius)
+          const collider = Entity.collider(from.x, from.y, radius)
 
-        if (waypointAvailable && tunnelClear) {
-          const distance = from.distanceTo(to)
-          graph.push({ from, to, distance })
+          const tunnelClear = !SATX.collideObjects(tunnel, colliders)
+          const waypointAvailable = !SATX.collideObjects(collider, colliders)
+
+          if (waypointAvailable && tunnelClear) {
+            const distance = from.distanceTo(to)
+            graph.push({ from, to, distance })
+            graph.push({ from: to, to: from, distance })
+          }
         }
       }
     }