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

Merge pull request #26 from orange-cpp/u/orange-cpp/improved-astar

Browse Source 
Improved A*
This commit is contained in:
Orange++
2025-02-22 23:06:33 +03:00
committed by GitHub
parent 96e4e1c9d6 906f5099d1
commit ae87257adf
2 changed files with 67 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
include/omath/pathfinding/Astar.hpp
View File

@@ -9,11 +9,17 @@
namespace omath::pathfinding
{
struct PathNode;
class Astar final
{
public:
[[nodiscard]]
static std::vector<Vector3> FindPath(const Vector3& start, const Vector3& end, const NavigationMesh& navMesh);
private:
[[nodiscard]]
static std::vector<Vector3> ReconstructFinalPath(const std::unordered_map<Vector3, PathNode>& closedList, const Vector3& current);
[[nodiscard]]
static auto GetPerfectNode(const std::unordered_map<Vector3, PathNode>& openList, const Vector3& endVertex);
};
}

84
source/pathfinding/Astar.cpp
View File

@@ -3,10 +3,10 @@
//
#include "omath/pathfinding/Astar.hpp"
#include <algorithm>
#include <optional>
#include <unordered_map>
#include <unordered_set>
#include <algorithm>
namespace omath::pathfinding
@@ -18,45 +18,83 @@ namespace omath::pathfinding
};
std::vector<Vector3> Astar::FindPath(const Vector3 &start, const Vector3 &end, const NavigationMesh &navMesh)
std::vector<Vector3> Astar::ReconstructFinalPath(const std::unordered_map<Vector3, PathNode>& closedList,
const Vector3& current)
{
std::vector<Vector3> path;
std::optional currentOpt = current;
while (currentOpt)
{
path.push_back(*currentOpt);
auto it = closedList.find(*currentOpt);
if (it == closedList.end())
break;
currentOpt = it->second.cameFrom;
}
std::ranges::reverse(path);
return path;
}
auto Astar::GetPerfectNode(const std::unordered_map<Vector3, PathNode>& openList, const Vector3& endVertex)
{
return std::ranges::min_element(openList,
[&endVertex](const auto& a, const auto& b)
{
const float fA = a.second.gCost + a.first.DistTo(endVertex);
const float fB = b.second.gCost + b.first.DistTo(endVertex);
return fA < fB;
});
}
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.GetClosestVertex(start).value();
const auto endVertex = navMesh.GetClosestVertex(end).value();
auto maybeStartVertex = navMesh.GetClosestVertex(start);
auto maybeEndVertex = navMesh.GetClosestVertex(end);
if (!maybeStartVertex || !maybeEndVertex)
return {};
const auto startVertex = maybeStartVertex.value();
const auto endVertex = maybeEndVertex.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;
});
auto currentIt = GetPerfectNode(openList, endVertex);
closedList.emplace(perfectVertex);
openList.erase(perfectVertex.first);
const auto current = currentIt->first;
const auto currentNode = currentIt->second;
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 (current == endVertex)
return ReconstructFinalPath(closedList, current);
if (perfectVertex.first != endVertex)
continue;
closedList.emplace(current, currentNode);
openList.erase(currentIt);
std::vector<Vector3> path = {};
for (const auto& neighbor: navMesh.GetNeighbors(current))
{
if (closedList.contains(neighbor))
continue;
for (std::optional current = perfectVertex.first; current; current = closedList.at(*current).cameFrom )
path.push_back(current.value());
const float tentativeGCost = currentNode.gCost + neighbor.DistTo(current);
return path;
const auto openIt = openList.find(neighbor);
if (openIt == openList.end() || tentativeGCost < openIt->second.gCost)
openList[neighbor] = PathNode{current, tentativeGCost};
}
}
return {};
}
}
} // namespace omath::pathfinding