From 2a9ef691fe311f780d31f8cbc5cfc5dc8e6b2210 Mon Sep 17 00:00:00 2001
From: Thayol <thayol@zovguran.net>
Date: Wed, 25 Dec 2024 09:33:07 +0900
Subject: [PATCH] fix some pathfinding problems

---
 src/entity.js   |  2 +-
 src/pathfind.js | 18 ++++++++++++++----
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/src/entity.js b/src/entity.js
index 537cb73..9759481 100644
--- a/src/entity.js
+++ b/src/entity.js
@@ -105,7 +105,7 @@ export default class Entity {
         const unadjustedGraph = (this.game?.waypointGraph ?? []).concat(extraGraphEntries)
         console.timeEnd('unadjustedGraph')
         console.time('path')
-        this.#path = Pathfind.shortestPath(unadjustedGraph, this.position, fixedDest).map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius)))
+        this.#path = Pathfind.shortestPath(unadjustedGraph, this.position, fixedDest, this.radius, this.collidables()).map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius)))
         console.log(this.#path)
         console.timeEnd('path')
         console.timeEnd('pathfinding')
diff --git a/src/pathfind.js b/src/pathfind.js
index 365d03a..9943240 100644
--- a/src/pathfind.js
+++ b/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 })
           }
         }
       }