fix pathfinding nekimegyafalnak style

2024-12-25 17:19:45 +09:00
parent 0f8a73911f
commit 8fe48fb679
5 changed files with 266 additions and 108 deletions
@@ -1,12 +1,42 @@
-import Entity from "./entity.js"
-import PriorityQueue from "./priority-queue.js"
-import SATX from "./satx.js"
+import { Path, Vector2 } from 'three'
+import Entity from './entity.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))) {
-          const toKey = this.key(to)
-          if (!visited.has(toKey) || visited.get(toKey) > cost + distance) {
-            queue.push([[...path, to], cost + distance])
+        for (let i = 0; i < graph.length; i += 5) {
+          if (Math.abs(waypoint[0] - graph[i]) < Pathfind.precision && Math.abs(waypoint[1] -  graph[i + 1]) < Pathfind.precision) {
+            continue
+          }
+
+          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) {
-    const graph = []
-    const calculated = new Set()
+  static buildGraph(waypoints = [], colliders = [], radius = 0, mergeNodes = true) {
+    const filteredWaypoints = []
+    const checked = new Set()

-    for (const from of waypoints) {
-      for (const to of waypoints) {
-        if (from.equals(to)) {
+    if (radius > 0) {
+      for (const waypoint of waypoints) {
+        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
+        }
+        
+        if (Math.abs(mergedWaypoints[i] - mergedWaypoints[j]) < Pathfind.precision && Math.abs(mergedWaypoints[i + 1] - mergedWaypoints[j + 1]) < Pathfind.precision) {
          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 key = Pathfind.floatKey4(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1])
+        if (!checked.has(key)) {
+          checked.add(key)
+          checked.add(Pathfind.floatKey4(mergedWaypoints[j], mergedWaypoints[j + 1], mergedWaypoints[i], mergedWaypoints[i + 1]))

-          const tunnel = SATX.entityTunnel(from, to, radius)
-          const collider = Entity.collider(from.x, from.y, radius)
+          const tunnel = SATX.entityTunnel(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius)
+          
+          if (!SATX.collideObjects(tunnel, colliders)) {
+            const node = new Float32Array(5)
+            node[0] = mergedWaypoints[i]
+            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 tunnelClear = !SATX.collideObjects(tunnel, colliders)
-          const waypointAvailable = !SATX.collideObjects(collider, colliders)
-
-          if (waypointAvailable && tunnelClear) {
-            const distance = from.distanceTo(to)
-            graph.push({ from, to, distance })
-            graph.push({ from: to, to: from, distance })
+            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
+  }
 }