diff --git a/include/omath/Vector3.h b/include/omath/Vector3.h index 88d5d56..1bb8807 100644 --- a/include/omath/Vector3.h +++ b/include/omath/Vector3.h @@ -4,6 +4,9 @@ #pragma once +#include +#include + namespace omath { class Vector3 { @@ -73,3 +76,22 @@ namespace omath Vector3 Normalized() const; }; } +// ReSharper disable once CppRedundantNamespaceDefinition +namespace std +{ + template<> + struct hash + { + std::size_t operator()(const omath::Vector3& vec) const noexcept + { + std::size_t hash = 0; + constexpr std::hash 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; + } + }; +} diff --git a/include/omath/pathfinding/Astar.h b/include/omath/pathfinding/Astar.h new file mode 100644 index 0000000..0525679 --- /dev/null +++ b/include/omath/pathfinding/Astar.h @@ -0,0 +1,20 @@ +// +// Created by Vlad on 28.07.2024. +// + +#pragma once +#include +#include "NavigationMesh.h" +#include "omath/Vector3.h" + + +namespace omath::pathfinding +{ + class Astar + { + public: + [[nodiscard]] + static std::vector FindPath(const Vector3& start, const Vector3& end, const NavigationMesh& navMesh); + + }; +} \ No newline at end of file diff --git a/include/omath/pathfinding/NavigationMesh.h b/include/omath/pathfinding/NavigationMesh.h new file mode 100644 index 0000000..7b4ffa6 --- /dev/null +++ b/include/omath/pathfinding/NavigationMesh.h @@ -0,0 +1,40 @@ +// +// Created by Vlad on 28.07.2024. +// + +#pragma once + +#include "omath/Vector3.h" +#include +#include +#include + + +namespace omath::pathfinding +{ + struct NavigationVertex + { + Vector3 origin; + std::vector connections; + }; + + + class NavigationMesh final + { + public: + + [[nodiscard]] + std::expected GetClossestVertex(const Vector3& point) const; + + + [[nodiscard]] + const std::vector& GetNeighbors(const Vector3& vertex) const; + + [[nodiscard]] + bool Empty() const; + [[nodiscard]] std::vector Serialize() const; + void Deserialize(const std::vector& raw); + + std::unordered_map> m_verTextMap; + }; +} \ No newline at end of file diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index 26a3d6a..499567c 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -4,4 +4,5 @@ target_sources(omath PRIVATE color.cpp Vector4.cpp) -add_subdirectory(prediction) \ No newline at end of file +add_subdirectory(prediction) +add_subdirectory(pathfinding) \ No newline at end of file diff --git a/source/pathfinding/Astar.cpp b/source/pathfinding/Astar.cpp new file mode 100644 index 0000000..a5002e9 --- /dev/null +++ b/source/pathfinding/Astar.cpp @@ -0,0 +1,62 @@ +// +// Created by Vlad on 28.07.2024. +// +#include "omath/pathfinding/Astar.h" + +#include +#include +#include +#include + + +namespace omath::pathfinding +{ + struct PathNode final + { + std::optional cameFrom; + float gCost = 0.f; + }; + + + std::vector Astar::FindPath(const Vector3 &start, const Vector3 &end, const NavigationMesh &navMesh) + { + std::unordered_map closedList; + std::unordered_map 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 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/CMakeLists.txt b/source/pathfinding/CMakeLists.txt new file mode 100644 index 0000000..d1da67f --- /dev/null +++ b/source/pathfinding/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(omath PRIVATE NavigationMesh.cpp Astar.cpp) \ No newline at end of file diff --git a/source/pathfinding/NavigationMesh.cpp b/source/pathfinding/NavigationMesh.cpp new file mode 100644 index 0000000..766558e --- /dev/null +++ b/source/pathfinding/NavigationMesh.cpp @@ -0,0 +1,95 @@ +// +// Created by Vlad on 28.07.2024. +// +#include "omath/pathfinding/NavigationMesh.h" + +#include +#include +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); + } + + bool NavigationMesh::Empty() const + { + return m_verTextMap.empty(); + } + + std::vector NavigationMesh::Serialize() const + { + auto dumpToVector =[](const T& t, std::vector& vec){ + for (size_t i = 0; i < sizeof(t); i++) + vec.push_back(*(reinterpret_cast(&t)+i)); + }; + + std::vector 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 &raw) + { + auto loadFromVector = [](const std::vector& 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 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)); + } + } +} 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