diff --git a/cpp/src/pathfinder.cpp b/cpp/src/pathfinder.cpp index 2d44061..091fd70 100644 --- a/cpp/src/pathfinder.cpp +++ b/cpp/src/pathfinder.cpp @@ -196,6 +196,62 @@ Path Dijkstra::CalculatePath(WorldPos start_world, WorldPos end_world) return path; } +float GBFS::Heuristic(const TilePos& a, const TilePos& b) +{ + return static_cast(std::abs(a.x- b.x) + std::abs(a.y - b.y)); +} + +Path GBFS::CalculatePath(WorldPos start_world, WorldPos end_world) +{ + 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 {}; + + m_CameFrom.clear(); + + std::priority_queue, std::greater<>> frontier; + frontier.push({Heuristic(start, end), start}); + m_CameFrom[start] = start; // sentinel + + 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)) + { + if (!m_CameFrom.count(next)) // not visited + { + m_CameFrom[next] = current.tile; + frontier.push({Heuristic(end, next), 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; +} std::unique_ptr create(PathFinderType type, const Map* map) { switch (type) { @@ -205,6 +261,8 @@ std::unique_ptr create(PathFinderType type, const Map* map) { return std::move(std::make_unique(map)); case PathFinderType::DIJKSTRA: return std::move(std::make_unique(map)); + case PathFinderType::GBFS: + return std::move(std::make_unique(map)); case PathFinderType::COUNT: LOG_WARNING("Incorrect pathfinder type"); return nullptr; diff --git a/cpp/src/pathfinder.hpp b/cpp/src/pathfinder.hpp index 1957923..18bf275 100644 --- a/cpp/src/pathfinder.hpp +++ b/cpp/src/pathfinder.hpp @@ -15,6 +15,7 @@ enum class PathFinderType { LINEAR = 1, BFS, DIJKSTRA, + GBFS, COUNT, }; @@ -75,6 +76,21 @@ private: }; +class GBFS: public PathFinderBase { + +public: + GBFS(const Map* m): PathFinderBase(m) {} + Path CalculatePath(WorldPos start, WorldPos end) override; + const std::string_view& GetName() const override { return m_Name; } + +private: + static float Heuristic(const TilePos& a, const TilePos& b); + const std::string_view m_Name = "Greedy Best First Search"; + std::unordered_map m_CameFrom; +}; + + + std::unique_ptr create(PathFinderType type, const Map* map); } // pathfinder namespace