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})
MESSAGE(STATUS "Building breeze tests is enabled")
#add_subdirectory(tests)
add_subdirectory(tests)
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_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);

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

@@ -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

19
engine/tests/heap_test.c
View File

@@ -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);

1
game/main.c
View File

@@ -158,6 +158,7 @@ bool init(void *userData) {
game->debugDraw.mapColliders = true;
game->debugDraw.spatialGrid = true;
game->debugDraw.path = 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;
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;

10
game/pathfinding.h
View File

@@ -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();

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);
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);