From b77e939c52a9090869672465cf1b58e64869411e Mon Sep 17 00:00:00 2001 From: Klemen Plestenjak Date: Fri, 24 Nov 2023 20:25:21 +0100 Subject: [PATCH] Overhaul pathfinding algorithm --- engine/CMakeLists.txt | 2 +- engine/breeze/util/heap.c | 25 +++++---- engine/breeze/util/heap.h | 2 + engine/tests/heap_test.c | 19 +++++++ game/main.c | 1 + game/pathfinding.c | 112 +++++++++++++++++++++++++------------- game/pathfinding.h | 10 ++-- game/systems_input.c | 24 ++++---- 8 files changed, 127 insertions(+), 68 deletions(-) diff --git a/engine/CMakeLists.txt b/engine/CMakeLists.txt index 909d8c8..9ce4561 100644 --- a/engine/CMakeLists.txt +++ b/engine/CMakeLists.txt @@ -96,6 +96,6 @@ file(COPY ${BreezeHeaders} DESTINATION "include") if (${BUILD_BREEZE_TESTS}) MESSAGE(STATUS "Building breeze tests is enabled") - #add_subdirectory(tests) + add_subdirectory(tests) endif() diff --git a/engine/breeze/util/heap.c b/engine/breeze/util/heap.c index 9ce11a1..19029e8 100644 --- a/engine/breeze/util/heap.c +++ b/engine/breeze/util/heap.c @@ -15,10 +15,8 @@ typedef struct BzHeapHead { #define HEAP_RIGHT(i) ((i32)(i * 2 + 2)) #define HEAP_PARENT(i) ((i32) ((i - 1) / 2)) -static void heapSiftUp(BzHeapHead *head, void *heap); -static void heapSiftDown(BzHeapHead *head, void *heap); - - +static void heapSiftUp(BzHeapHead *head, void *heap, i32 idx); +static void heapSiftDown(BzHeapHead *head, void *heap, i32 idx); void *_bzHeapCreate(i32 startCapacity, i32 stride, i32 weightOffset) { i32 numBytes = sizeof(BzHeapHead) + (startCapacity + 1) * stride; @@ -60,7 +58,7 @@ i32 _bzHeapPop(void *heap) { // Move last to index 0 bzMemMove(heap, ((u8 *) heap) + head->size * head->stride, head->stride); - heapSiftDown(head, heap); + heapSiftDown(head, heap, 0); return head->capacity; } @@ -72,13 +70,20 @@ void _bzHeapPush(void *heap) { bzMemMove(((u8 *)heap) + head->size * head->stride, item, head->stride); head->size++; - heapSiftUp(head, heap); + heapSiftUp(head, heap, head->size - 1); } i32 _bzHeapPushIdx(void *heap) { BzHeapHead *head = HEAP_HEAD(heap); BZ_ASSERT(head->size < head->capacity); return head->size; } +void _bzHeapUpdate(void *heap, i32 idx) { + BzHeapHead *head = HEAP_HEAD(heap); + BZ_ASSERT(idx >= 0 && idx < head->size); + + heapSiftUp(head, heap, idx); + heapSiftDown(head, heap, idx); +} static void heapSwap(BzHeapHead *head, void *heap, i32 aIdx, i32 bIdx) { u8 *aItem = ((u8 *)heap) + aIdx * head->stride; @@ -95,9 +100,7 @@ static int heapCmp(BzHeapHead *head, void *heap, i32 lhs, i32 rhs) { int *bWeight = (i32 *) (((u8 *)heap) + rhs * head->stride + head->weightOffset); return (*aWeight) - (*bWeight); } -static void heapSiftUp(BzHeapHead *head, void *heap) { - i32 idx = head->size - 1; - +static void heapSiftUp(BzHeapHead *head, void *heap, i32 idx) { while (idx >= 0) { i32 parent = HEAP_PARENT(idx); if (heapCmp(head, heap, idx, parent) >= 0) @@ -107,9 +110,7 @@ static void heapSiftUp(BzHeapHead *head, void *heap) { } } -static void heapSiftDown(BzHeapHead *head, void *heap) { - i32 idx = 0; - +static void heapSiftDown(BzHeapHead *head, void *heap, i32 idx) { while (idx < head->size) { i32 l = HEAP_LEFT(idx); i32 r = HEAP_RIGHT(idx); diff --git a/engine/breeze/util/heap.h b/engine/breeze/util/heap.h index cead255..ba9f440 100644 --- a/engine/breeze/util/heap.h +++ b/engine/breeze/util/heap.h @@ -12,6 +12,7 @@ bool _bzHeapIsEmpty(void *heap); i32 _bzHeapPop(void *heap); void _bzHeapPush(void *heap); i32 _bzHeapPushIdx(void *heap); +void _bzHeapUpdate(void *heap, i32 idx); #define bzHeapCreate(T, n) (T *) ((T *)_bzHeapCreate((n), sizeof(T), offsetof(T, weight))) #define bzHeapDestroy(heap) _bzHeapDestroy((void *) (heap)) @@ -25,6 +26,7 @@ i32 _bzHeapPushIdx(void *heap); (heap)[_bzHeapPushIdx(h)] = (__VA_ARGS__); \ _bzHeapPush(h); \ } while(0) +#define bzHeapUpdate(heap, idx) _bzHeapUpdate((void *) heap, idx) #endif //BREEZE_HEAP_H diff --git a/engine/tests/heap_test.c b/engine/tests/heap_test.c index a325659..69a334d 100644 --- a/engine/tests/heap_test.c +++ b/engine/tests/heap_test.c @@ -29,6 +29,25 @@ int main() { printf("%d\n", node.weight); } + printf("\n\n"); + bzHeapDestroy(heap); + + heap = bzHeapCreate(Node, 10); + bzHeapPush(heap, (Node) {3}); + bzHeapPush(heap, (Node) {8}); + bzHeapPush(heap, (Node) {10}); + bzHeapPush(heap, (Node) {5}); + bzHeapPush(heap, (Node) {12}); + bzHeapPush(heap, (Node) {7}); + + heap[3].weight = 20; + bzHeapUpdate(heap, 3); + + while (!bzHeapIsEmpty(heap)) { + Node node = bzHeapPop(heap); + printf("%d ", node.weight); + } + bzHeapDestroy(heap); diff --git a/game/main.c b/game/main.c index 810b43a..47363f1 100644 --- a/game/main.c +++ b/game/main.c @@ -158,6 +158,7 @@ bool init(void *userData) { game->debugDraw.mapColliders = true; game->debugDraw.spatialGrid = true; + game->debugDraw.path = true; return true; } diff --git a/game/pathfinding.c b/game/pathfinding.c index 9275cee..90272e9 100644 --- a/game/pathfinding.c +++ b/game/pathfinding.c @@ -64,37 +64,42 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) { size_t outIdx = 1; Position outPos = outPath->waypoints[0]; - PathData *prevPath = pathData; - size_t prevIdx = 1; +#define NEXT_WAYPOINT(path, idx, len) \ +do { \ + if (idx >= len) { \ + path = path->next; \ + idx = 0; \ + if (path) len = path->numWaypoints; \ +} \ +} while (0) + + PathData *currPath = pathData; + size_t currIdx = 0; PathData *nextPath = pathData; - size_t nextIdx = 2; - Position lastPos = {INFINITY, INFINITY}; + size_t nextIdx = 0; + Position lastPos = outPos; // Needed, because we overwrite numWaypoints - size_t currPathLen = prevPath->numWaypoints; + size_t currPathLen = currPath->numWaypoints; size_t nextPathLen = nextPath->numWaypoints; - if (prevPath->next) { - currPathLen = PATH_DATA_SIZE; - nextPathLen = PATH_DATA_SIZE; - } + + // Second element + NEXT_WAYPOINT(currPath, currIdx, currPathLen); + // Third element + NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen); + NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen); + outPath->numWaypoints = 1; while (nextPath && nextIdx < nextPathLen) { - Position currPos = prevPath->waypoints[prevIdx]; + Position currPos = currPath->waypoints[currIdx]; Position nextPos = nextPath->waypoints[nextIdx]; lastPos = nextPos; - prevIdx++; + currIdx++; nextIdx++; - if (prevIdx >= currPathLen) { - prevPath = prevPath->next; - prevIdx = 0; - if (prevPath) currPathLen = prevPath->numWaypoints; - } - if (nextIdx >= nextPathLen) { - nextPath = nextPath->next; - nextIdx = 0; - if (nextPath) nextPathLen = nextPath->numWaypoints; - } + NEXT_WAYPOINT(currPath, currIdx, currPathLen); + NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen); + if (!canRayCastLine(map, outPos, nextPos)) { outPos = currPos; outPath->waypoints[outIdx++] = currPos; @@ -107,6 +112,7 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) { } } +#undef NEXT_WAYPOINT BZ_ASSERT(lastPos.x != INFINITY && lastPos.y != INFINITY); outPath->waypoints[outIdx++] = lastPos; outPath->numWaypoints = outIdx; @@ -121,7 +127,7 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) { } -bool findPath(const PathfindingDesc *desc) { +bool pathfindAStar(const PathfindingDesc *desc) { BZ_ASSERT(desc->map); BzTileMap *map = desc->map; @@ -131,7 +137,7 @@ bool findPath(const PathfindingDesc *desc) { BZ_ASSERT(start.x >= 0 && start.x < map->width); BZ_ASSERT(start.y >= 0 && start.y < map->height); - PathClosedNode *closedSet = desc->closedSet; + PathNodeRecord *closedSet = desc->closedSet; if (!closedSet) closedSet = bzAlloc(sizeof(*closedSet) * map->width * map->height); bzMemSet(closedSet, 0, sizeof(*closedSet) * map->width * map->height); @@ -145,9 +151,9 @@ bool findPath(const PathfindingDesc *desc) { .weight=toTargetCost, .gCost=0, .hCost=toTargetCost, - .visited=false, .pos=start }); + closedSet[start.y * map->width + start.x].open = true; bool foundPath = false; @@ -159,6 +165,11 @@ bool findPath(const PathfindingDesc *desc) { foundPath = true; break; } + TilePosition pos = node.pos; + PathNodeRecord *nodeRecord = &closedSet[pos.y * map->width + pos.x]; + nodeRecord->visited = true; + nodeRecord->open = false; + // Node edges for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) { @@ -171,26 +182,51 @@ bool findPath(const PathfindingDesc *desc) { // not walkable if (bzTileMapHasCollision(map, x, y)) continue; - PathClosedNode *curClosed = &closedSet[y * map->width + x]; - if (curClosed->visited) + PathNodeRecord *curRecord = &closedSet[y * map->width + x]; + if (curRecord->visited) continue; - curClosed->visited = true; TilePosition curPos = {x, y}; i32 gCost = node.gCost + dst(node.pos, curPos); - //if (gCost >= node.gCost) continue; - toTargetCost = dst(curPos, target); +#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; + } - curClosed->x = (i8) (curPos.x - node.pos.x); - curClosed->y = (i8) (curPos.y - node.pos.y); + } + BZ_ASSERT(curNode); + 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); + BZ_ASSERT(curPos.x == curNode->pos.x && curPos.y == curNode->pos.y); + bzHeapUpdate(openSet, curNodeIdx); + } + } +#endif - bzHeapPush(openSet, (PathNode) { - .weight = gCost + toTargetCost, - .gCost = gCost, - .hCost = toTargetCost, - .pos = curPos - }); + if (!curRecord->open) { + toTargetCost = dst(curPos, target); + curRecord->x = (i8) (curPos.x - node.pos.x); + curRecord->y = (i8) (curPos.y - node.pos.y); + curRecord->open = true; + + bzHeapPush(openSet, (PathNode) { + .weight = gCost + toTargetCost, + .gCost = gCost, + .hCost = toTargetCost, + .pos = curPos + }); + } } } } @@ -217,7 +253,7 @@ bool findPath(const PathfindingDesc *desc) { if (pathLen != 0) pathData = pushPathWaypoint(pathData, waypoint, desc->pool); - PathClosedNode visit = closedSet[pos.y * map->width + pos.x]; + 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; diff --git a/game/pathfinding.h b/game/pathfinding.h index f177398..e2aea0c 100644 --- a/game/pathfinding.h +++ b/game/pathfinding.h @@ -9,15 +9,15 @@ typedef struct PathNode { i32 weight; // fCost = g + h i32 gCost; // from start cost i32 hCost; // to target cost - bool visited; TilePosition pos; } PathNode; -typedef struct PathClosedNode { +typedef struct PathNodeRecord { bool visited : 1; + bool open : 1; i8 x : 3; i8 y : 3; -} PathClosedNode; +} PathNodeRecord; typedef struct PathfindingDesc { Position start; @@ -25,11 +25,11 @@ typedef struct PathfindingDesc { BzObjectPool *pool; BzTileMap *map; PathNode *openSet; // heap (size: width * height) - PathClosedNode *closedSet; // size: width * height + PathNodeRecord *closedSet; // size: width * height Path *outPath; } PathfindingDesc; -bool findPath(const PathfindingDesc *desc); +bool pathfindAStar(const PathfindingDesc *desc); // TODO: Flowfield void calculateFlowField(); diff --git a/game/systems_input.c b/game/systems_input.c index f7e12cd..9e0fea7 100644 --- a/game/systems_input.c +++ b/game/systems_input.c @@ -117,12 +117,12 @@ void updatePlayerInput(ecs_iter_t *it) { const Position *start = ecs_get(ECS, entity, Position); Path path = {NULL, 0}; - findPath(&(PathfindingDesc) { - .start=*start, - .target=target, - .map=map, - .outPath=&path, - .pool=game->pools.pathData + pathfindAStar(&(PathfindingDesc) { + .start=*start, + .target=target, + .map=map, + .outPath=&path, + .pool=game->pools.pathData }); if (!path.paths) continue; ecs_set_ptr(ECS, entity, Path, &path); @@ -134,12 +134,12 @@ void updatePlayerInput(ecs_iter_t *it) { const Position *start = ecs_get(ECS, entity, Position); Path path = {NULL, 0}; - findPath(&(PathfindingDesc) { - .start=*start, - .target=worldPos, - .map=map, - .outPath=&path, - .pool=game->pools.pathData + pathfindAStar(&(PathfindingDesc) { + .start=*start, + .target=worldPos, + .map=map, + .outPath=&path, + .pool=game->pools.pathData }); if (!path.paths) continue; ecs_set_ptr(ECS, entity, Path, &path);