fix pathfinding nekimegyafalnak style

public/client.js

@@ -13,12 +13,14 @@ camera.rotation.set((60 / 180) * Math.PI, 0, 0)
 const entityMaterial = new THREE.MeshToonMaterial({ color: 0xffffff })
 const terrainMaterial = new THREE.MeshToonMaterial({ color: 0xffd700 })
 
-const minimapCamera = new THREE.OrthographicCamera(-10, 10, 10, -10)
+// const minimapCamera = new THREE.OrthographicCamera(-10, 10, 10, -10)
+const minimapCamera = new THREE.OrthographicCamera(-6, 6, 6, -6)
 const minimapRenderer = new THREE.WebGLRenderer()
 
 minimapRenderer.setSize(600, 600)
 minimapRenderer.setAnimationLoop(minimapRender)
-minimapCamera.position.set(10, 10, 10)
+// minimapCamera.position.set(10, 10, 10)
+minimapCamera.position.set(6, 6, 6)
 
 const entities = {}
 const terrains = {}

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,15 +95,29 @@ export default class Entity {
 
       if (this.#path.length < 1 || !this.#path.at(-1).equals(fixedDest)) {
         console.time('pathfinding')
-        // console.time('waypoints')
+        console.time('waypoints')
-        const waypoints = this.waypoints().concat([this.position, fixedDest])
+        const start = SATX.vectorToFloat32Array(this.position)
-        // console.timeEnd('waypoints')
+        const goal = SATX.vectorToFloat32Array(fixedDest)
-        // console.time('graph')
+        const nonUniqueWaypoints = this.waypoints().map((w) => SATX.vectorToFloat32Array(w)).concat([start, goal])
+        const waypoints = Pathfind.uniqueWaypoints(nonUniqueWaypoints)
+        console.timeEnd('waypoints')
+        console.time('graph')
         const graph = Pathfind.buildGraph(waypoints, collidables, this.radius)
-        // console.timeEnd('graph')
+
-        // console.time('path')
+        // console.log(Pathfind.formatFloat32Array(graph, 5, true))
-        this.#path = Pathfind.shortestPath(graph, this.position, fixedDest)
+        // const tunnels = []
-        // console.timeEnd('path')
+        // for (let i = 0; i < graph.length; i += 5) {
+        //   tunnels.push(SATX.entityTunnel(graph[i], graph[i + 1], graph[i + 2], graph[i + 3], 1))
+        // }
+
+        // tunnels.map((t) => SATX.satPolygonToVectors(t)).forEach((t) => this.#game.add_terrain(new Terrain(t)))
+        // this.#dest = null
+
+        console.timeEnd('graph')
+        console.time('path')
+        this.#path = Pathfind.shortestPath(graph, start, goal).map((waypoint) => new Vector2(waypoint[0], waypoint[1]))
+        console.log(this.#path)
+        console.timeEnd('path')
         console.timeEnd('pathfinding')
       }
 
@@ -140,6 +155,10 @@ export default class Entity {
   }
 
   waypoints() {
-    return this.game?.unadjustedWaypoints.map(([waypoint, direction]) => waypoint.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []
+    const entityColliders = (this.game?.entities ?? []).filter((e) => e.id != this.id)
+    const terrainColliders = (this.game?.terrains ?? [])
+    const unadjustedWaypoints = entityColliders.concat(terrainColliders).map((e) => e.unadjustedWaypoints).flat()
+
+    return unadjustedWaypoints.map(([waypoint, direction]) => waypoint.clone().add(direction.clone().multiplyScalar(this.radius))) ?? []
   }
 }

src/index.js

@@ -46,66 +46,78 @@ app.listen(port, () => {
   entity1.radius = 50
   game.spawn_entity(entity1)
 
-  const entity2 = new Entity()
+  // const entity2 = new Entity()
-  entity2.id = '2'
+  // entity2.id = '2'
-  entity2.teleport(110, 110)
+  // entity2.teleport(110, 110)
-  entity2.radius = 50
+  // entity2.radius = 50
-  game.spawn_entity(entity2)
+  // game.spawn_entity(entity2)
   
-  const horseshoe = new Terrain([
+  const triangle = new Terrain([
     { x: 400, y: 200 },
-    { x: 600, y: 200 },
-    { x: 700, y: 300 },
-    { x: 650, y: 600 },
     { x: 400, y: 600 },
-    { x: 400, y: 450 },
-    { x: 600, y: 500 },
     { x: 600, y: 300 },
-    { x: 400, y: 300 },
   ])
-  horseshoe.id = 'horseshoe'
+  triangle.id = 'triangle'
-  game.add_terrain(horseshoe)
+  game.add_terrain(triangle)
 
-  const stopsign = new Terrain([
+  // const horseshoe = new Terrain([
-    { x: 800, y: 800 },
+  //   { x: 400, y: 200 },
-    { x: 900, y: 900 },
+  //   { x: 600, y: 200 },
-    { x: 900, y: 1000 },
+  //   { x: 700, y: 300 },
-    { x: 800, y: 1100 },
+  //   { x: 650, y: 600 },
-    { x: 800, y: 1100 },
+  //   { x: 400, y: 600 },
-    { x: 700, y: 1100 },
+  //   { x: 400, y: 450 },
-    { x: 600, y: 1000 },
+  //   { x: 600, y: 500 },
-    { x: 600, y: 900 },
+  //   { x: 600, y: 300 },
-    { x: 700, y: 800 },
+  //   { x: 400, y: 300 },
-  ])
+  // ])
-  stopsign.id = 'stopsign'
+  // horseshoe.id = 'horseshoe'
-  game.add_terrain(stopsign)
+  // game.add_terrain(horseshoe)
 
-  const box = new Terrain([
+  // const stopsign = new Terrain([
-    { x: 1200, y: 700 },
+  //   { x: 800, y: 800 },
-    { x: 1200, y: 800 },
+  //   { x: 900, y: 900 },
-    { x: 1300, y: 800 },
+  //   { x: 900, y: 1000 },
-    { x: 1300, y: 700 },
+  //   { x: 800, y: 1100 },
-  ])
+  //   { x: 800, y: 1100 },
-  box.id = 'box'
+  //   { x: 700, y: 1100 },
-  game.add_terrain(box)
+  //   { x: 600, y: 1000 },
+  //   { x: 600, y: 900 },
+  //   { x: 700, y: 800 },
+  // ])
+  // stopsign.id = 'stopsign'
+  // game.add_terrain(stopsign)
 
-  const diamond = new Terrain([
+  // const box = new Terrain([
-    { x: 1000, y: 300 },
+  //   { x: 1200, y: 700 },
-    { x: 1100, y: 400 },
+  //   { x: 1200, y: 800 },
-    { x: 1000, y: 500 },
+  //   { x: 1300, y: 800 },
-    { x: 900, y: 400 },
+  //   { x: 1300, y: 700 },
-  ])
+  // ])
-  diamond.id = 'diamond'
+  // box.id = 'box'
-  game.add_terrain(diamond)
+  // game.add_terrain(box)
 
-  const pole = new Terrain([
+  // const diamond = new Terrain([
-    { x: 400, y: 1000 },
+  //   { x: 1000, y: 300 },
-    { x: 410, y: 1000 },
+  //   { x: 1100, y: 400 },
-    { x: 410, y: 1010 },
+  //   { x: 1000, y: 500 },
-    { x: 400, y: 1010 },
+  //   { x: 900, y: 400 },
-  ])
+  // ])
-  pole.id = 'pole'
+  // diamond.id = 'diamond'
-  game.add_terrain(pole)
+  // game.add_terrain(diamond)
+
+  // const pole = new Terrain([
+  //   { x: 400, y: 1000 },
+  //   { x: 410, y: 1000 },
+  //   { x: 410, y: 1010 },
+  //   { x: 400, y: 1010 },
+  // ])
+  // pole.id = 'pole'
+  // game.add_terrain(pole)
+
+  entity1.moveAction(1000, 500)
+
+  // setTimeout(() => entity1.moveAction(100, 400), 10)
 
   game.start()
 })

src/pathfind.js

@@ -1,12 +1,42 @@
-import Entity from "./entity.js"
+import { Path, Vector2 } from 'three'
-import PriorityQueue from "./priority-queue.js"
+import Entity from './entity.js'
-import SATX from "./satx.js"
+import PriorityQueue from './priority-queue.js'
+import SATX from './satx.js'
 
 export default class Pathfind {
+  static precision = 0.001
+  static multiplier = 1000 // (1 / this.precision)
+
   static key(pos) {
     return `${pos.x},${pos.y}`
   }
 
+  static floatKey4(a, b, c, d) {
+    return Math.floor(a * Pathfind.multiplier) +
+      Math.floor(b * Pathfind.multiplier) * Pathfind.multiplier +
+      Math.floor(c * Pathfind.multiplier) * Pathfind.multiplier ** 2 +
+      Math.floor(d * Pathfind.multiplier) * Pathfind.multiplier ** 3
+  }
+
+  static floatKey2(a, b) {
+    return Math.floor(a * Pathfind.multiplier) +
+      Math.floor(b * Pathfind.multiplier) * Pathfind.multiplier
+  }
+
+  static uniqueWaypoints(waypoints) {
+    const included = new Set()
+    const uniqueWaypoints = []
+    for (const waypoint of waypoints) {
+      const key = Pathfind.floatKey2(waypoint[0], waypoint[1])
+      if (!included.has(key)) {
+        included.add(key)
+        uniqueWaypoints.push(waypoint)
+      }
+    }
+
+    return uniqueWaypoints
+  }
+
   static shortestPath(graph, start, goal) {
     const queue = new PriorityQueue((a, b) => a[1] < b[1])
     const visited = new Map()
@@ -17,19 +47,26 @@ export default class Pathfind {
       const [path, cost] = queue.pop()
       const waypoint = path.at(-1)
 
-      if (waypoint.equals(goal)) {
+      if (Math.abs(waypoint[0] - goal[0]) < Pathfind.precision && Math.abs(waypoint[1] - goal[1]) < Pathfind.precision) {
         path.shift()
         return path
       }
 
-      const waypointKey = this.key(waypoint)
+      const waypointKey = Pathfind.floatKey2(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))) {
+        for (let i = 0; i < graph.length; i += 5) {
-          const toKey = this.key(to)
+          if (Math.abs(waypoint[0] - graph[i]) < Pathfind.precision && Math.abs(waypoint[1] -  graph[i + 1]) < Pathfind.precision) {
-          if (!visited.has(toKey) || visited.get(toKey) > cost + distance) {
+            continue
-            queue.push([[...path, to], cost + distance])
+          }
+
+          const nextKey = `${graph[i + 2]},${graph[i + 3]}`
+          if (!visited.has(nextKey) || visited.get(nextKey) > cost + graph[i + 4]) {
+            const next = new Float32Array(2)
+            next[0] = graph[i + 2]
+            next[1] = graph[i + 3]
+            queue.push([[...path, next], cost + graph[i + 4]])
           }
         }
       }
@@ -38,36 +75,108 @@ export default class Pathfind {
     return []
   }
 
-  static buildGraph(waypoints = [], colliders = [], radius = 0) {
+  static buildGraph(waypoints = [], colliders = [], radius = 0, mergeNodes = true) {
-    const graph = []
+    const filteredWaypoints = []
-    const calculated = new Set()
+    const checked = new Set()
 
-    for (const from of waypoints) {
+    if (radius > 0) {
-      for (const to of waypoints) {
+      for (const waypoint of waypoints) {
-        if (from.equals(to)) {
+        const collider = Entity.collider(waypoint[0], waypoint[1], radius)
+        const waypointAvailable = !SATX.collideObjects(collider, colliders)
+        if (waypointAvailable) {
+          filteredWaypoints.push(waypoint)
+        }
+      }
+    }
+
+    const mergedWaypoints = new Float32Array(filteredWaypoints.length * 2)
+    let mergedWaypointsIndex = 0
+    for (const waypoint of filteredWaypoints) {
+      mergedWaypoints[mergedWaypointsIndex] = waypoint[0]
+      mergedWaypoints[mergedWaypointsIndex + 1] = waypoint[1]
+      mergedWaypointsIndex += 2
+    }
+
+    const nodes = []
+    for (let i = 0; i < mergedWaypoints.length; i += 2) {
+      for (let j = 0; j < mergedWaypoints.length; j += 2) {
+        if (i == j) {
           continue
         }
         
-        const key = `${from.x},${from.y};${to.x},${to.y}`
+        if (Math.abs(mergedWaypoints[i] - mergedWaypoints[j]) < Pathfind.precision && Math.abs(mergedWaypoints[i + 1] - mergedWaypoints[j + 1]) < Pathfind.precision) {
-        if (!calculated.has(key)) {
+          continue
-          calculated.add(key)
+        }
-          calculated.add(`${to.x},${to.y};${from.x},${from.y}`)
 
-          const tunnel = SATX.entityTunnel(from, to, radius)
+        const key = Pathfind.floatKey4(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1])
-          const collider = Entity.collider(from.x, from.y, radius)
+        if (!checked.has(key)) {
+          checked.add(key)
+          checked.add(Pathfind.floatKey4(mergedWaypoints[j], mergedWaypoints[j + 1], mergedWaypoints[i], mergedWaypoints[i + 1]))
 
-          const tunnelClear = !SATX.collideObjects(tunnel, colliders)
+          const tunnel = SATX.entityTunnel(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius)
-          const waypointAvailable = !SATX.collideObjects(collider, colliders)
           
-          if (waypointAvailable && tunnelClear) {
+          if (!SATX.collideObjects(tunnel, colliders)) {
-            const distance = from.distanceTo(to)
+            const node = new Float32Array(5)
-            graph.push({ from, to, distance })
+            node[0] = mergedWaypoints[i]
-            graph.push({ from: to, to: from, distance })
+            node[1] = mergedWaypoints[i + 1]
+            node[2] = mergedWaypoints[j]
+            node[3] = mergedWaypoints[j + 1]
+            node[4] = Math.hypot(mergedWaypoints[j] - mergedWaypoints[i], mergedWaypoints[j + 1] - mergedWaypoints[i + 1])
+            nodes.push(node)
+
+            const reverseNode = new Float32Array(5)
+            reverseNode[0] = mergedWaypoints[j]
+            reverseNode[1] = mergedWaypoints[j + 1]
+            reverseNode[2] = mergedWaypoints[i]
+            reverseNode[3] = mergedWaypoints[i + 1]
+            reverseNode[4] = node[4] // distance is the same, copying is less expensive
+            nodes.push(reverseNode)
           }
         }
       }
     }
 
+    if (!mergeNodes) {
+      return nodes
+    }
+
+    const graph = new Float32Array(nodes.length * 5)
+    let graphIndex = 0
+    for (const node of nodes) {
+      graph[graphIndex] = node[0]
+      graph[graphIndex + 1] = node[1]
+      graph[graphIndex + 2] = node[2]
+      graph[graphIndex + 3] = node[3]
+      graph[graphIndex + 4] = node[4]
+      graphIndex += 5
+    }
+
     return graph
   }
+
+  static formatFloat32Array(array, columns = 2, text = false) {
+    const formatted = []
+    let columnWidth = 0
+    for (let i = 0; i < array.length; i += columns) {
+      const row = []
+      for (let j = i; j < i + columns; j++) {
+        if (text) {
+          row.push(`${array[j]}`)
+          if (`${array[j]}`.length > columnWidth) {
+            columnWidth = `${array[j]}`.length
+          }
+        }
+        else {
+          row.push(array[j])
+        }
+      }
+      formatted.push(row)
+    }
+
+    if (text) {
+      return formatted.map((row) => row.map((v) => v.padEnd(columnWidth, ' ')).join(' | ')).join('\n')
+    }
+
+    return formatted
+  }
 }

src/satx.js

@@ -49,24 +49,24 @@ export default class SATX {
     return 1 / Math.cos(Math.PI / numberOfVertices)
   }
 
-  static entityTunnel(from, to, radius = 0) {
+  static entityTunnel(fromX, fromY, toX, toY, radius = 0) {
     if (radius <= 0) {
-      return this.line(from, to)
+      return this.line(fromX, fromY, toX, toY)
     }
 
-    const length = to.clone().sub(from)
+    const sides = new Float32Array(5)
-    const halfWidth = length.clone().normalize().multiplyScalar(radius).rotateAround(new Vector2(), Math.PI / 2)
+    sides[0] = toX - fromX
-    const width = halfWidth.clone().multiplyScalar(2)
+    sides[1] = toY - fromY
+    sides[4] = Math.hypot(sides[0], sides[1])
+    sides[2] = (sides[1] / sides[4]) * -radius // optimization: negation and swapping rotates
+    sides[3] = (sides[0] / sides[4]) * radius
 
-    const origin = from.clone().sub(halfWidth)
+    return new SAT.Polygon(new SAT.Vector(fromX - sides[2], fromY - sides[3]), [
-    const satPoints = [
+      new SAT.Vector(),
-      new SAT.Vector(...origin.toArray()),
+      new SAT.Vector(sides[0], sides[1]),
-      new SAT.Vector(...from.clone().sub(halfWidth).add(length).sub(origin).toArray()),
+      new SAT.Vector(sides[0] + (2 * sides[2]), sides[1] + (2 * sides[3])),
-      new SAT.Vector(...from.clone().sub(halfWidth).add(length.clone().add(width)).sub(origin).toArray()),
+      new SAT.Vector(2 * sides[2], 2 * sides[3]),
-      new SAT.Vector(...from.clone().sub(halfWidth).add(width).sub(origin).toArray()),
+    ])
-    ]
-
-    return new SAT.Polygon(satPoints[0], [new SAT.Vector(), ...satPoints.slice(1)])
   }
 
   static fixCollisions(entityPosition, colliders, radius = 0) {
@@ -83,12 +83,28 @@ export default class SATX {
     return position
   }
 
-  static line(from, to) {
+  static line(fromX, fromY, toX, toY) {
-    return new SAT.Polygon(new SAT.Vector(...from.toArray()), [new SAT.Vector(), new SAT.Vector(...to.clone().sub(from).toArray())])
+    return new SAT.Polygon(new SAT.Vector(fromX, fromY), [new SAT.Vector(), new SAT.Vector(toX - fromX, toY - fromY)])
   }
 
-  static polygonToThreeVector2(polygon) {
+  static satPolygonToVectors(polygon) {
     const position = new Vector2(polygon.pos.x, polygon.pos.y)
     return polygon.points.map((p) => new Vector2(p.x, p.y).add(position))
   }
+
+  static vectorToFloat32Array(vector) {
+    const array = new Float32Array(2)
+    array[0] = vector.x
+    array[1] = vector.y
+
+    return array
+  }
+
+  static float32ArrayToVector(array) {
+    return new Vector2(array[0], array[1])
+  }
+
+  static float32ArrayWithIndexToVector(array, index) {
+    return new Vector2(array[index], array[index + 1])
+  }
 }