feat: async pathfinding

src/voxels/Pathfinding.hpp

@@ -5,6 +5,7 @@
 
 #include <vector>
 #include <memory>
+#include <queue>
 #include <glm/vec2.hpp>
 #include <glm/vec3.hpp>
 #include <unordered_map>
@@ -21,7 +22,28 @@ namespace voxels {
     struct Route {
         bool found;
         std::vector<RouteNode> nodes;
-        std::unordered_set<glm::ivec3> visited;
+    };
+
+    struct Node {
+        glm::ivec3 pos;
+        glm::ivec3 parent;
+        float gScore;
+        float fScore;
+    };
+
+    struct NodeLess {
+        bool operator()(const Node& l, const Node& r) const {
+            return l.fScore > r.fScore;
+        }
+    };
+
+    struct State {
+        std::priority_queue<Node, std::vector<Node>, NodeLess> queue;
+        std::unordered_set<glm::ivec3> blocked;
+        std::unordered_map<glm::ivec3, Node> parents;
+        glm::ivec3 nearest;
+        float minHScore;
+        bool finished = true;
     };
 
     struct Agent {
@@ -32,26 +54,7 @@ namespace voxels {
         glm::ivec3 start;
         glm::ivec3 target;
         Route route;
-    };
-
-    struct Map {
-        int width;
-        int height;
-        std::unique_ptr<uint8_t[]> map;
-
-        Map(int width, int height)
-            : width(width),
-              height(height),
-              map(std::make_unique<uint8_t[]>(width * height)) {
-        }
-
-        uint8_t& operator[](int i) {
-            return map[i];
-        }
-
-        const uint8_t& operator[](int i) const {
-            return map[i];
-        }
+        State state {};
     };
 
     class Pathfinding {
@@ -60,9 +63,9 @@ namespace voxels {
 
         int createAgent();
 
-        Route perform(
-            const Agent& agent, const glm::ivec3& start, const glm::ivec3& end
-        );
+        void performAllAsync(int stepsPerAgent);
+
+        Route perform(Agent& agent, int maxVisited = -1);
 
         Agent* getAgent(int id);