123 lines
3.8 KiB
C
123 lines
3.8 KiB
C
#include "pathfinding.h"
|
|
|
|
static i32 dst(TilePosition a, TilePosition b) {
|
|
//i32 dX = a.x - b.x;
|
|
//i32 dY = a.y - b.y;
|
|
//return dX * dX + dY * dY;
|
|
int dstX = a.x - b.x;
|
|
int dstY = a.y - b.y;
|
|
if (dstX < 0) dstX = -dstX;
|
|
if (dstY < 0) dstY = -dstY;
|
|
|
|
if (dstX > dstY)
|
|
return 14 * dstY + 10 * (dstX - dstY);
|
|
return 14 * dstX + 10 * (dstY - dstX);
|
|
}
|
|
|
|
bool findPath(const PathfindingDesc *desc) {
|
|
BZ_ASSERT(desc->map);
|
|
BzTileMap *map = desc->map;
|
|
typedef struct Visited {
|
|
bool visited : 1;
|
|
i8 x : 3;
|
|
i8 y : 3;
|
|
} Visited;
|
|
Visited visited[map->width * map->height] = {};
|
|
bzMemSet(visited, 0, sizeof(visited));
|
|
|
|
PathNode *heap = desc->heap;
|
|
if (!heap) heap = bzHeapNew(PathNode, map->width * map->height);
|
|
else bzHeapReset(heap);
|
|
|
|
i32 toTargetCost = dst(desc->start, desc->target);
|
|
bzHeapPush(heap, (PathNode) {toTargetCost, 0, toTargetCost, desc->start});
|
|
|
|
bool foundPath = false;
|
|
|
|
while (!bzHeapIsEmpty(heap)) {
|
|
PathNode node = bzHeapPop(heap);
|
|
if (node.pos.x == desc->target.x &&
|
|
node.pos.y == desc->target.y) {
|
|
// Found path
|
|
foundPath = true;
|
|
break;
|
|
}
|
|
|
|
visited[node.pos.y * map->width + node.pos.x].visited = true;
|
|
|
|
for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) {
|
|
for (int x = node.pos.x - 1; x <= node.pos.x + 1; x++) {
|
|
if (x == node.pos.x && y == node.pos.y)
|
|
continue;
|
|
if (y < 0 || y >= map->height ||
|
|
x < 0 || x >= map->width)
|
|
continue;
|
|
if (bzTileMapGetCollider(map, x, y).shapes[0].type != BZ_TILE_SHAPE_NONE)
|
|
continue;
|
|
Visited *curVisited = &visited[y * map->width + x];
|
|
if (curVisited->visited)
|
|
continue;
|
|
curVisited->visited = true;
|
|
|
|
TilePosition curPos = {x, y};
|
|
i32 gCost = node.gCost + dst(node.pos, curPos);
|
|
//if (gCost >= node.gCost) continue;
|
|
toTargetCost = dst(curPos, desc->target);
|
|
|
|
curVisited->x = (i8) (curPos.x - node.pos.x);
|
|
curVisited->y = (i8) (curPos.y - node.pos.y);
|
|
|
|
bzHeapPush(heap, (PathNode) {
|
|
.weight = gCost + toTargetCost,
|
|
.gCost = gCost,
|
|
.hCost = toTargetCost,
|
|
.pos = (TilePosition) {x, y}
|
|
});
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
i32 pathLen = 0;
|
|
if (foundPath && desc->outPath) {
|
|
TilePosition pos = desc->target;
|
|
while (pos.x != desc->start.x || pos.y != desc->start.y) {
|
|
Visited *visit = &visited[pos.y * map->width + pos.x];
|
|
BZ_ASSERT(visit->x != 0 && visit->y != 0);
|
|
pos.x -= visit->x;
|
|
pos.y -= visit->y;
|
|
pathLen++;
|
|
}
|
|
Path *out = desc->outPath;
|
|
out->curWaypoint = 0;
|
|
pos = desc->target;
|
|
i32 len = pathLen;
|
|
// Skip positions
|
|
while (len >= out->maxWaypoints) {
|
|
Visited visit = visited[pos.y * map->width + pos.x];
|
|
pos.x -= visit.x;
|
|
pos.y -= visit.y;
|
|
len--;
|
|
}
|
|
// Write path
|
|
for (i32 i = 0; i < len; i++) {
|
|
out->waypoints[len - i - 1] = (Position){
|
|
pos.x * map->tileWidth + map->tileWidth * 0.5f,
|
|
pos.y * map->tileHeight + map->tileHeight * 0.5f
|
|
};
|
|
out->numWaypoints++;
|
|
Visited visit = visited[pos.y * map->width + pos.x];
|
|
pos.x -= visit.x;
|
|
pos.y -= visit.y;
|
|
}
|
|
BZ_ASSERT(len == out->maxWaypoints);
|
|
out->numWaypoints = len;
|
|
}
|
|
|
|
if (!desc->heap) {
|
|
bzHeapFree(heap);
|
|
heap = NULL;
|
|
}
|
|
|
|
return foundPath ? pathLen : -1;
|
|
} |