admin/omath
1
0
Fork 0
mirror of https://github.com/orange-cpp/omath.git synced 2026-02-13 15:03:27 +00:00
Code Issues Packages Projects Releases Wiki Activity

improved code added unit test

Browse Source
This commit is contained in:
Orange
2024-08-18 10:33:01 +03:00
parent f4c988247c
commit 945fd7daa2
5 changed files with 73 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
source/pathfinding/Astar.cpp
View File

@@ -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 {};