diff --git a/game/pathfinding.c b/game/pathfinding.c index 90272e9..d2e2f07 100644 --- a/game/pathfinding.c +++ b/game/pathfinding.c @@ -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; -} \ No newline at end of file +} + +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); +} diff --git a/game/pathfinding.h b/game/pathfinding.h index e2aea0c..9537b72 100644 --- a/game/pathfinding.h +++ b/game/pathfinding.h @@ -15,8 +15,9 @@ typedef struct PathNode { typedef struct PathNodeRecord { bool visited : 1; bool open : 1; - i8 x : 3; - i8 y : 3; + i8 toParentX : 3; + i8 toParentY : 3; + i32 nodeIdx; } PathNodeRecord; typedef struct PathfindingDesc {