instructions-clear/src/pathfind.js

import Entity from './entity.js'
import PriorityQueue from './priority-queue.js'
import SATX from './satx.js'

export default class Pathfind {
  static precision = 0.01
  static multiplier = 1000000 // (1 / this.precision) * 10^expected_digit_count / 10

  static key2(a, b) {
    return `${a},${b}`
  }

  // Fowler-Noll-Vo hash prime and offset basis for small keyspaces
  static floatKey4(a, b, c, d) {
    const prime = 16777619
    let result = 2166136261
    result ^= Math.floor(a * Pathfind.multiplier)
    result *= prime
    result ^= Math.floor(b * Pathfind.multiplier)
    result *= prime
    result ^= Math.floor(c * Pathfind.multiplier)
    result *= prime
    result ^= Math.floor(d * Pathfind.multiplier)
    result *= prime
    return result
  }

  static uniqueWaypoints(waypoints) {
    const included = new Set()
    const uniqueWaypoints = []
    for (const waypoint of waypoints) {
      const key = Pathfind.key2(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()

    queue.push([[start], 0])

    while (!queue.isEmpty()) {
      const [path, cost] = queue.pop()
      const waypoint = path.at(-1)

      if (Math.abs(waypoint[0] - goal[0]) < Pathfind.precision && Math.abs(waypoint[1] - goal[1]) < Pathfind.precision) {
        path.shift()
        return path
      }

      const waypointKey = Pathfind.key2(waypoint[0], waypoint[1])
      if (!visited.has(waypointKey) || visited.get(waypointKey) > cost) {
        visited.set(waypointKey, cost)

        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 // waypoint and graph.from aren't the same (so graph.to isn't a neighbor)
          }

          const nextKey = Pathfind.key2(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]])
          }
        }
      }
    }

    return []
  }

  static buildGraph(waypoints, bboxes, obstacles, radius) {
    const filteredWaypoints = []
    const checked = new Set()

    if (radius > 0) {
      for (const waypoint of waypoints) {
        const bbox = Entity.bbox(waypoint[0], waypoint[1], radius)
        const bboxCheckedObstacles = []
        for (let i = 0; i < bboxes.length; i += 5) {
          if (bbox[0] <= bboxes[i + 2]) { continue }
          if (bbox[1] <= bboxes[i + 3]) { continue }
          if (bbox[2] >= bboxes[i]) { continue }
          if (bbox[3] >= bboxes[i + 1]) { continue }

          bboxCheckedObstacles.push(obstacles[bboxes[i + 4]])
        }

        if (bboxCheckedObstacles.length > 0) {
          const collider = Entity.collider(waypoint[0], waypoint[1], radius)
          const colliding = bboxCheckedObstacles.flat().some((it) => SATX.collideObject(collider, it))
          if (colliding) {
            continue
          }
        }

        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 = Pathfind.floatKey4(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1])
        if (checked.has(key)) {
          continue
        }

        checked.add(key)
        checked.add(Pathfind.floatKey4(mergedWaypoints[j], mergedWaypoints[j + 1], mergedWaypoints[i], mergedWaypoints[i + 1]))

        const bbox = Entity.tunnelBbox(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius)

        const bboxCheckedObstacles = []
        for (let i = 0; i < bboxes.length; i += 5) {
          if (bbox[0] <= bboxes[i + 2]) { continue }
          if (bbox[1] <= bboxes[i + 3]) { continue }
          if (bbox[2] >= bboxes[i]) { continue }
          if (bbox[3] >= bboxes[i + 1]) { continue }

          bboxCheckedObstacles.push(obstacles[bboxes[i + 4]])
        }

        if (bboxCheckedObstacles.length > 0) {
          const tunnel = Entity.tunnelCollider(mergedWaypoints[i], mergedWaypoints[i + 1], mergedWaypoints[j], mergedWaypoints[j + 1], radius)
          const colliding = bboxCheckedObstacles.some((it) => it.some((c) => SATX.collideObject(tunnel, c)))
          if (colliding) {
            continue
          }
        }

        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 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)
      }
    }

    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
    }

    // const niceGraph = []
    // for (let i = 0; i < graph.length / 5; i += 5) {
    //   niceGraph.push({
    //     from: [graph[i], graph[i + 1]],
    //     to: [graph[i + 2], graph[i + 3]],
    //     distance: graph[i + 4],
    //   })
    // }
    // console.log(niceGraph)
    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
  }
}