KlemenPl/PixelDefense
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Ensure pathfinding always finds the shortest path

Browse Source
This commit is contained in:
Klemen Plestenjak
2023-11-25 08:27:20 +01:00
parent b77e939c52
commit 2c3ee8afd6
2 changed files with 131 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files

167
game/pathfinding.c
View File

@@ -127,6 +127,18 @@ do { \
}
typedef struct Heap {
PathNode *arr;
i32 size;
i32 capacity;
i32 mapWidth; // For calculating record idx
PathNodeRecord *records;
} Heap;
static void heapPush(Heap *heap, PathNode node);
static PathNode heapPop(Heap *heap);
static void heapUpdate(Heap *heap, i32 idx);
bool pathfindAStar(const PathfindingDesc *desc) {
BZ_ASSERT(desc->map);
BzTileMap *map = desc->map;
@@ -137,28 +149,34 @@ bool pathfindAStar(const PathfindingDesc *desc) {
BZ_ASSERT(start.x >= 0 && start.x < map->width);
BZ_ASSERT(start.y >= 0 && start.y < map->height);
i32 numTiles = map->width * map->height;
PathNodeRecord *closedSet = desc->closedSet;
if (!closedSet) closedSet = bzAlloc(sizeof(*closedSet) * map->width * map->height);
bzMemSet(closedSet, 0, sizeof(*closedSet) * map->width * map->height);
PathNode *openSet = desc->openSet;
if (!openSet) openSet = bzHeapCreate(PathNode, map->width * map->height);
else bzHeapClear(openSet);
if (!closedSet) closedSet = bzAlloc(sizeof(*closedSet) * numTiles);
bzMemSet(closedSet, 0, sizeof(*closedSet) * numTiles);
Heap openSet = {
.arr=desc->openSet,
.size=0,
.capacity=numTiles,
.mapWidth=map->width,
.records=closedSet
};
if (!openSet.arr) openSet.arr = bzAlloc(sizeof(*openSet.arr) * numTiles);
i32 toTargetCost = dst(start, target);
bzHeapPush(openSet, (PathNode) {
.weight=toTargetCost,
.gCost=0,
.hCost=toTargetCost,
.pos=start
heapPush(&openSet, (PathNode) {
.weight=toTargetCost,
.gCost=0,
.hCost=toTargetCost,
.pos=start
});
closedSet[start.y * map->width + start.x].open = true;
bool foundPath = false;
while (!bzHeapIsEmpty(openSet)) {
PathNode node = bzHeapPop(openSet);
while (openSet.size) {
PathNode node = heapPop(&openSet);
if (node.pos.x == target.x &&
node.pos.y == target.y) {
// Found path
@@ -188,39 +206,27 @@ bool pathfindAStar(const PathfindingDesc *desc) {
TilePosition curPos = {x, y};
i32 gCost = node.gCost + dst(node.pos, curPos);
#if 0
if (curRecord->open) {
PathNode *curNode = NULL;
i32 curNodeIdx = -1;
for (i32 i = 0; i < bzHeapSize(openSet); i++) {
PathNode n = openSet[i];
if (curPos.x == n.pos.x && curPos.y == n.pos.y) {
curNode = &openSet[i];
curNodeIdx = i;
break;
}
}
BZ_ASSERT(curNode);
i32 nodeIdx = curRecord->nodeIdx;
PathNode *curNode = &openSet.arr[nodeIdx];
if (gCost < curNode->gCost) {
curNode->gCost = gCost;
curNode->weight = curNode->gCost + curNode->hCost;
curRecord->x = (i8) (curPos.x - node.pos.x);
curRecord->y = (i8) (curPos.y - node.pos.y);
curRecord->toParentX = (i8) (curPos.x - node.pos.x);
curRecord->toParentY = (i8) (curPos.y - node.pos.y);
BZ_ASSERT(curPos.x == curNode->pos.x && curPos.y == curNode->pos.y);
bzHeapUpdate(openSet, curNodeIdx);
heapUpdate(&openSet, nodeIdx);
}
}
#endif
if (!curRecord->open) {
toTargetCost = dst(curPos, target);
curRecord->x = (i8) (curPos.x - node.pos.x);
curRecord->y = (i8) (curPos.y - node.pos.y);
curRecord->toParentX = (i8) (curPos.x - node.pos.x);
curRecord->toParentY = (i8) (curPos.y - node.pos.y);
curRecord->open = true;
bzHeapPush(openSet, (PathNode) {
heapPush(&openSet, (PathNode) {
.weight = gCost + toTargetCost,
.gCost = gCost,
.hCost = toTargetCost,
@@ -254,12 +260,11 @@ bool pathfindAStar(const PathfindingDesc *desc) {
pathData = pushPathWaypoint(pathData, waypoint, desc->pool);
PathNodeRecord visit = closedSet[pos.y * map->width + pos.x];
BZ_ASSERT(visit.x != 0 || visit.y != 0);
pos.x -= visit.x;
pos.y -= visit.y;
BZ_ASSERT(visit.toParentX != 0 || visit.toParentY != 0);
pos.x -= visit.toParentX;
pos.y -= visit.toParentY;
pathLen++;
}
//pushPathWaypoint(pathData, desc->start, desc->pool);
if (pathLen == 0) {
bzObjectPoolRelease(desc->pool, pathData);
pathData = NULL;
@@ -274,7 +279,91 @@ bool pathfindAStar(const PathfindingDesc *desc) {
if (!desc->closedSet)
bzFree(closedSet);
if (!desc->openSet)
bzHeapDestroy(openSet);
bzFree(openSet.arr);
return foundPath ? pathLen : -1;
}
}
static void heapSwap(Heap *heap, i32 left, i32 right) {
PathNode tmp = heap->arr[left];
heap->arr[left] = heap->arr[right];
heap->arr[right] = tmp;
TilePosition leftPos = heap->arr[left].pos;
TilePosition rightPos = heap->arr[right].pos;
i32 leftIdx = leftPos.y * heap->mapWidth + leftPos.x;
i32 rightIdx = rightPos.y * heap->mapWidth + rightPos.x;
PathNodeRecord *leftNode = &heap->records[leftIdx];
PathNodeRecord *rightNode = &heap->records[rightIdx];
i32 tmpIdx = leftNode->nodeIdx;
leftNode->nodeIdx = rightNode->nodeIdx;
rightNode->nodeIdx = tmpIdx;
}
static i32 heapCmp(Heap *heap, i32 leftIdx, i32 rightIdx) {
PathNode left = heap->arr[leftIdx];
PathNode right = heap->arr[rightIdx];
return left.weight - right.weight;
}
#define HEAP_LEFT(i) ((i32)(i * 2 + 1))
#define HEAP_RIGHT(i) ((i32)(i * 2 + 2))
#define HEAP_PARENT(i) ((i32) ((i - 1) / 2))
static void heapSiftUp(Heap *heap, i32 idx) {
while (idx >= 0) {
i32 parent = HEAP_PARENT(idx);
if (heapCmp(heap, idx, parent) >= 0)
break;
heapSwap(heap, idx, parent);
idx = parent;
}
}
static void heapSiftDown(Heap *heap, i32 idx) {
while (idx < heap->size) {
i32 l = HEAP_LEFT(idx);
i32 r = HEAP_RIGHT(idx);
i32 smallest = idx;
if (l < heap->size && heapCmp(heap, l, idx) < 0)
smallest = l;
if (r < heap->size && heapCmp(heap, r, smallest) < 0)
smallest = r;
if (smallest == idx) break;
heapSwap(heap, idx, smallest);
idx = smallest;
}
}
static void heapPush(Heap *heap, PathNode node) {
BZ_ASSERT(heap->size < heap->capacity);
heap->arr[heap->size] = node;
heap->size++;
TilePosition pos = node.pos;
PathNodeRecord *record = &heap->records[pos.y * heap->mapWidth + pos.x];
record->nodeIdx = heap->size - 1;
heapSiftUp(heap, heap->size - 1);
}
static PathNode heapPop(Heap *heap) {
BZ_ASSERT(heap->size > 0);
PathNode popped = heap->arr[0];
heap->size--;
heapSwap(heap, 0, heap->size);
heapSiftDown(heap, 0);
return popped;
}
static void heapUpdate(Heap *heap, i32 idx) {
heapSiftUp(heap, idx);
heapSiftDown(heap, idx);
}