129 lines
3.3 KiB
C
129 lines
3.3 KiB
C
//
|
|
// Created by bruno on 7.6.2025.
|
|
//
|
|
|
|
#include "pathfinding.h"
|
|
|
|
uint32_t globalPathfindingVersion = 1;
|
|
|
|
Node openList[MAX_OPEN_NODES];
|
|
int openCount = 0;
|
|
|
|
|
|
void clear_pathfind_data() {
|
|
for (int y = 0; y < MAP_HEIGHT; ++y)
|
|
for (int x = 0; x < MAP_WIDTH; ++x)
|
|
tileMap[y][x].pathFind = (PathFindDat) {0};
|
|
}
|
|
|
|
int heuristic(MiniRect a, MiniRect b) {
|
|
return abs(a.x - b.x) + abs(a.y - b.y);
|
|
}
|
|
|
|
|
|
void add_to_open(MiniRect pos, int fCost) {
|
|
if (openCount < MAX_OPEN_NODES)
|
|
openList[openCount++] = (Node) {pos.x, pos.y, fCost};
|
|
}
|
|
|
|
Node pop_best_node() {
|
|
int bestIdx = 0;
|
|
for (int i = 1; i < openCount; ++i) {
|
|
if (openList[i].fCost < openList[bestIdx].fCost)
|
|
bestIdx = i;
|
|
}
|
|
|
|
Node best = openList[bestIdx];
|
|
openList[bestIdx] = openList[--openCount];
|
|
return best;
|
|
}
|
|
|
|
bool find_path(MiniRect start, MiniRect end) {
|
|
clear_pathfind_data();
|
|
openCount = 0;
|
|
|
|
Tile *startT = &tileMap[start.y][start.x];
|
|
startT->pathFind.gCost = 0;
|
|
startT->pathFind.hCost = heuristic(start, end);
|
|
startT->pathFind.open = true;
|
|
|
|
add_to_open(start, startT->pathFind.gCost + startT->pathFind.hCost);
|
|
|
|
const int dirs[4][2] = {
|
|
{0, -1},
|
|
{1, 0},
|
|
{0, 1},
|
|
{-1, 0}
|
|
};
|
|
|
|
while (openCount > 0) {
|
|
Node current = pop_best_node();
|
|
PathFindDat *currentPath = &tileMap[current.pos.y][current.pos.x].pathFind;
|
|
currentPath->open = false;
|
|
currentPath->closed = true;
|
|
|
|
if (memcmp(¤t.pos, &end, sizeof(MiniRect)) == 0) {
|
|
return true; // Path found!
|
|
}
|
|
|
|
for (int i = 0; i < 4; ++i) {
|
|
MiniRect n = {current.pos.x + dirs[i][0], current.pos.y + dirs[i][1]};
|
|
|
|
if (!isWalkable(n)) continue;
|
|
|
|
PathFindDat *neighbor = &tileMap[n.y][n.x].pathFind;
|
|
|
|
if (neighbor->closed) continue;
|
|
|
|
int tentativeG = currentPath->gCost + 1;
|
|
|
|
if (!neighbor->open || tentativeG < neighbor->gCost) {
|
|
neighbor->gCost = tentativeG;
|
|
neighbor->hCost = heuristic(n, end);
|
|
neighbor->parent = currentPath;
|
|
|
|
if (!neighbor->open) {
|
|
neighbor->open = true;
|
|
add_to_open(n, neighbor->gCost + neighbor->hCost);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return false; // No path found
|
|
}
|
|
|
|
Path reconstruct_path(MiniRect end) {
|
|
Path path = { .length = 0 };
|
|
|
|
PathFindDat* current = &tileMap[end.y][end.x].pathFind;
|
|
|
|
// Walk back through the parent pointers
|
|
while (current != NULL) {
|
|
// Find the tile coordinates for the current node
|
|
for (int y = 0; y < MAP_HEIGHT; ++y) {
|
|
for (int x = 0; x < MAP_WIDTH; ++x) {
|
|
if (&tileMap[y][x].pathFind == current) {
|
|
if (path.length < MAX_OPEN_NODES) {
|
|
path.steps[path.length++] = (MiniRect){ x, y };
|
|
}
|
|
goto found;
|
|
}
|
|
}
|
|
}
|
|
found:
|
|
current = current->parent;
|
|
}
|
|
|
|
// Reverse the path to go from start to end
|
|
for (int i = 0; i < path.length / 2; ++i) {
|
|
MiniRect tmp = path.steps[i];
|
|
path.steps[i] = path.steps[path.length - i - 1];
|
|
path.steps[path.length - i - 1] = tmp;
|
|
}
|
|
path.stepIndex = 0;
|
|
|
|
return path;
|
|
}
|
|
|