Merge pull request #1 from orange-cpp/pathfinding

Added A* path finding and NavMeshes

include/omath/Vector3.h

@@ -4,6 +4,9 @@
 
 #pragma once
 
+#include <cstdint>
+#include <functional>
+
 namespace omath
 {
     class Vector3 {
@@ -73,3 +76,22 @@ namespace omath
         Vector3 Normalized() const;
     };
 }
+// ReSharper disable once CppRedundantNamespaceDefinition
+namespace std
+{
+    template<>
+    struct hash<omath::Vector3>
+    {
+        std::size_t operator()(const omath::Vector3& vec) const noexcept
+        {
+            std::size_t hash = 0;
+            constexpr std::hash<float> hasher;
+
+            hash ^= hasher(vec.x) + 0x9e3779b9 + (hash<<6) + (hash>>2);
+            hash ^= hasher(vec.y) + 0x9e3779b9 + (hash<<6) + (hash>>2);
+            hash ^= hasher(vec.z) + 0x9e3779b9 + (hash<<6) + (hash>>2);
+
+            return hash;
+        }
+    };
+}

include/omath/pathfinding/Astar.h

@@ -0,0 +1,20 @@
+//
+// Created by Vlad on 28.07.2024.
+//
+
+#pragma once
+#include <vector>
+#include "NavigationMesh.h"
+#include "omath/Vector3.h"
+
+
+namespace omath::pathfinding
+{
+    class Astar
+    {
+    public:
+        [[nodiscard]]
+        static std::vector<Vector3> FindPath(const Vector3& start, const Vector3& end, const NavigationMesh& navMesh);
+
+    };
+}

include/omath/pathfinding/NavigationMesh.h

@@ -0,0 +1,40 @@
+//
+// Created by Vlad on 28.07.2024.
+//
+
+#pragma once
+
+#include "omath/Vector3.h"
+#include <expected>
+#include <vector>
+#include <string>
+
+
+namespace omath::pathfinding
+{
+    struct NavigationVertex
+    {
+        Vector3 origin;
+        std::vector<NavigationVertex*> connections;
+    };
+
+
+    class NavigationMesh final
+    {
+    public:
+
+        [[nodiscard]]
+        std::expected<Vector3, std::string> GetClossestVertex(const Vector3& point) const;
+
+
+        [[nodiscard]]
+        const std::vector<Vector3>& GetNeighbors(const Vector3& vertex) const;
+
+        [[nodiscard]]
+        bool Empty() const;
+        [[nodiscard]] std::vector<uint8_t> Serialize() const;
+        void Deserialize(const std::vector<uint8_t>& raw);
+
+        std::unordered_map<Vector3, std::vector<Vector3>> m_verTextMap;
+    };
+}

source/CMakeLists.txt

@@ -4,4 +4,5 @@ target_sources(omath PRIVATE
         color.cpp
         Vector4.cpp)
 
-add_subdirectory(prediction)
+add_subdirectory(prediction)
+add_subdirectory(pathfinding)

source/pathfinding/Astar.cpp

@@ -0,0 +1,62 @@
+//
+// Created by Vlad on 28.07.2024.
+//
+#include "omath/pathfinding/Astar.h"
+
+#include <optional>
+#include <unordered_map>
+#include <unordered_set>
+#include <algorithm>
+
+
+namespace omath::pathfinding
+{
+    struct PathNode final
+    {
+        std::optional<Vector3> cameFrom;
+        float gCost = 0.f;
+    };
+
+
+    std::vector<Vector3> Astar::FindPath(const Vector3 &start, const Vector3 &end, const NavigationMesh &navMesh)
+    {
+        std::unordered_map<Vector3, PathNode> closedList;
+        std::unordered_map<Vector3, PathNode> openList;
+
+        const auto startVertex = navMesh.GetClossestVertex(start).value();
+        const auto endVertex = navMesh.GetClossestVertex(end).value();
+
+        openList.emplace(startVertex, PathNode{std::nullopt, 0.f});
+
+        while (!openList.empty())
+        {
+            const auto perfectVertex = *std::ranges::min_element(openList,
+                [&endVertex](const auto& a, const auto& b) -> bool
+                {
+                    const auto aCost = a.second.gCost + a.first.DistTo(endVertex);
+                    const auto bCost = b.second.gCost + b.first.DistTo(endVertex);
+                    return aCost < bCost;
+                });
+
+            closedList.emplace(perfectVertex);
+            openList.erase(perfectVertex.first);
+
+            for (const auto& neighbor : navMesh.GetNeighbors(perfectVertex.first))
+                if (!closedList.contains(neighbor))
+                    openList.emplace(neighbor, PathNode{perfectVertex.first, neighbor.DistTo(perfectVertex.first) + perfectVertex.second.gCost});
+
+
+            if (perfectVertex.first != endVertex)
+                continue;
+
+            std::vector<Vector3> path = {};
+
+            for (std::optional current = perfectVertex.first; current; current = closedList.at(*current).cameFrom )
+                path.push_back(current.value());
+
+            return path;
+        }
+
+        return {};
+    }
+}

source/pathfinding/CMakeLists.txt

@@ -0,0 +1 @@
+target_sources(omath PRIVATE NavigationMesh.cpp Astar.cpp)

source/pathfinding/NavigationMesh.cpp

@@ -0,0 +1,95 @@
+//
+// Created by Vlad on 28.07.2024.
+//
+#include "omath/pathfinding/NavigationMesh.h"
+
+#include <stdexcept>
+#include <algorithm>
+namespace omath::pathfinding
+{
+    std::expected<Vector3, std::string> NavigationMesh::GetClossestVertex(const Vector3 &point) const
+    {
+        const auto res = std::ranges::min_element(m_verTextMap,
+            [&point](const auto& a, const auto& b)
+            {
+                return a.first.DistTo(point) < b.first.DistTo(point);
+            });
+
+        if (res == m_verTextMap.cend())
+            return std::unexpected("Failed to get clossest point");
+
+        return res->first;
+    }
+
+    const std::vector<Vector3>& NavigationMesh::GetNeighbors(const Vector3 &vertex) const
+    {
+        return m_verTextMap.at(vertex);
+    }
+
+    bool NavigationMesh::Empty() const
+    {
+        return m_verTextMap.empty();
+    }
+
+    std::vector<uint8_t> NavigationMesh::Serialize() const
+    {
+        auto dumpToVector =[]<typename T>(const T& t, std::vector<uint8_t>& vec){
+            for (size_t i = 0; i < sizeof(t); i++)
+                vec.push_back(*(reinterpret_cast<const uint8_t*>(&t)+i));
+        };
+
+        std::vector<uint8_t> raw;
+
+
+        for (const auto& [vertex, neighbors] : m_verTextMap)
+        {
+            const uint16_t neighborsCount = neighbors.size();
+
+            dumpToVector(vertex, raw);
+            dumpToVector(neighborsCount, raw);
+
+            for (const auto& neighbor : neighbors)
+                dumpToVector(neighbor, raw);
+        }
+        return raw;
+
+    }
+
+    void NavigationMesh::Deserialize(const std::vector<uint8_t> &raw)
+    {
+        auto loadFromVector = [](const std::vector<uint8_t>& vec, size_t& offset, auto& value)
+        {
+            if (offset + sizeof(value) > vec.size())
+            {
+                throw std::runtime_error("Deserialize: Invalid input data size.");
+            }
+            std::copy_n(vec.data() + offset, sizeof(value), (uint8_t*)&value);
+            offset += sizeof(value);
+        };
+
+        m_verTextMap.clear();
+
+        size_t offset = 0;
+
+        while (offset < raw.size())
+        {
+            Vector3 vertex;
+            loadFromVector(raw, offset, vertex);
+
+            uint16_t neighborsCount;
+            loadFromVector(raw, offset, neighborsCount);
+
+            std::vector<Vector3> neighbors;
+            neighbors.reserve(neighborsCount);
+
+            for (size_t i = 0; i < neighborsCount; ++i)
+            {
+                Vector3 neighbor;
+                loadFromVector(raw, offset, neighbor);
+                neighbors.push_back(neighbor);
+            }
+
+            m_verTextMap.emplace(vertex, std::move(neighbors));
+        }
+    }
+}

tests/CMakeLists.txt

@@ -5,7 +5,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_SOURCE_DIR}/out/${CMAKE_BUILD_TYPE}"
 file(GLOB TEST_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/*.cpp)
 
 include(GoogleTest)
-add_executable(unit-tests UnitTestPrediction.cpp UnitTestMatrix.cpp)
+add_executable(unit-tests UnitTestPrediction.cpp UnitTestMatrix.cpp UnitTestAstar.cpp)
 
 target_link_libraries(unit-tests PRIVATE gtest gtest_main omath)
 

tests/UnitTestAstar.cpp

@@ -0,0 +1,17 @@
+//
+// Created by Vlad on 18.08.2024.
+//
+#include <gtest/gtest.h>
+#include <omath/pathfinding/Astar.h>
+
+
+TEST(UnitTestAstar, FindingRightPath)
+{
+    omath::pathfinding::NavigationMesh mesh;
+
+    mesh.m_verTextMap[{0.f, 0.f, 0.f}] = {{0.f, 1.f, 0.f}};
+    mesh.m_verTextMap[{0.f, 1.f, 0.f}] = {{0.f, 2.f, 0.f}};
+    mesh.m_verTextMap[{0.f, 2.f, 0.f}] = {{0.f, 3.f, 0.f}};
+    mesh.m_verTextMap[{0.f, 3.f, 0.f}] = {};
+    omath::pathfinding::Astar::FindPath({}, {0.f, 3.f, 0.f}, mesh);
+}