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

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;