forked from Mirrors/openclonk
Add generic A* implementation for path finding in graphs
parent
679eedaf50
commit
10622a9b61
|
@ -0,0 +1,194 @@
|
|||
/* Configurable pathfinder for graphs */
|
||||
|
||||
// Generic A* implementation.
|
||||
static const AStar = new Global {
|
||||
/* Overwrite these functions */
|
||||
/* ========================= */
|
||||
|
||||
// Distance heuristic for selecting nodes. A good heuristic will speed up
|
||||
// A*, a bad heuristic can lead to non-optimal paths.
|
||||
distance = func(node, goal) { FatalError("todo"); },
|
||||
|
||||
// Cost between two neighboring nodes. You'll need a different
|
||||
// implementation if your goal isn't a regular node.
|
||||
cost = func() { return this->distance(...); },
|
||||
|
||||
// Returns an array of neighboring nodes.
|
||||
successors = func(a) { FatalError("todo"); },
|
||||
|
||||
// Equality function for nodes. DeepEqual works well for for objects as
|
||||
// well as state proplists, but you may be able to supply a more efficient
|
||||
// implementation.
|
||||
node_equal = DeepEqual,
|
||||
|
||||
// Goal identification. You'll need a different implementation if your goal
|
||||
// isn't a regular node.
|
||||
goal_equal = func(node, goal) { return this->node_equal(node, goal); },
|
||||
|
||||
/* Use these functions */
|
||||
/* =================== */
|
||||
|
||||
// Find a path from start to goal. Valid types for both are defined by the
|
||||
// functions above.
|
||||
FindPath = func(start, goal)
|
||||
{
|
||||
var state = {
|
||||
open = [[this->distance(start, goal), 0, start]],
|
||||
closed = [],
|
||||
goal = goal,
|
||||
};
|
||||
var current;
|
||||
while (GetLength(state.open))
|
||||
{
|
||||
current = MinHeap->Extract(state.open);
|
||||
if (this->goal_equal(current[2], goal))
|
||||
{
|
||||
// Reconstruct the path.
|
||||
var path = [current[2]];
|
||||
while (current = current[3])
|
||||
PushFront(path, current[2]);
|
||||
return path;
|
||||
}
|
||||
PushBack(state.closed, current[2]);
|
||||
_Expand(state, current);
|
||||
}
|
||||
return nil;
|
||||
},
|
||||
|
||||
/* Internal functions */
|
||||
/* ================== */
|
||||
|
||||
_Expand = func(proplist state, array current)
|
||||
{
|
||||
/* Log("open: %v, closed: %v", state.open, state.closed); */
|
||||
/* Log("current: %v", current); */
|
||||
for (var successor in this->successors(current[2]))
|
||||
{
|
||||
/* Log(" - successor: %v", successor); */
|
||||
// Skip successor if it's in the closed list.
|
||||
var i = 0, el;
|
||||
while ((el = state.closed[i++]) && !this->node_equal(el, successor));
|
||||
if (el)
|
||||
continue;
|
||||
var cost = current[1] + this->cost(current[2], successor);
|
||||
// Find successor in the open list.
|
||||
i = 0;
|
||||
while ((el = state.open[i++]) && !this->node_equal(el[2], successor));
|
||||
if (el && el[1] <= cost)
|
||||
continue;
|
||||
successor = [cost + this->distance(successor, state.goal), cost, successor, current];
|
||||
if (el)
|
||||
{
|
||||
el[:] = successor;
|
||||
MinHeap->DecreaseKey(state.open, i-1);
|
||||
}
|
||||
else
|
||||
MinHeap->Insert(state.open, successor);
|
||||
}
|
||||
},
|
||||
|
||||
};
|
||||
|
||||
// Sample implementation for searching paths on the map, i.e. "intelligent PathFree()".
|
||||
//
|
||||
// Graph nodes are a regular grid over the landscape with a configurable size
|
||||
// (step). Nodes are represented as {x, y} proplists.
|
||||
static const AStarMap = new AStar {
|
||||
// This function is used both as heuristic (node to the goal) and as cost
|
||||
// function (two neighboring nodes).
|
||||
distance = func(proplist a, proplist b)
|
||||
{
|
||||
// Manhattan distance
|
||||
return Abs(a.x - b.x) + Abs(a.y - b.y);
|
||||
},
|
||||
|
||||
// Returns all neighboring nodes (right/down/left/up) with a free path.
|
||||
successors = func(proplist a)
|
||||
{
|
||||
var successors = [], pt;
|
||||
if (pathfree(a, (pt = {x = a.x + this.step, y = a.y}))) PushBack(successors, pt);
|
||||
if (pathfree(a, (pt = {x = a.x, y = a.y + this.step}))) PushBack(successors, pt);
|
||||
if (pathfree(a, (pt = {x = a.x - this.step, y = a.y}))) PushBack(successors, pt);
|
||||
if (pathfree(a, (pt = {x = a.x, y = a.y - this.step}))) PushBack(successors, pt);
|
||||
return successors;
|
||||
},
|
||||
|
||||
// Helper functions for successors()
|
||||
pathfree = func(proplist a, proplist b)
|
||||
{
|
||||
return PathFree(a.x, a.y, b.x, b.y);
|
||||
},
|
||||
|
||||
// Start and goal may not be a multiple of `step` apart.
|
||||
goal_equal = func(proplist node, proplist goal)
|
||||
{
|
||||
var nx = node.x / step, ny = node.y / step;
|
||||
var gx = goal.x / step, gy = goal.y / step;
|
||||
return (nx == gx || nx == gx + 1) && (ny == gy || ny == gy + 1) && pathfree(node, goal);
|
||||
},
|
||||
|
||||
step = 10
|
||||
|
||||
};
|
||||
|
||||
/* Binary Min-Heap */
|
||||
static const MinHeap = new Global {
|
||||
|
||||
// A heap is an array of [key, value] array-tuples.
|
||||
Heapify = func(array heap, int i)
|
||||
{
|
||||
var min = i, size = GetLength(heap);
|
||||
var left = 2*i+1, right = 2*i+2;
|
||||
if (left < size && heap[left][0] < heap[min][0])
|
||||
min = left;
|
||||
if (right < size && heap[right][0] < heap[min][0])
|
||||
min = right;
|
||||
if (min != i)
|
||||
{
|
||||
ArraySwap(heap, min, i);
|
||||
Heapify(heap, min);
|
||||
}
|
||||
},
|
||||
|
||||
BuildHeap = func(array a)
|
||||
{
|
||||
for (var i = GetLength(a)/2-1; i >= 0; --i)
|
||||
Heapify(a, i);
|
||||
},
|
||||
|
||||
DecreaseKey = func(array heap, int i)
|
||||
{
|
||||
var parent;
|
||||
while (i > 0 && heap[i][0] < heap[(parent = (i-1)/2)][0])
|
||||
{
|
||||
ArraySwap(heap, i, parent);
|
||||
i = parent;
|
||||
}
|
||||
},
|
||||
|
||||
Insert = func(array heap, array kv)
|
||||
{
|
||||
var i = GetLength(heap);
|
||||
heap[i] = kv;
|
||||
DecreaseKey(heap, i);
|
||||
},
|
||||
|
||||
Extract = func(array heap)
|
||||
{
|
||||
var item = heap[0], last = PopBack(heap);
|
||||
if (GetLength(heap))
|
||||
{
|
||||
heap[0] = last;
|
||||
Heapify(heap, 0);
|
||||
}
|
||||
return item;
|
||||
},
|
||||
|
||||
ArraySwap = func(array a, int i, int j)
|
||||
{
|
||||
var tmp = a[i];
|
||||
a[i] = a[j];
|
||||
a[j] = tmp;
|
||||
},
|
||||
|
||||
};
|
Loading…
Reference in New Issue