improved code added unit test

include/omath/pathfinding/NavigationMesh.h

@@ -24,10 +24,14 @@ namespace omath::pathfinding
     public:
 
         [[nodiscard]]
-        std::expected<const NavigationVertex, std::string> GetClossestVertex(const Vector3& point) const;
-
-    private:
+        std::expected<Vector3, std::string> GetClossestVertex(const Vector3& point) const;
 
 
+        [[nodiscard]]
+        const std::vector<Vector3>& GetNeighbors(const Vector3& vertex) const;
+
+        std::list<NavigationVertex> m_vertexes;
+
+        std::unordered_map<Vector3, std::vector<Vector3>> m_verTextMap;
     };
 }

source/pathfinding/Astar.cpp

@@ -2,6 +2,8 @@
 // Created by Vlad on 28.07.2024.
 //
 #include "omath/pathfinding/Astar.h"
+
+#include <optional>
 #include <unordered_map>
 #include <unordered_set>
 
@@ -10,8 +12,7 @@ namespace omath::pathfinding
 {
     struct PathNode final
     {
-        Vector3 cameFrom;
-        NavigationVertex const* navVertex;
+        std::optional<Vector3> cameFrom;
         float gCost = 0.f;
     };
 
@@ -21,26 +22,38 @@ namespace omath::pathfinding
         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();
+        const auto startVertex = navMesh.GetClossestVertex(start).value();
+        const auto endVertex = navMesh.GetClossestVertex(end).value();
 
-        openList.emplace(startVertex.origin, PathNode{startVertex.origin, &startVertex, 0.f});
+        openList.emplace(startVertex, PathNode{std::nullopt, 0.f});
 
         while (!openList.empty())
         {
-            const auto [cord, node] = *std::ranges::min_element(openList,
+            const auto perfectVertex = *std::ranges::min_element(openList,
                 [&endVertex](const auto& a, const auto& b) -> bool
                 {
-                    const auto aCost = a.second.gCost + a.second.navVertex->origin.DistTo(endVertex.origin);
-                    const auto bCost = b.second.gCost + b.second.navVertex->origin.DistTo(endVertex.origin);
+                    const auto aCost = a.second.gCost + a.first.DistTo(endVertex);
+                    const auto bCost = b.second.gCost + b.first.DistTo(endVertex);
                     return aCost < bCost;
                 });
 
-            openList.erase(cord);
+            closedList.emplace(perfectVertex);
+            openList.erase(perfectVertex.first);
 
-            for (const auto& neighbor : node.navVertex->connections)
-                if (!closedList.contains(neighbor->origin))
-                    closedList.emplace(neighbor->origin, PathNode{cord, neighbor, neighbor->origin.DistTo(cord) + node.gCost});
+            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/NavigationMesh.cpp

@@ -2,3 +2,25 @@
 // Created by Vlad on 28.07.2024.
 //
 #include "omath/pathfinding/NavigationMesh.h"
+
+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);
+    }
+}

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);
+}