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

Add path smoothing

Browse Source
This commit is contained in:
Klemen Plestenjak
2023-11-23 19:01:28 +01:00
parent d62e08a6b8
commit 3885e911a3
4 changed files with 147 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
game/pathfinding.c
View File

@@ -1,5 +1,7 @@
#include "pathfinding.h"
#include <raymath.h>
static i32 dst(TilePosition a, TilePosition b) {
//i32 dX = a.x - b.x;
//i32 dY = a.y - b.y;
@@ -17,6 +19,7 @@ static i32 dst(TilePosition a, TilePosition b) {
static PathData *pushPathWaypoint(PathData *pathData, Position waypoint, BzObjectPool *pathPool) {
if (pathData->numWaypoints + 1 > PATH_DATA_SIZE) {
PathData *newPathData = bzObjectPool(pathPool);
bzMemSet(newPathData, 0, sizeof(*newPathData));
BZ_ASSERT(newPathData);
newPathData->numWaypoints = 0;
newPathData->next = pathData;
@@ -26,6 +29,93 @@ static PathData *pushPathWaypoint(PathData *pathData, Position waypoint, BzObjec
return pathData;
}
static bool canRayCastLine(BzTileMap *map, Position from, Position to) {
Vector2 step = Vector2Subtract(to, from);
step = Vector2Normalize(step);
while (Vector2DistanceSqr(from, to) > 1.0f) {
from = Vector2Add(from, step);
BzTile tileX = 0, tileY = 0;
bzTileMapPosToTile(map, from, &tileX, &tileY);
if (bzTileMapHasCollision(map, tileX, tileY))
return false;
}
return true;
}
static void reversePath(PathData *pathData) {
while (pathData) {
for (i32 i = 0; i < pathData->numWaypoints / 2; i++) {
i32 left = i;
i32 right = (i32) (pathData->numWaypoints - 1 - i);
Position tmp = pathData->waypoints[left];
pathData->waypoints[left] = pathData->waypoints[right];
pathData->waypoints[right] = tmp;
}
pathData = pathData->next;
}
}
static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) {
// Our smoothed path
PathData *outPath = pathData;
size_t outIdx = 1;
Position outPos = outPath->waypoints[0];
PathData *prevPath = pathData;
size_t prevIdx = 1;
PathData *nextPath = pathData;
size_t nextIdx = 2;
Position lastPos;
// Needed, because we overwrite numWaypoints
size_t currPathLen = prevPath->numWaypoints;
size_t nextPathLen = nextPath->numWaypoints;
outPath->numWaypoints = 1;
while (nextPath && nextIdx < nextPathLen) {
Position currPos = prevPath->waypoints[prevIdx];
Position nextPos = nextPath->waypoints[nextIdx];
lastPos = nextPos;
prevIdx++;
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;
}
if (!canRayCastLine(map, outPos, nextPos)) {
outPos = currPos;
outPath->waypoints[outIdx++] = currPos;
outPath->numWaypoints = outIdx;
if (outIdx >= PATH_DATA_SIZE) {
outPath = outPath->next;
outIdx = 0;
outPath->numWaypoints = 0;
}
}
}
outPath->waypoints[outIdx++] = lastPos;
outPath->numWaypoints = outIdx;
// Free old path
pathData = outPath->next;
while (pathData) {
bzObjectPoolRelease(pool, pathData);
pathData = pathData->next;
}
outPath->next = NULL;
}
bool findPath(const PathfindingDesc *desc) {
BZ_ASSERT(desc->map);
BzTileMap *map = desc->map;
@@ -36,12 +126,9 @@ bool findPath(const PathfindingDesc *desc) {
BZ_ASSERT(start.x >= 0 && start.x < map->width);
BZ_ASSERT(start.y >= 0 && start.y < map->height);
typedef struct Visited {
bool visited : 1;
i8 x : 3;
i8 y : 3;
} Visited;
Visited closedSet[map->width * map->height] = {};
PathClosedNode *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);
@@ -79,7 +166,7 @@ bool findPath(const PathfindingDesc *desc) {
// not walkable
if (bzTileMapHasCollision(map, x, y))
continue;
Visited *curClosed = &closedSet[y * map->width + x];
PathClosedNode *curClosed = &closedSet[y * map->width + x];
if (curClosed->visited)
continue;
curClosed->visited = true;
@@ -110,8 +197,7 @@ bool findPath(const PathfindingDesc *desc) {
out->curWaypoint = 0;
BZ_ASSERT(desc->pool);
PathData *pathData = bzObjectPool(desc->pool);
pathData->numWaypoints = 0;
pathData->next = NULL;
bzMemSet(pathData, 0, sizeof(*pathData));
pathData = pushPathWaypoint(pathData, desc->target, desc->pool);
@@ -125,32 +211,26 @@ bool findPath(const PathfindingDesc *desc) {
if (pathLen != 0)
pathData = pushPathWaypoint(pathData, waypoint, desc->pool);
Visited visit = closedSet[pos.y * map->width + pos.x];
PathClosedNode visit = closedSet[pos.y * map->width + pos.x];
BZ_ASSERT(visit.x != 0 || visit.y != 0);
pos.x -= visit.x;
pos.y -= visit.y;
pathLen++;
}
//pushPathWaypoint(pathData, desc->start, desc->pool);
if (pathLen == 0) {
bzObjectPoolRelease(desc->pool, pathData);
pathData = NULL;
}
out->paths = pathData;
// Reverse paths
while (pathData) {
for (i32 i = 0; i < pathData->numWaypoints / 2; i++) {
i32 left = i;
i32 right = (i32) (pathData->numWaypoints - 1 - i);
Position tmp = pathData->waypoints[left];
pathData->waypoints[left] = pathData->waypoints[right];
pathData->waypoints[right] = tmp;
}
pathData = pathData->next;
}
reversePath(pathData);
if (pathLen > 2)
smoothPath(map, pathData, desc->pool);
*desc->outPath = (Path) {pathData, 0};
}
if (!desc->closedSet)
bzFree(closedSet);
if (!desc->openSet)
bzHeapDestroy(openSet);