check bi-directional paths in graph building

src/entity.js

@@ -88,20 +88,21 @@ export default class Entity {
 
   async takeStep(distanceTraveled = 0) {
     const speed = (this.speed / (this.game?.tickBudget ?? 1000)) - distanceTraveled
+    const collidables = this.collidables()
     if (this.#dest != null) {
-      const fixedDest = SATX.clamp(SATX.fixCollisions(this.#dest, this.collidables(), this.radius), this.game?.width, this.game?.height, this.radius)
+      const fixedDest = SATX.clamp(SATX.fixCollisions(this.#dest, collidables, this.radius), this.game?.width, this.game?.height, this.radius)
 
-      if (this.#path.length < 1 || !this.#path.at(-1).equals(this.#dest)) {
+      if (this.#path.length < 1 || !this.#path.at(-1).equals(fixedDest)) {
         console.time('pathfinding')
-        console.time('waypoints')
+        // console.time('waypoints')
-        const waypoints = (this.game?.unadjustedWaypoints.map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []).concat([this.position, fixedDest])
+        const waypoints = this.waypoints().concat([this.position, fixedDest])
-        console.timeEnd('waypoints')
+        // console.timeEnd('waypoints')
-        console.time('graph')
+        // console.time('graph')
-        const graph = Pathfind.buildGraph(waypoints, this.collidables(), this.radius)
+        const graph = Pathfind.buildGraph(waypoints, collidables, this.radius)
-        console.timeEnd('graph')
+        // console.timeEnd('graph')
-        console.time('path')
+        // console.time('path')
         this.#path = Pathfind.shortestPath(graph, this.position, fixedDest)
-        console.timeEnd('path')
+        // console.timeEnd('path')
         console.timeEnd('pathfinding')
       }
 
@@ -137,4 +138,8 @@ export default class Entity {
       this.takeStep(),
     ])
   }
+
+  waypoints() {
+    return this.game?.unadjustedWaypoints.map(([waypoint, direction]) => waypoint.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []
+  }
 }

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) {
+      const waypointKey = this.key(waypoint)
-        visited.set(key(waypoint), cost)
+      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,6 +48,11 @@ export default class Pathfind {
           continue
         }
 
+        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 tunnel = SATX.entityTunnel(from, to, radius)
           const collider = Entity.collider(from.x, from.y, radius)
 
@@ -51,6 +62,8 @@ export default class Pathfind {
           if (waypointAvailable && tunnelClear) {
             const distance = from.distanceTo(to)
             graph.push({ from, to, distance })
+            graph.push({ from: to, to: from, distance })
+          }
         }
       }
     }