114 lines
3.3 KiB
GDScript
114 lines
3.3 KiB
GDScript
class_name HexPathfinder
|
|
|
|
extends RefCounted
|
|
|
|
## A* pathfinding over a hex grid, independent of units, terrain and
|
|
## ownership. The caller describes the graph with callables, which keeps this
|
|
## class pure and testable:
|
|
##
|
|
## neighbours(coords) -> Array[Vector2i]
|
|
## can_enter(coords) -> bool
|
|
## step_cost(from, to) -> float
|
|
## heuristic(from, to) -> float
|
|
##
|
|
## find_path returns the cells from start to goal inclusive, or an empty array
|
|
## when the goal cannot be entered or is unreachable.
|
|
|
|
func find_path(
|
|
start: Vector2i,
|
|
goal: Vector2i,
|
|
neighbours: Callable,
|
|
can_enter: Callable,
|
|
step_cost: Callable,
|
|
heuristic: Callable
|
|
) -> Array[Vector2i]:
|
|
if start == goal or not can_enter.call(goal):
|
|
return []
|
|
var open := _MinHeap.new()
|
|
var came_from: Dictionary = {}
|
|
var g_score: Dictionary = {start: 0.0}
|
|
var closed: Dictionary = {}
|
|
open.push(float(heuristic.call(start, goal)), start)
|
|
while not open.is_empty():
|
|
var current := open.pop()
|
|
if closed.has(current):
|
|
continue
|
|
if current == goal:
|
|
return _reconstruct(came_from, current)
|
|
closed[current] = true
|
|
for neighbour in neighbours.call(current):
|
|
if closed.has(neighbour):
|
|
continue
|
|
if neighbour != goal and not can_enter.call(neighbour):
|
|
continue
|
|
var tentative := float(g_score.get(current, INF)) + float(step_cost.call(current, neighbour))
|
|
if tentative < float(g_score.get(neighbour, INF)):
|
|
came_from[neighbour] = current
|
|
g_score[neighbour] = tentative
|
|
open.push(tentative + float(heuristic.call(neighbour, goal)), neighbour)
|
|
return []
|
|
|
|
|
|
func _reconstruct(came_from: Dictionary, current: Vector2i) -> Array[Vector2i]:
|
|
var path: Array[Vector2i] = [current]
|
|
while came_from.has(current):
|
|
current = came_from[current]
|
|
path.push_front(current)
|
|
return path
|
|
|
|
|
|
## Binary min-heap keyed by f-score. Stale entries left behind when a node is
|
|
## improved are skipped by the closed set, so there is no decrease-key step.
|
|
class _MinHeap:
|
|
var _keys := PackedFloat64Array()
|
|
var _values: Array[Vector2i] = []
|
|
|
|
func is_empty() -> bool:
|
|
return _keys.is_empty()
|
|
|
|
func push(key: float, value: Vector2i) -> void:
|
|
_keys.append(key)
|
|
_values.append(value)
|
|
var index := _keys.size() - 1
|
|
while index > 0:
|
|
var parent := (index - 1) >> 1
|
|
if _keys[parent] <= _keys[index]:
|
|
break
|
|
# Swaps are inlined: the heap is the hottest loop in A*, and a
|
|
# GDScript call per level dominated the profile.
|
|
var key_swap := _keys[parent]
|
|
_keys[parent] = _keys[index]
|
|
_keys[index] = key_swap
|
|
var value_swap := _values[parent]
|
|
_values[parent] = _values[index]
|
|
_values[index] = value_swap
|
|
index = parent
|
|
|
|
func pop() -> Vector2i:
|
|
var value := _values[0]
|
|
var last := _keys.size() - 1
|
|
_keys[0] = _keys[last]
|
|
_values[0] = _values[last]
|
|
_keys.resize(last)
|
|
_values.resize(last)
|
|
var index := 0
|
|
var count := _keys.size()
|
|
while true:
|
|
var smallest := index
|
|
var left := (index << 1) + 1
|
|
var right := left + 1
|
|
if left < count and _keys[left] < _keys[smallest]:
|
|
smallest = left
|
|
if right < count and _keys[right] < _keys[smallest]:
|
|
smallest = right
|
|
if smallest == index:
|
|
break
|
|
var key_swap := _keys[index]
|
|
_keys[index] = _keys[smallest]
|
|
_keys[smallest] = key_swap
|
|
var value_swap := _values[index]
|
|
_values[index] = _values[smallest]
|
|
_values[smallest] = value_swap
|
|
index = smallest
|
|
return value
|