fix some pathfinding problems

src/pathfind.js

@@ -7,7 +7,7 @@ export default class Pathfind {
     return `${pos.x},${pos.y}`
   }
 
-  static shortestPath(graph, start, goal) {
+  static shortestPath(graph, start, goal, radius = 0, colliders = []) {
     const queue = new PriorityQueue((a, b) => a[1] < b[1])
     const visited = new Map()
 
@@ -26,7 +26,17 @@ export default class Pathfind {
       if (!visited.has(key) || visited.get(key) > cost) {
         visited.set(key, cost)
 
-        for (const { to, direction, distance } of graph.filter(e => e.from.equals(waypoint))) {
+        for (const { from, to, direction, reverseDirection, distance } of graph.filter(e => e.from.equals(waypoint))) {
+          if (radius > 0 && colliders.length > 0) {
+            const adjustedFrom = from.clone().add(reverseDirection.clone().multiplyScalar(radius))
+            const adjustedTo = to.clone().add(direction.clone().multiplyScalar(radius))
+            const colliding = SATX.collideObjects(SATX.entityTunnel(adjustedFrom, adjustedTo, radius), colliders)
+
+            if (colliding) {
+              continue
+            }
+          }
+
           const keyTo = this.keyFor(to)
           if (!visited.has(keyTo) || visited.get(keyTo) > cost + distance) {
             queue.push([[...path, [to, direction]], cost + distance])
@@ -57,8 +67,8 @@ export default class Pathfind {
 
           if (lineOfSight) {
             const distance = from.distanceTo(to)
-            graph.push({ from, to, distance, direction })
-            graph.push({ to: from, from: to, distance, direction: reverseDirection })
+            graph.push({ from, to, distance, direction, reverseDirection })
+            graph.push({ to: from, from: to, distance, direction: reverseDirection, reverseDirection: direction })
           }
         }
       }