BFS

cpp/src/gameloop.cpp

@@ -28,7 +28,7 @@ void GameLoop::Run() {
       for (size_t col = 0; col < tiles[row].size(); col++) {
         // LOG_DEBUG("Drawing rect (", row, ", ", col, ")");
         m_Window->DrawRect(
-            map.TileToWorld(TilePos{row, col}),
+            map.TileEdgeToWorld(TilePos{row, col}),
             map.GetTileSize(), tiles[row][col]->R, tiles[row][col]->G,
             tiles[row][col]->B, tiles[row][col]->A);
       }

cpp/src/main.cpp

@@ -5,7 +5,7 @@
 #include "window.hpp"
 #include <memory>
 
-int main(int argc, char **argv) {
+int main() {
   constexpr int error = -1;
 
   /*

cpp/src/map.cpp

@@ -23,6 +23,10 @@ Map::Map(int rows, int cols) : m_Cols(cols), m_Rows(rows) {
 }
 
 WorldPos Map::TileToWorld(TilePos p) const {
+  return WorldPos{(p.x + 0.5) * TILE_SIZE, (p.y + 0.5) * TILE_SIZE}; 
+}
+
+WorldPos Map::TileEdgeToWorld(TilePos p) const {
   return WorldPos{p.x * TILE_SIZE, p.y * TILE_SIZE}; 
 }
 
@@ -50,3 +54,33 @@ bool Map::IsTilePosValid(TilePos p) const {
 
   return row < m_Tiles.size() && col < m_Tiles[0].size();
 }
+
+
+std::vector<TilePos> Map::GetNeighbors(TilePos center) const
+{
+  std::vector<TilePos> neighbours;
+  neighbours.reserve(4);
+
+  std::array<TilePos, 4> candidates = {
+    center + TilePos{1,0},
+    center + TilePos{-1,0},
+    center + TilePos{0, 1},
+    center + TilePos{0, -1},
+  };
+
+  for (const auto& c : candidates) {
+    if (IsTilePosValid(c))
+        neighbours.push_back(c);
+  }
+  return neighbours;
+}
+//  std::vector<TilePos> neighbours;
+//  neighbours.reserve(8);
+//  for (int dx = -1; dx <= 1; ++dx) {
+//      for (int dy = -1; dy <= 1; ++dy) {
+//          if (dx == 0 && dy == 0) continue;
+//          TilePos p{center.x + dx, center.y + dy};
+//          if (IsTilePosValid(p)) neighbours.push_back(std::move(p));
+//      }
+//  }
+//  return neighbours;

cpp/src/map.hpp

@@ -21,7 +21,9 @@ public:
 
   const TileGrid &GetMapTiles() const { return m_Tiles; }
 
+  // coordinate conversion functions
   WorldPos TileToWorld(TilePos p) const;
+  WorldPos TileEdgeToWorld(TilePos p) const;
   TilePos WorldToTile(WorldPos p) const;
 
   WorldPos GetTileSize() const;
@@ -30,6 +32,9 @@ public:
 
   bool IsTilePosValid(TilePos p) const;
 
+
+  std::vector<TilePos> GetNeighbors(TilePos center) const;
+
   template <typename T> double GetTileVelocityCoeff(T p) const {
     return 1.0 / GetTileAt(p)->cost;
   }

cpp/src/math.hpp

@@ -7,6 +7,7 @@
 #include <iostream>
 #include <utility>
 
+constexpr double EQUALITY_LIMIT = 1e-6;
 
 template <typename T> struct Vec2D {
 public:
@@ -31,6 +32,16 @@ public:
     return Vec2D{k * v.x, k * v.y};
   }
 
+  friend bool operator==(const Vec2D &a, const Vec2D &b) {
+    if constexpr (std::is_integral_v<T>) {
+      return a.x == b.x && a.y == b.y;
+    } else if constexpr (std::is_floating_point_v<T>) {
+      return a.distance(b) < EQUALITY_LIMIT;
+    } else {
+      static_assert("Unhandled comparison");
+    }
+  }
+
   Vec2D operator*(float b) const { return Vec2D{b * x, b * y}; }
 
   T distance_squared(const Vec2D &other) const {
@@ -49,7 +60,7 @@ public:
     requires std::floating_point<T>
   {
     auto length = sqrt(x * x + y * y);
-    if (length < 1e-6) {
+    if (length < EQUALITY_LIMIT) {
       x = y = 0;
     } else {
       x /= length;
@@ -92,3 +103,12 @@ public:
 
 using TilePos = Vec2D<int>;
 using WorldPos = Vec2D<float>;
+
+struct TilePosHash {
+    std::size_t operator()(const TilePos& p) const noexcept {
+        std::size_t h1 = std::hash<int>{}(p.x);
+        std::size_t h2 = std::hash<int>{}(p.y);
+        return h1 ^ (h2 + 0x9e3779b9 + (h1<<6) + (h1>>2));
+    }
+};
+

cpp/src/pathfinder.cpp

@@ -1,34 +1,84 @@
 #include <memory>
 #include <cassert>
+#include <queue>
 
 #include "pathfinder.hpp"
 #include "log.hpp"
 
 namespace pathfinder {
 
-void PathFinderBase::SetMap(const Map* map)
-{
-  m_Map = map;
-}
 
-Path LinearPathFinder::CalculatePath(WorldPos target)
+PathFinderBase::PathFinderBase(const Map* map) : m_Map(map) {}
+
+
+Path LinearPathFinder::CalculatePath(WorldPos start, WorldPos end)
 {
-  auto path = Path{target};
+  auto path = Path{end};
   return path;
 }
 
-Path BFS::CalculatePath(WorldPos target)
-{
-  auto path = Path{target};
+
+Path BFS::CalculatePath(WorldPos start_world, WorldPos end_world) {
+    if (m_Map == nullptr) 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_Distance.clear();
+
+    std::queue<TilePos> frontier;
+    frontier.push(start);
+    m_CameFrom[start] = start;
+    m_Distance[start] = 0.0f;
+
+    // ---------------- build flow-field ----------------
+    bool early_exit = false;
+    while (!frontier.empty() && !early_exit) {
+        TilePos current = frontier.front();
+        frontier.pop();
+
+        for (TilePos next : m_Map->GetNeighbors(current)) {
+            if (m_CameFrom.find(next) == m_CameFrom.end()) {  // not visited
+                frontier.push(next);
+                m_Distance[next] = m_Distance[current] + 1.0f;
+                m_CameFrom[next] = current;
+
+                if (next == end) {          // early exit
+                    early_exit = true;
+                    break;
+                }
+            }
+        }
+    }
+
+    // --------------- reconstruct path -----------------
+    if (m_CameFrom.find(end) == m_CameFrom.end())
+        return {};                        // end not 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<PathFinderBase> create(PathFinderType type) {
+std::unique_ptr<PathFinderBase> create(PathFinderType type, const Map* map) {
   switch (type) {
     case PathFinderType::LINEAR:
-      return std::move(std::make_unique<LinearPathFinder>());
+      return std::move(std::make_unique<LinearPathFinder>(map));
     case PathFinderType::BFS:
-      return std::move(std::make_unique<BFS>());
+      return std::move(std::make_unique<BFS>(map));
     case PathFinderType::COUNT:
       LOG_WARNING("Incorrect pathfinder type");
       return nullptr;

cpp/src/pathfinder.hpp

@@ -2,6 +2,7 @@
 
 #include <vector>
 #include <memory>
+#include <unordered_map>
 
 #include "math.hpp"
 #include "map.hpp"
@@ -18,7 +19,7 @@ enum class PathFinderType {
 
 class PathFinderBase {
 public:
-  PathFinderBase() = default;
+  PathFinderBase(const Map* m);
   ~PathFinderBase() = default;
 
   PathFinderBase(const PathFinderBase&) = delete;
@@ -26,11 +27,10 @@ public:
   PathFinderBase& operator=(const PathFinderBase&) = delete;
   PathFinderBase& operator=(PathFinderBase&&) = delete;
 
-  void SetMap(const Map* map);
   virtual const std::string_view& GetName() const = 0; 
-  virtual Path CalculatePath(WorldPos target) = 0;
+  virtual Path CalculatePath(WorldPos start, WorldPos end) = 0;
 
-private:
+protected:
   const Map* m_Map;
 };
 
@@ -38,24 +38,29 @@ private:
 class LinearPathFinder : public PathFinderBase {
 
 public:
-  Path CalculatePath(WorldPos target) override;
+  LinearPathFinder(const Map* m): PathFinderBase(m) {}
+  Path CalculatePath(WorldPos start, WorldPos end) override;
   const std::string_view& GetName() const override { return m_Name; }
 
 private:
   const std::string_view m_Name = "Linear Path";
 };
 
+
 class BFS: public PathFinderBase {
 
 public:
-  Path CalculatePath(WorldPos target) override;
+  BFS(const Map* m): PathFinderBase(m) {}
+  Path CalculatePath(WorldPos start, WorldPos end) override;
   const std::string_view& GetName() const override { return m_Name; }
 
 private:
   const std::string_view m_Name = "Breadth First Search";
+  std::unordered_map<TilePos, double, TilePosHash> m_Distance;
+  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
 };
 
-std::unique_ptr<PathFinderBase> create(PathFinderType type);
+std::unique_ptr<PathFinderBase> create(PathFinderType type, const Map* map);
 
 } // pathfinder namespace
   

cpp/src/pathfindingdemo.cpp

@@ -16,8 +16,7 @@ PathFindingDemo::PathFindingDemo(int width, int height) :
 {
   LOG_DEBUG(".");
   // set default pathfinder method
-  m_PathFinder = pathfinder::create(pathfinder::PathFinderType::LINEAR);
-  m_PathFinder->SetMap(&m_Map);
+  m_PathFinder = pathfinder::create(pathfinder::PathFinderType::LINEAR, (const Map*)&m_Map);
 }
 
 PathFindingDemo::~PathFindingDemo() { LOG_DEBUG("."); }
@@ -84,13 +83,12 @@ void PathFindingDemo::HandleActions(const std::vector<UserAction> &actions) {
     } else if (action.type == UserAction::Type::SET_MOVE_TARGET) {
       WorldPos wp = action.Argument.position;
       LOG_INFO("Calculating path to target: ", wp);
-      m_Path = m_PathFinder->CalculatePath(wp);
+      m_Path = m_PathFinder->CalculatePath(m_Player->GetPosition(), wp);
       LOG_INFO("Done, path node count: ", m_Path.size());
     } else if (action.type == UserAction::Type::SELECT_PATHFINDER) {
       using namespace pathfinder;
       PathFinderType type = static_cast<PathFinderType>(action.Argument.number);
-      m_PathFinder = create(type);
-      m_PathFinder->SetMap(&m_Map);
+      m_PathFinder = pathfinder::create(type, (const Map*)&m_Map);
       LOG_INFO("Switched to path finding method: ", m_PathFinder->GetName());
     }
   };