101 lines
3.2 KiB
C
101 lines
3.2 KiB
C
#include "../nav.h"
|
|
#include "../../core/types.h"
|
|
#include <stdio.h>
|
|
|
|
#define MAX_UNFINISHED 128
|
|
|
|
typedef struct navSearchNodeState navSearchNodeState;
|
|
struct navSearchNodeState{
|
|
bool visited;
|
|
U64 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(int i = 0; i < meshSize; i++) {
|
|
state.nodeStates[i].visited = false;
|
|
state.nodeStates[i].addedToUnvisited = false;
|
|
// underflow to the max :)
|
|
state.nodeStates[i].distance = -1;
|
|
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 = -1;
|
|
U32 lowestI = -1;
|
|
bool startFound = false;
|
|
for(int 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 = 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) {
|
|
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;}
|
|
U32 distance = state.nodeStates[lowestNodeIndex].distance + connection.Cost;
|
|
distance += mesh.nodes[end].pos.x - mesh.nodes[connection.NodeIndex].pos.x;
|
|
distance += 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;
|
|
}
|