74 lines
1.7 KiB
C++
74 lines
1.7 KiB
C++
#include <queue>
|
|
|
|
#include "dijkstra.hpp"
|
|
|
|
#include "base.hpp"
|
|
#include "utils.hpp"
|
|
#include "math.hpp"
|
|
#include "map.hpp"
|
|
|
|
namespace pathfinder {
|
|
|
|
Path Dijkstra::CalculatePath(WorldPos start_world, WorldPos end_world)
|
|
{
|
|
using QueueEntry = utils::QueueEntry;
|
|
|
|
if (!m_Map) return {};
|
|
|
|
const TilePos start = m_Map->WorldToTile(start_world);
|
|
const TilePos end = m_Map->WorldToTile(end_world);
|
|
|
|
if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
|
|
return {};
|
|
if (start == end) return {};
|
|
|
|
// clear previous run
|
|
m_CameFrom.clear();
|
|
m_Cost.clear();
|
|
|
|
std::priority_queue<QueueEntry, std::vector<QueueEntry>, std::greater<>> frontier;
|
|
frontier.push({0.0f, start});
|
|
m_CameFrom[start] = start; // sentinel
|
|
m_Cost[start] = 0.0f;
|
|
|
|
while (!frontier.empty())
|
|
{
|
|
const QueueEntry current = frontier.top();
|
|
frontier.pop();
|
|
|
|
if (current.tile == end) // early exit
|
|
break;
|
|
|
|
for (TilePos next : m_Map->GetNeighbors(current.tile))
|
|
{
|
|
// cost of moving to neighbour (uniform 1.0 matches original BFS)
|
|
const float newCost = m_Cost[current.tile] + m_Map->GetCost(next);
|
|
|
|
if (!m_Cost.count(next) || newCost < m_Cost[next])
|
|
{
|
|
m_Cost[next] = newCost;
|
|
m_CameFrom[next] = current.tile;
|
|
frontier.push({newCost, next});
|
|
}
|
|
}
|
|
}
|
|
|
|
// reconstruct path
|
|
if (!m_CameFrom.count(end))
|
|
return {}; // goal never reached
|
|
|
|
Path path;
|
|
TilePos cur = end;
|
|
path.push_back(m_Map->TileToWorld(cur));
|
|
|
|
while (cur != start)
|
|
{
|
|
cur = m_CameFrom[cur];
|
|
path.push_back(m_Map->TileToWorld(cur));
|
|
}
|
|
std::reverse(path.begin(), path.end());
|
|
return path;
|
|
}
|
|
|
|
} // pathfinder namespace
|