disable collision to fix pathfinding phasing through walls

src/entity.js

@@ -2,6 +2,7 @@ import { Vector2 } from 'three'
 import SAT from 'sat'
 import SATX from './satx.js'
 import Pathfind from './pathfind.js'
+import Terrain from './terrain.js'
 
 export default class Entity {
   id = crypto.randomUUID()
@@ -94,13 +95,18 @@ export default class Entity {
       if (this.#path.length < 1 || !this.#path.at(-1).equals(this.#dest)) {
         console.time('pathfinding')
         console.time('waypoints')
-        const waypoints = (this.game?.unadjustedWaypoints.map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []).concat([this.position, fixedDest])
+        const extraWaypoints = [[this.position, new Vector2()], [fixedDest, new Vector2()]]
+        const waypoints = (this.game?.unadjustedWaypoints ?? []).concat(extraWaypoints)
         console.timeEnd('waypoints')
-        console.time('graph')
+        console.time('extraEntries')
-        const graph = Pathfind.buildGraph(waypoints, this.collidables(), this.radius)
+        const extraGraphEntries = Pathfind.buildGraph(waypoints, this.collidables(), extraWaypoints)
-        console.timeEnd('graph')
+        console.timeEnd('extraEntries')
+        console.time('unadjustedGraph')
+        const unadjustedGraph = (this.game?.waypointGraph ?? []).concat(extraGraphEntries)
+        console.timeEnd('unadjustedGraph')
         console.time('path')
-        this.#path = Pathfind.shortestPath(graph, this.position, fixedDest)
+        this.#path = Pathfind.shortestPath(unadjustedGraph, this.position, fixedDest).map(([unadjusted, direction]) => unadjusted.clone().add(direction.clone().multiplyScalar(this.radius)))
+        console.log(this.#path)
         console.timeEnd('path')
         console.timeEnd('pathfinding')
       }
@@ -113,11 +119,14 @@ export default class Entity {
         const position = distance <= speed ? destination : stepTaken
   
         const collider = Entity.collider(position.x, position.y, this.radius)
-        const isColliding = SATX.collideObjects(collider, this.collidables())
+        // TODO: fix collisions while pathfinding
+        // const isColliding = SATX.collideObjects(collider, this.collidables())
   
-        if (!isColliding) {
+        // if (!isColliding) {
-          this.position.copy(position)
+        //   this.position.copy(position)
-        }
+        // }
+  
+        this.position.copy(position)
 
         if (this.position.equals(destination)) {
           this.#path = this.#path.slice(1)

src/game.js

@@ -1,4 +1,5 @@
 import { EventEmitter } from 'node:events'
+import Pathfind from './pathfind.js'
 
 export default class Game {
   tickRate = 30
@@ -10,14 +11,23 @@ export default class Game {
   #eventEmitter = new EventEmitter()
   #terrains = []
   #tickBudget = Math.floor(1000 / this.tickRate)
+  #waypointGraph = []
 
   get entities() { return this.#entities }
   get eventEmitter() { return this.#eventEmitter }
   get terrains() { return this.#terrains }
   get tickBudget() { return this.#tickBudget }
+  get waypointGraph() { return this.#waypointGraph }
 
   get unadjustedWaypoints() {
-    return this.terrains.map((t) => t.unadjustedWaypoints).concat(this.entities.map((e) => e.unadjustedWaypoints)).flat()
+    // const terrainAndEntities = this.entities.concat(this.terrains)
+    const terrainAndEntities = this.terrains
+    return terrainAndEntities.map((e) => e.unadjustedWaypoints).flat()
+  }
+
+  colliders() {
+    const terrainAndEntities = this.entities.concat(this.terrains)
+    return terrainAndEntities.map((e) => e.colliders).flat()
   }
 
   spawn_entity(entity) {
@@ -32,6 +42,7 @@ export default class Game {
 
   add_terrain(terrain) {
     this.#terrains.push(terrain)
+    this.#calculateWaypointGraph()
   }
 
   state() {
@@ -53,4 +64,8 @@ export default class Game {
     this.currentTick++
     this.eventEmitter.emit('tick')
   }
+
+  #calculateWaypointGraph() {
+    this.#waypointGraph = Pathfind.buildGraph(this.unadjustedWaypoints, this.colliders())
+  }
 }

src/index.js

@@ -52,6 +52,14 @@ app.listen(port, () => {
   entity2.radius = 50
   game.spawn_entity(entity2)
 
+  // const triangle = new Terrain([
+  //   { x: 400, y: 200 },
+  //   { x: 400, y: 600 },
+  //   { x: 600, y: 300 },
+  // ])
+  // triangle.id = 'triangle'
+  // game.add_terrain(triangle)
+
   const horseshoe = new Terrain([
     { x: 400, y: 200 },
     { x: 600, y: 200 },
@@ -107,5 +115,7 @@ app.listen(port, () => {
   pole.id = 'pole'
   game.add_terrain(pole)
 
+  entity1.moveAction(entity1.x + 600, entity1.y)
+
   game.start()
 })

src/pathfind.js

@@ -3,28 +3,33 @@ import PriorityQueue from "./priority-queue.js"
 import SATX from "./satx.js"
 
 export default class Pathfind {
+  static keyFor(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()
 
-    queue.push([[start], 0])
+    queue.push([[[start, null]], 0])
 
     while (!queue.isEmpty()) {
       const [path, cost] = queue.pop()
-      const waypoint = path.at(-1)
+      const waypoint = path.at(-1).at(0)
 
       if (waypoint.equals(goal)) {
         path.shift()
         return path
       }
 
-      if (!visited.has(key(waypoint)) || visited.get(key(waypoint)) > cost) {
+      const key = this.keyFor(waypoint)
-        visited.set(key(waypoint), cost)
+      if (!visited.has(key) || visited.get(key) > cost) {
+        visited.set(key, cost)
 
-        for (const { to, distance } of graph.filter(e => e.from.equals(waypoint))) {
+        for (const { to, direction, distance } of graph.filter(e => e.from.equals(waypoint))) {
-          if (!visited.has(key(to)) || visited.get(key(to)) > cost + distance) {
+          const keyTo = this.keyFor(to)
-            queue.push([[...path, to], cost + distance])
+          if (!visited.has(keyTo) || visited.get(keyTo) > cost + distance) {
+            queue.push([[...path, [to, direction]], cost + distance])
           }
         }
       }
@@ -33,24 +38,28 @@ export default class Pathfind {
     return []
   }
 
-  static buildGraph(waypoints = [], colliders = [], radius = 0) {
+  static buildGraph(toWaypoints, colliders, fromWaypoints = null) {
     const graph = []
+    const calculated = new Set()
 
-    for (const from of waypoints) {
+    for (const [from, reverseDirection] of (fromWaypoints ?? toWaypoints)) {
-      for (const to of waypoints) {
+      for (const [to, direction] of toWaypoints) {
         if (from.equals(to)) {
           continue
         }
 
-        const tunnel = SATX.entityTunnel(from, to, radius)
+        const key = `${from.x},${from.y};${to.x},${to.y}`
-        const collider = Entity.collider(from.x, from.y, radius)
+        if (!calculated.has(key)) {
+          calculated.add(key)
+          calculated.add(`${to.x},${to.y};${from.x},${from.y}`)
 
-        const tunnelClear = !SATX.collideObjects(tunnel, colliders)
+          const lineOfSight = !SATX.collideObjects(SATX.line(from, to), colliders)
-        const waypointAvailable = !SATX.collideObjects(collider, colliders)
 
-        if (waypointAvailable && tunnelClear) {
+          if (lineOfSight) {
-          const distance = from.distanceTo(to)
+            const distance = from.distanceTo(to)
-          graph.push({ from, to, distance })
+            graph.push({ from, to, distance, direction })
+            graph.push({ to: from, from: to, distance, direction: reverseDirection })
+          }
         }
       }
     }