From 945fd7daa213260e13b64ccb061c529e204cb219 Mon Sep 17 00:00:00 2001 From: Orange Date: Sun, 18 Aug 2024 10:33:01 +0300 Subject: [PATCH] improved code added unit test --- include/omath/pathfinding/NavigationMesh.h | 10 ++++-- source/pathfinding/Astar.cpp | 37 +++++++++++++++------- source/pathfinding/NavigationMesh.cpp | 24 +++++++++++++- tests/CMakeLists.txt | 2 +- tests/UnitTestAstar.cpp | 17 ++++++++++ 5 files changed, 73 insertions(+), 17 deletions(-) create mode 100644 tests/UnitTestAstar.cpp diff --git a/include/omath/pathfinding/NavigationMesh.h b/include/omath/pathfinding/NavigationMesh.h index c37da96..f786501 100644 --- a/include/omath/pathfinding/NavigationMesh.h +++ b/include/omath/pathfinding/NavigationMesh.h @@ -24,10 +24,14 @@ namespace omath::pathfinding public: [[nodiscard]] - std::expected GetClossestVertex(const Vector3& point) const; - - private: + std::expected GetClossestVertex(const Vector3& point) const; + [[nodiscard]] + const std::vector& GetNeighbors(const Vector3& vertex) const; + + std::list m_vertexes; + + std::unordered_map> m_verTextMap; }; } \ No newline at end of file diff --git a/source/pathfinding/Astar.cpp b/source/pathfinding/Astar.cpp index 12c3b55..06f2090 100644 --- a/source/pathfinding/Astar.cpp +++ b/source/pathfinding/Astar.cpp @@ -2,6 +2,8 @@ // Created by Vlad on 28.07.2024. // #include "omath/pathfinding/Astar.h" + +#include #include #include @@ -10,8 +12,7 @@ namespace omath::pathfinding { struct PathNode final { - Vector3 cameFrom; - NavigationVertex const* navVertex; + std::optional cameFrom; float gCost = 0.f; }; @@ -21,26 +22,38 @@ namespace omath::pathfinding std::unordered_map closedList; std::unordered_map 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 path = {}; + + for (std::optional current = perfectVertex.first; current; current = closedList.at(*current).cameFrom ) + path.push_back(current.value()); + + return path; } return {}; diff --git a/source/pathfinding/NavigationMesh.cpp b/source/pathfinding/NavigationMesh.cpp index 5ff1b3f..55de0f0 100644 --- a/source/pathfinding/NavigationMesh.cpp +++ b/source/pathfinding/NavigationMesh.cpp @@ -1,4 +1,26 @@ // // Created by Vlad on 28.07.2024. // -#include "omath/pathfinding/NavigationMesh.h" \ No newline at end of file +#include "omath/pathfinding/NavigationMesh.h" + +namespace omath::pathfinding +{ + std::expected 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& NavigationMesh::GetNeighbors(const Vector3 &vertex) const + { + return m_verTextMap.at(vertex); + } +} diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 3c09cbb..9781a8b 100644 --- a/tests/CMakeLists.txt +++ b/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) diff --git a/tests/UnitTestAstar.cpp b/tests/UnitTestAstar.cpp new file mode 100644 index 0000000..91780e8 --- /dev/null +++ b/tests/UnitTestAstar.cpp @@ -0,0 +1,17 @@ +// +// Created by Vlad on 18.08.2024. +// +#include +#include + + +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); +} \ No newline at end of file