feat: Added nav and start of npc stuff

code/game/impl/nav.c

@@ -0,0 +1,100 @@
+#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;
+}