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

Overhaul pathfinding algorithm

Browse Source
This commit is contained in:
Klemen Plestenjak
2023-11-24 20:25:21 +01:00
parent 6e977b7433
commit b77e939c52
8 changed files with 127 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
engine/CMakeLists.txt
View File

@@ -96,6 +96,6 @@ file(COPY ${BreezeHeaders} DESTINATION "include")
if (${BUILD_BREEZE_TESTS}) if (${BUILD_BREEZE_TESTS})
MESSAGE(STATUS "Building breeze tests is enabled") MESSAGE(STATUS "Building breeze tests is enabled")
#add_subdirectory(tests) add_subdirectory(tests)
endif() endif()

25
engine/breeze/util/heap.c
View File

@@ -15,10 +15,8 @@ typedef struct BzHeapHead {
#define HEAP_RIGHT(i) ((i32)(i * 2 + 2)) #define HEAP_RIGHT(i) ((i32)(i * 2 + 2))
#define HEAP_PARENT(i) ((i32) ((i - 1) / 2)) #define HEAP_PARENT(i) ((i32) ((i - 1) / 2))
static void heapSiftUp(BzHeapHead *head, void *heap); static void heapSiftUp(BzHeapHead *head, void *heap, i32 idx);
static void heapSiftDown(BzHeapHead *head, void *heap); static void heapSiftDown(BzHeapHead *head, void *heap, i32 idx);
void *_bzHeapCreate(i32 startCapacity, i32 stride, i32 weightOffset) { void *_bzHeapCreate(i32 startCapacity, i32 stride, i32 weightOffset) {
i32 numBytes = sizeof(BzHeapHead) + (startCapacity + 1) * stride; i32 numBytes = sizeof(BzHeapHead) + (startCapacity + 1) * stride;
@@ -60,7 +58,7 @@ i32 _bzHeapPop(void *heap) {
// Move last to index 0 // Move last to index 0
bzMemMove(heap, ((u8 *) heap) + head->size * head->stride, head->stride); bzMemMove(heap, ((u8 *) heap) + head->size * head->stride, head->stride);
heapSiftDown(head, heap); heapSiftDown(head, heap, 0);
return head->capacity; return head->capacity;
} }
@@ -72,13 +70,20 @@ void _bzHeapPush(void *heap) {
bzMemMove(((u8 *)heap) + head->size * head->stride, item, head->stride); bzMemMove(((u8 *)heap) + head->size * head->stride, item, head->stride);
head->size++; head->size++;
heapSiftUp(head, heap); heapSiftUp(head, heap, head->size - 1);
} }
i32 _bzHeapPushIdx(void *heap) { i32 _bzHeapPushIdx(void *heap) {
BzHeapHead *head = HEAP_HEAD(heap); BzHeapHead *head = HEAP_HEAD(heap);
BZ_ASSERT(head->size < head->capacity); BZ_ASSERT(head->size < head->capacity);
return head->size; 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) { static void heapSwap(BzHeapHead *head, void *heap, i32 aIdx, i32 bIdx) {
u8 *aItem = ((u8 *)heap) + aIdx * head->stride; 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); int *bWeight = (i32 *) (((u8 *)heap) + rhs * head->stride + head->weightOffset);
return (*aWeight) - (*bWeight); return (*aWeight) - (*bWeight);
} }
static void heapSiftUp(BzHeapHead *head, void *heap) { static void heapSiftUp(BzHeapHead *head, void *heap, i32 idx) {
i32 idx = head->size - 1;
while (idx >= 0) { while (idx >= 0) {
i32 parent = HEAP_PARENT(idx); i32 parent = HEAP_PARENT(idx);
if (heapCmp(head, heap, idx, parent) >= 0) 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) { static void heapSiftDown(BzHeapHead *head, void *heap, i32 idx) {
i32 idx = 0;
while (idx < head->size) { while (idx < head->size) {
i32 l = HEAP_LEFT(idx); i32 l = HEAP_LEFT(idx);
i32 r = HEAP_RIGHT(idx); i32 r = HEAP_RIGHT(idx);

2
engine/breeze/util/heap.h
View File

@@ -12,6 +12,7 @@ bool _bzHeapIsEmpty(void *heap);
i32 _bzHeapPop(void *heap); i32 _bzHeapPop(void *heap);
void _bzHeapPush(void *heap); void _bzHeapPush(void *heap);
i32 _bzHeapPushIdx(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 bzHeapCreate(T, n) (T *) ((T *)_bzHeapCreate((n), sizeof(T), offsetof(T, weight)))
#define bzHeapDestroy(heap) _bzHeapDestroy((void *) (heap)) #define bzHeapDestroy(heap) _bzHeapDestroy((void *) (heap))
@@ -25,6 +26,7 @@ i32 _bzHeapPushIdx(void *heap);
(heap)[_bzHeapPushIdx(h)] = (__VA_ARGS__); \ (heap)[_bzHeapPushIdx(h)] = (__VA_ARGS__); \
_bzHeapPush(h); \ _bzHeapPush(h); \
} while(0) } while(0)
#define bzHeapUpdate(heap, idx) _bzHeapUpdate((void *) heap, idx)
#endif //BREEZE_HEAP_H #endif //BREEZE_HEAP_H

19
engine/tests/heap_test.c
View File

@@ -29,6 +29,25 @@ int main() {
printf("%d\n", node.weight); 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); bzHeapDestroy(heap);

1
game/main.c
View File

@@ -158,6 +158,7 @@ bool init(void *userData) {
game->debugDraw.mapColliders = true; game->debugDraw.mapColliders = true;
game->debugDraw.spatialGrid = true; game->debugDraw.spatialGrid = true;
game->debugDraw.path = true;
return true; return true;
} }

112
game/pathfinding.c
View File

@@ -64,37 +64,42 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) {
size_t outIdx = 1; size_t outIdx = 1;
Position outPos = outPath->waypoints[0]; Position outPos = outPath->waypoints[0];
PathData *prevPath = pathData; #define NEXT_WAYPOINT(path, idx, len) \
size_t prevIdx = 1; 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; PathData *nextPath = pathData;
size_t nextIdx = 2; size_t nextIdx = 0;
Position lastPos = {INFINITY, INFINITY}; Position lastPos = outPos;
// Needed, because we overwrite numWaypoints // Needed, because we overwrite numWaypoints
size_t currPathLen = prevPath->numWaypoints; size_t currPathLen = currPath->numWaypoints;
size_t nextPathLen = nextPath->numWaypoints; size_t nextPathLen = nextPath->numWaypoints;
if (prevPath->next) {
currPathLen = PATH_DATA_SIZE; // Second element
nextPathLen = PATH_DATA_SIZE; NEXT_WAYPOINT(currPath, currIdx, currPathLen);
} // Third element
NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen);
NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen);
outPath->numWaypoints = 1; outPath->numWaypoints = 1;
while (nextPath && nextIdx < nextPathLen) { while (nextPath && nextIdx < nextPathLen) {
Position currPos = prevPath->waypoints[prevIdx]; Position currPos = currPath->waypoints[currIdx];
Position nextPos = nextPath->waypoints[nextIdx]; Position nextPos = nextPath->waypoints[nextIdx];
lastPos = nextPos; lastPos = nextPos;
prevIdx++; currIdx++;
nextIdx++; nextIdx++;
if (prevIdx >= currPathLen) { NEXT_WAYPOINT(currPath, currIdx, currPathLen);
prevPath = prevPath->next; NEXT_WAYPOINT(nextPath, nextIdx, nextPathLen);
prevIdx = 0;
if (prevPath) currPathLen = prevPath->numWaypoints;
}
if (nextIdx >= nextPathLen) {
nextPath = nextPath->next;
nextIdx = 0;
if (nextPath) nextPathLen = nextPath->numWaypoints;
}
if (!canRayCastLine(map, outPos, nextPos)) { if (!canRayCastLine(map, outPos, nextPos)) {
outPos = currPos; outPos = currPos;
outPath->waypoints[outIdx++] = 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); BZ_ASSERT(lastPos.x != INFINITY && lastPos.y != INFINITY);
outPath->waypoints[outIdx++] = lastPos; outPath->waypoints[outIdx++] = lastPos;
outPath->numWaypoints = outIdx; 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); BZ_ASSERT(desc->map);
BzTileMap *map = 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.x >= 0 && start.x < map->width);
BZ_ASSERT(start.y >= 0 && start.y < map->height); 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); if (!closedSet) closedSet = bzAlloc(sizeof(*closedSet) * map->width * map->height);
bzMemSet(closedSet, 0, 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, .weight=toTargetCost,
.gCost=0, .gCost=0,
.hCost=toTargetCost, .hCost=toTargetCost,
.visited=false,
.pos=start .pos=start
}); });
closedSet[start.y * map->width + start.x].open = true;
bool foundPath = false; bool foundPath = false;
@@ -159,6 +165,11 @@ bool findPath(const PathfindingDesc *desc) {
foundPath = true; foundPath = true;
break; break;
} }
TilePosition pos = node.pos;
PathNodeRecord *nodeRecord = &closedSet[pos.y * map->width + pos.x];
nodeRecord->visited = true;
nodeRecord->open = false;
// Node edges // Node edges
for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) { for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) {
@@ -171,26 +182,51 @@ bool findPath(const PathfindingDesc *desc) {
// not walkable // not walkable
if (bzTileMapHasCollision(map, x, y)) if (bzTileMapHasCollision(map, x, y))
continue; continue;
PathClosedNode *curClosed = &closedSet[y * map->width + x]; PathNodeRecord *curRecord = &closedSet[y * map->width + x];
if (curClosed->visited) if (curRecord->visited)
continue; continue;
curClosed->visited = true;
TilePosition curPos = {x, y}; TilePosition curPos = {x, y};
i32 gCost = node.gCost + dst(node.pos, curPos); i32 gCost = node.gCost + dst(node.pos, curPos);
//if (gCost >= node.gCost) continue; #if 0
toTargetCost = dst(curPos, target); 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) { if (!curRecord->open) {
.weight = gCost + toTargetCost, toTargetCost = dst(curPos, target);
.gCost = gCost,
.hCost = toTargetCost,
.pos = curPos
});
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) if (pathLen != 0)
pathData = pushPathWaypoint(pathData, waypoint, desc->pool); 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); BZ_ASSERT(visit.x != 0 || visit.y != 0);
pos.x -= visit.x; pos.x -= visit.x;
pos.y -= visit.y; pos.y -= visit.y;

10
game/pathfinding.h
View File

@@ -9,15 +9,15 @@ typedef struct PathNode {
i32 weight; // fCost = g + h i32 weight; // fCost = g + h
i32 gCost; // from start cost i32 gCost; // from start cost
i32 hCost; // to target cost i32 hCost; // to target cost
bool visited;
TilePosition pos; TilePosition pos;
} PathNode; } PathNode;
typedef struct PathClosedNode { typedef struct PathNodeRecord {
bool visited : 1; bool visited : 1;
bool open : 1;
i8 x : 3; i8 x : 3;
i8 y : 3; i8 y : 3;
} PathClosedNode; } PathNodeRecord;
typedef struct PathfindingDesc { typedef struct PathfindingDesc {
Position start; Position start;
@@ -25,11 +25,11 @@ typedef struct PathfindingDesc {
BzObjectPool *pool; BzObjectPool *pool;
BzTileMap *map; BzTileMap *map;
PathNode *openSet; // heap (size: width * height) PathNode *openSet; // heap (size: width * height)
PathClosedNode *closedSet; // size: width * height PathNodeRecord *closedSet; // size: width * height
Path *outPath; Path *outPath;
} PathfindingDesc; } PathfindingDesc;
bool findPath(const PathfindingDesc *desc); bool pathfindAStar(const PathfindingDesc *desc);
// TODO: Flowfield // TODO: Flowfield
void calculateFlowField(); void calculateFlowField();

24
game/systems_input.c
View File

@@ -117,12 +117,12 @@ void updatePlayerInput(ecs_iter_t *it) {
const Position *start = ecs_get(ECS, entity, Position); const Position *start = ecs_get(ECS, entity, Position);
Path path = {NULL, 0}; Path path = {NULL, 0};
findPath(&(PathfindingDesc) { pathfindAStar(&(PathfindingDesc) {
.start=*start, .start=*start,
.target=target, .target=target,
.map=map, .map=map,
.outPath=&path, .outPath=&path,
.pool=game->pools.pathData .pool=game->pools.pathData
}); });
if (!path.paths) continue; if (!path.paths) continue;
ecs_set_ptr(ECS, entity, Path, &path); 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); const Position *start = ecs_get(ECS, entity, Position);
Path path = {NULL, 0}; Path path = {NULL, 0};
findPath(&(PathfindingDesc) { pathfindAStar(&(PathfindingDesc) {
.start=*start, .start=*start,
.target=worldPos, .target=worldPos,
.map=map, .map=map,
.outPath=&path, .outPath=&path,
.pool=game->pools.pathData .pool=game->pools.pathData
}); });
if (!path.paths) continue; if (!path.paths) continue;
ecs_set_ptr(ECS, entity, Path, &path); ecs_set_ptr(ECS, entity, Path, &path);