forked from Mirrors/openclonk
202 lines
5.3 KiB
C
202 lines
5.3 KiB
C
/**
|
|
AStar.c
|
|
Configurable pathfinder for graphs.
|
|
|
|
@author Luchs
|
|
*/
|
|
|
|
// 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("This function needs to be implemented by an actual algorithm. The generic function does not exist and throws this error on purpose."); },
|
|
|
|
// 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("This function needs to be implemented by an actual algorithm. The generic function does not exist and throws this error on purpose."); },
|
|
|
|
// 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;
|
|
},
|
|
|
|
};
|