add pathfinding

src/voxels/Pathfinding.cpp

@@ -0,0 +1,115 @@
+#include "Pathfinding.hpp"
+
+#define GLM_ENABLE_EXPERIMENTAL
+#include <glm/gtx/hash.hpp>
+
+#include <queue>
+#include <unordered_set>
+#include <unordered_map>
+
+#include "world/Level.hpp"
+#include "voxels/GlobalChunks.hpp"
+#include "voxels/Chunk.hpp"
+#include "voxels/blocks_agent.hpp"
+#include "content/Content.hpp"
+
+using namespace voxels;
+
+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;
+    }
+};
+
+static float distance(const glm::ivec3& a, const glm::ivec3& b) {
+    return glm::distance(glm::vec3(a), glm::vec3(b));
+}
+
+Pathfinding::Pathfinding(const Level& level) : level(level) {}
+
+Route Pathfinding::perform(const glm::ivec3& start, const glm::ivec3& end) {
+    Route route {};
+
+    std::priority_queue<Node, std::vector<Node>, NodeLess> queue;
+    queue.push({start, {}, 0, distance(start, end)});
+
+    std::unordered_set<glm::ivec3> blocked;
+    std::unordered_map<glm::ivec3, Node> parents;
+
+    const auto& chunks = *level.chunks;
+
+    while (!queue.empty()) {
+        auto node = queue.top();
+        queue.pop();
+
+        if (blocked.find(node.pos) != blocked.end()) {
+            continue;
+        }
+
+        if (node.pos.x == end.x && node.pos.z == end.z) {
+            auto prev = glm::ivec3();
+            auto pos = node.pos;
+            while (pos != start) {
+                const auto& found = parents.find(pos);
+                if (found == parents.end()) {
+                    route.nodes.push_back({pos});
+                    break;
+                }
+                route.nodes.push_back({pos});
+
+                prev = pos;
+                pos = found->second.pos;
+            }
+            route.nodes.push_back({start});
+            route.found = true;
+            break;
+        }
+
+        blocked.emplace(node.pos);
+        glm::ivec2 neighbors[8] {
+            {0, 1}, {1, 0}, {0, -1}, {-1, 0},
+            {-1, -1}, {1, -1}, {1, 1}, {-1, 1},
+        };
+
+        for (int i = 0; i < sizeof(neighbors) / sizeof(glm::ivec2); i++) {
+            auto offset = neighbors[i];
+            auto point = node.pos + glm::ivec3(offset.x, 0, offset.y);
+            if (blocks_agent::is_obstacle_at(
+                    chunks, point.x + 0.5f, point.y + 0.5f, point.z + 0.5f
+                )) {
+                continue;
+            }
+            if (i >= 4) {
+                auto a = node.pos + glm::ivec3(offset.x, 0, offset.y);
+                auto b = node.pos + glm::ivec3(offset.x, 0, offset.y);
+                if (blocks_agent::is_obstacle_at(chunks, a.x, a.y, a.z))
+                    continue;
+                if (blocks_agent::is_obstacle_at(chunks, b.x, b.y, b.z))
+                    continue;
+            }
+            if (blocked.find(point) != blocked.end()) {
+                continue;
+            }
+            float gScore =
+                node.gScore + glm::abs(offset.x) + glm::abs(offset.y);
+            const auto& foundParent = parents.find(point);
+            bool queued = foundParent != parents.end();
+            if (!queued || gScore < foundParent->second.gScore) {
+                float hScore = distance(point, end);
+                float fScore = gScore + hScore;
+                Node nNode {point, node.pos, gScore, fScore};
+                parents[point] = Node {node.pos, node.parent, gScore, fScore};
+                queue.push(nNode);
+            }
+        }
+    }
+    return route;
+}

src/voxels/Pathfinding.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include <vector>
+#include <memory>
+#include <glm/vec2.hpp>
+#include <glm/vec3.hpp>
+
+class Level;
+
+namespace voxels {
+    struct RouteNode {
+        glm::ivec3 pos;
+    };
+
+    struct Route {
+        bool found;
+        std::vector<RouteNode> nodes;
+    };
+
+    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];
+        }
+    };
+
+    class Pathfinding {
+    public:
+        Pathfinding(const Level& level);
+        
+        Route perform(const glm::ivec3& start, const glm::ivec3& end);
+    private:
+        const Level& level;
+    };
+}

src/world/Level.cpp

@@ -12,6 +12,7 @@
 #include "settings.hpp"
 #include "voxels/Chunk.hpp"
 #include "voxels/GlobalChunks.hpp"
+#include "voxels/Pathfinding.hpp"
 #include "window/Camera.hpp"
 #include "LevelEvents.hpp"
 #include "World.hpp"
@@ -27,7 +28,8 @@ Level::Level(
       physics(std::make_unique<PhysicsSolver>(glm::vec3(0, -22.6f, 0))),
       events(std::make_unique<LevelEvents>()),
       entities(std::make_unique<Entities>(*this)),
-      players(std::make_unique<Players>(*this)) {
+      players(std::make_unique<Players>(*this)),
+      pathfinding(std::make_unique<voxels::Pathfinding>(*this)) {
     const auto& worldInfo = world->getInfo();
     auto& cameraIndices = content.getIndices(ResourceType::CAMERA);
     for (size_t i = 0; i < cameraIndices.size(); i++) {

src/world/Level.hpp

@@ -18,6 +18,10 @@ class Camera;
 class Players;
 struct EngineSettings;
 
+namespace voxels {
+    class Pathfinding;
+}
+
 /// @brief A level, contains chunks and objects
 class Level {
     std::unique_ptr<World> world;
@@ -30,6 +34,7 @@ public:
     std::unique_ptr<LevelEvents> events;
     std::unique_ptr<Entities> entities;
     std::unique_ptr<Players> players;
+    std::unique_ptr<voxels::Pathfinding> pathfinding;
     std::vector<std::shared_ptr<Camera>> cameras;  // move somewhere?
 
     Level(