119 lines
3.5 KiB
C
119 lines
3.5 KiB
C
#include "game/nav.h"
|
|
#include "core/types.h"
|
|
|
|
#include <stdio.h>
|
|
|
|
#define MAX_UNFINISHED 128
|
|
|
|
typedef struct navSearchNodeState navSearchNodeState;
|
|
struct navSearchNodeState
|
|
{
|
|
bool visited;
|
|
F64 distance;
|
|
U32 shortest;
|
|
bool addedToUnvisited;
|
|
};
|
|
|
|
typedef struct navSearchState navSearchState;
|
|
struct navSearchState
|
|
{
|
|
navSearchNodeState nodeStates[NAV_MAX_NODES];
|
|
};
|
|
|
|
navSearchState initState(U32 start, U32 meshSize)
|
|
{
|
|
navSearchState state;
|
|
for (U32 i = 0; i < meshSize; i++)
|
|
{
|
|
state.nodeStates[i].visited = false;
|
|
state.nodeStates[i].addedToUnvisited = false;
|
|
state.nodeStates[i].distance = F64_MAX;
|
|
state.nodeStates[i].shortest = 0;
|
|
}
|
|
state.nodeStates[start].distance = 0;
|
|
return state;
|
|
}
|
|
|
|
U32 getLowestState(U32 unfinishedIndexes[128], U32 unfinishedCount, navSearchState state, U32 *offset)
|
|
{
|
|
U32 lowest = U32_MAX;
|
|
U32 lowestI = U32_MAX;
|
|
bool startFound = false;
|
|
for (U32 i = *offset; i < unfinishedCount; i++)
|
|
{
|
|
navSearchNodeState checkNode = state.nodeStates[unfinishedIndexes[i]];
|
|
if (checkNode.visited)
|
|
{
|
|
if (!startFound)
|
|
{
|
|
*offset = i;
|
|
}
|
|
continue;
|
|
}
|
|
startFound = true;
|
|
if (lowest > checkNode.distance)
|
|
{
|
|
lowest = cast(U32) checkNode.distance;
|
|
lowestI = unfinishedIndexes[i];
|
|
}
|
|
}
|
|
return lowestI;
|
|
}
|
|
|
|
// Generate a path to follow between the start and end node.
|
|
NavPath Nav_Path(NavMesh *mesh, U32 start, U32 end) {
|
|
// This is a stupid fix, since it's easier to work backwards
|
|
// to generate a path, I'm swapping the start / end, to generate
|
|
// it backwards
|
|
U32 tmp = end;
|
|
end = start;
|
|
start = tmp;
|
|
navSearchState state = initState(start, mesh->nodeCount);
|
|
U32 unfinishedCount = 1;
|
|
U32 unfinishedIndexes[NAV_MAX_NODES] = {start};
|
|
// I don't want to spend time removing items from
|
|
// the unfinished nodes, so when checking for a lowest
|
|
// if I find the first N items have been checked, I'll mark
|
|
// an offset to skip the first N items.
|
|
U32 unfinishedOffset = 0;
|
|
U32 lowestNodeIndex = start;
|
|
bool found = false;
|
|
while(!found) {
|
|
for(int connectionI = 0 ; connectionI < mesh->nodes[lowestNodeIndex].connectionCount; connectionI++) {
|
|
NavConnection *connection = &mesh->nodes[lowestNodeIndex].connections[connectionI];
|
|
navSearchNodeState *testNode = &state.nodeStates[connection->NodeIndex];
|
|
if(testNode->visited) {continue;}
|
|
F64 distance = cast(F64) (state.nodeStates[lowestNodeIndex].distance + connection->Cost);
|
|
distance += cast(F64) Abs((mesh->nodes[end].pos.x - mesh->nodes[connection->NodeIndex].pos.x));
|
|
distance += cast(F64) Abs((mesh->nodes[end].pos.y - mesh->nodes[connection->NodeIndex].pos.y));
|
|
if(testNode->distance > distance) {
|
|
testNode->distance = distance;
|
|
testNode->shortest = lowestNodeIndex;
|
|
}
|
|
if (!testNode->addedToUnvisited)
|
|
{
|
|
unfinishedIndexes[unfinishedCount] = connection->NodeIndex;
|
|
unfinishedCount++;
|
|
testNode->addedToUnvisited = true;
|
|
}
|
|
}
|
|
state.nodeStates[lowestNodeIndex].visited = true;
|
|
lowestNodeIndex = getLowestState(unfinishedIndexes, unfinishedCount, state, &unfinishedOffset);
|
|
if (lowestNodeIndex == end)
|
|
{
|
|
found = true;
|
|
}
|
|
}
|
|
NavPath res_path = {0};
|
|
U32 index = end;
|
|
while (index != start)
|
|
{
|
|
res_path.indexes[res_path.nodeCount] = index;
|
|
res_path.nodeCount++;
|
|
index = state.nodeStates[index].shortest;
|
|
}
|
|
res_path.indexes[res_path.nodeCount] = start;
|
|
res_path.nodeCount++;
|
|
return res_path;
|
|
}
|