Added pathfinder src file

README.md

@@ -48,6 +48,6 @@ Alternatively (or if you want to edit the file), you can use the [Jupyeter Lab o
         - [x] drawing tiles
         - [x] add "terrain tiles" with different costs
     - [x] add mouse-click action
-    - [ ] add direct movement (through mouse click action, no pathfinding)
+    - [x] add direct movement (through mouse click action, no pathfinding)
     - [ ] implement pathfinding
     - [ ] windows build?

cpp/src/main.cpp

@@ -1,4 +1,5 @@
 #include <cassert>
+#include <queue>
 #include <chrono>
 #include <cmath>
 #include <cstdlib>
@@ -13,18 +14,14 @@
 #include "array.hpp"
 #include "log.hpp"
 #include "math.hpp"
+#include "pathfinder.hpp"
 
 //
 // Utils
 //
 
-//
-// Math stuff
-//
-
 // Forward declarations
 class Sprite;
-class Sound;
 class UserAction;
 
 //
@@ -37,29 +34,6 @@ class UserAction;
 #include <SDL3/SDL.h>
 #include <SDL3_image/SDL_image.h>
 
-class Sound {
-public:
-  Sound() { LOG_DEBUG("."); }
-  Sound(const Sound &x) { LOG_DEBUG("."); }
-  Sound(Sound &&x) noexcept { LOG_DEBUG("."); }
-  ~Sound() { LOG_DEBUG("."); }
-};
-
-class AudioOutput {
-public:
-  AudioOutput() { LOG_DEBUG("."); }
-  AudioOutput(const AudioOutput &x) = delete;
-  AudioOutput(AudioOutput &&x) = delete;
-  ~AudioOutput() { LOG_DEBUG("."); }
-
-  std::expected<void, std::string> Init() {
-    LOG_INFO("Initialing audio output");
-    return {};
-  }
-
-  void PlaySound(Sound &s);
-};
-
 class Sprite {
 public:
   Sprite() : m_Texture(nullptr, SDL_DestroyTexture) {}
@@ -375,10 +349,6 @@ public:
   }
 
   virtual Sprite &GetSprite() = 0;
-  virtual constexpr float GetCollisionRadius() const = 0;
-  virtual constexpr float GetCollisionRadiusSquared() {
-    return GetCollisionRadius() * GetCollisionRadius();
-  }
 
   virtual constexpr Type GetType() const = 0;
   void SetFlagExpired() { m_FlagExpired = true; }
@@ -398,34 +368,10 @@ public:
     m_RequestedVelocity = new_velocity;
   }
 
-  void ZeroActualVelocityInDirection(Vec2D<float> direction) {
-    // Vectors e1, e2 form the basis for a local coord system,
-    // where e1 is formed by the direction where we want to zero-out
-    // the velocity, and e2 is the orthogonal vector.
-    // Scalars q1, q2 are coordinates for e1, e2 basis.
-    Vec2D<float> e1 = direction.normalized();
-    Vec2D<float> e2 = e1.orthogonal();
-
-    // q1 * e1 + q2 * e2 = v, from this follows:
-    auto &v = GetActualVelocity();
-    float q2 = (v.y * e1.x - v.x * e1.y) / (e2.y * e1.x - e2.x * e1.y);
-    float q1 = (v.x - q2 * e2.x) / e1.x;
-
-    // We then zero-out the q1, but only if it's positive - meaning
-    // it is aiming in the direction of "direction", not out.
-    // (otherwise we're not able to move out from collision with
-    // another object)
-    if (q1 > 0.0f) {
-      SetActualVelocity(q2 * e2);
-    }
-  }
-
   virtual void Update(float time_delta) {
     m_Position += m_ActualVelocity * time_delta;
   }
 
-  virtual bool IsMovable() const = 0;
-  virtual bool IsCollidable() const = 0;
 
 protected:
   Vec2D<float> m_Position;
@@ -434,7 +380,6 @@ protected:
 
 private:
   bool m_FlagExpired = false;
-  static constexpr float m_CollisionRadiusSq = 1000.0f;
 };
 
 class Wall final : public Entity {
@@ -453,9 +398,6 @@ public:
     return *m_Sprite;
   }
   constexpr Entity::Type GetType() const override { return Entity::Type::WALL; }
-  constexpr float GetCollisionRadius() const override { return 50.0f; }
-  bool IsMovable() const override { return false; }
-  bool IsCollidable() const override { return true; }
 
 private:
   void LoadResources() {
@@ -485,9 +427,6 @@ public:
   constexpr Entity::Type GetType() const override {
     return Entity::Type::PLAYER;
   }
-  constexpr float GetCollisionRadius() const override { return 50.0f; }
-  bool IsMovable() const override { return true; }
-  bool IsCollidable() const override { return true; }
 
 private:
   void LoadResources() {
@@ -499,7 +438,6 @@ private:
 
 std::unique_ptr<Sprite> Player::m_Sprite;
 
-using Collision = std::pair<std::shared_ptr<Entity>, std::shared_ptr<Entity>>;
 
 struct Tile {
   float cost;
@@ -513,8 +451,6 @@ static const std::map<std::string_view, Tile> tile_types = {
     {"Water", {10.0, 0, 50, 200, 255}},
 };
 
-using TilePos = Vec2D<int>;
-using WorldPos = Vec2D<float>;
 
 class Map {
 
@@ -523,17 +459,17 @@ class Map {
 public:
   static constexpr float TILE_SIZE = 100.0f; // tile size in world
 
-  Map(int width, int height) : m_Width(width), m_Height(height) {
+  Map(int rows, int cols) : m_Cols(cols), m_Rows(rows) {
     bool sw = true;
-    LOG_DEBUG("width = ", width, " height = ", height);
+    LOG_DEBUG("cols = ", cols, " rows = ", rows);
     m_Tiles = std::vector<std::vector<const Tile *>>{};
-    for (int i = 0; i < m_Width; i++) {
+    for (size_t row = 0; row < m_Rows; row++) {
       m_Tiles.push_back(std::vector<const Tile *>{});
-      for (int j = 0; j < m_Height; j++) {
+      for (size_t col = 0; col < m_Cols; col++) {
         if (sw)
-          m_Tiles[i].push_back(&tile_types.at("Grass"));
+          m_Tiles[row].push_back(&tile_types.at("Grass"));
         else
-          m_Tiles[i].push_back(&tile_types.at("Water"));
+          m_Tiles[row].push_back(&tile_types.at("Road"));
         sw = !sw;
       }
       sw = !sw;
@@ -557,11 +493,36 @@ public:
     return Vec2D<float>{TILE_SIZE, TILE_SIZE};
   }
 
+  const Tile* GetTileAt(TilePos p) const {
+    assert(IsTilePosValid(p));
+    size_t row = p.x;
+    size_t col = p.y;
+
+    return m_Tiles[row][col];
+
+  }
+
+  const Tile* GetTileAt(WorldPos p) const {
+    return GetTileAt(WorldToTile(p));
+  }
+
+  bool IsTilePosValid(TilePos p) const {
+    size_t row = p.x;
+    size_t col = p.y;
+
+    return row < m_Tiles.size() && col < m_Tiles[0].size();
+  }
+
+  template<typename T>
+  double GetTileVelocityCoeff(T p) const {
+    return 1.0 / GetTileAt(p)->cost;
+  }
+
 private:
   // std::vector<std::vector<const Tile*>> m_Tiles;
   std::vector<std::vector<const Tile *>> m_Tiles;
-  int m_Width = 0;
+  size_t m_Cols = 0;
-  int m_Height = 0;
+  size_t m_Rows = 0;
 };
 
 class PathFindingDemo {
@@ -595,85 +556,41 @@ public:
 
   std::vector<std::shared_ptr<Entity>> &GetEntities() { return m_Entities; }
 
-  void UpdateEntities() {
+  std::optional<WorldPos> GetMoveTarget() {
+    WorldPos current_player_pos = GetPlayer()->GetPosition();
+
+    if (m_MoveQueue.empty()) {
+      return {};
+    }
+
+    WorldPos next_player_pos = m_MoveQueue.front();
+
+    if (current_player_pos.distance(next_player_pos) > 10.0) {
+      // target not reached yet
+      return next_player_pos; 
+    }
+    // target reached, pop it
+    m_MoveQueue.pop();
+    // return nothing - we'll get the next value in the next iteration
+    return {};
+  }
+
+
+  void UpdatePlayerVelocity()
+  {
+    auto player = GetPlayer();
+    auto current_pos = player->GetPosition();
+    double tile_velocity_coeff = m_Map.GetTileVelocityCoeff(current_pos);
+    auto next_pos = GetMoveTarget();
+    auto velocity = WorldPos{};
+    if (next_pos) {
+      velocity = next_pos.value() - current_pos;
+      velocity.normalize();
+      LOG_DEBUG("I want to move to: ", next_pos.value(), ", velocity: ", velocity);
+    }
+    player->SetActualVelocity(velocity * tile_velocity_coeff);
     float time_delta = 1.0f;
-
+    player->Update(time_delta);
-    // Remove entities marked as expired and handle expiry logic (e.g. bomb
-    // exploding); for all other entities, reset the actual velocity to the
-    // requested; actual velocity will be updated later with collisions
-    for (auto &entity : m_Entities) {
-      if (entity->IsFlaggedExpired()) {
-        ExpiryGameLogic(*entity);
-        auto it = std::find(m_Entities.begin(), m_Entities.end(), entity);
-        if (it != m_Entities.end()) {
-          std::swap(*it, m_Entities.back());
-          m_Entities.pop_back();
-        }
-      } else {
-        // Actual velocity might be changed later by collisions
-        entity->SetActualVelocity(entity->GetRequestedVelocity());
-      }
-    }
-
-    // Handle collisions:
-    //  - update actual velocity for colliding objects
-    //  - handle collision logic
-    auto &collisions = GetEntityCollisions();
-    //    LOG_DEBUG("number of collisions: ", collisions.size());
-    for (auto &collision : collisions) {
-      Entity &A = *std::get<0>(collision);
-      Entity &B = *std::get<1>(collision);
-      if (!A.IsMovable())
-        continue;
-      // modify actual speed
-      // LOG_DEBUG("Collision: A is ", A, ", B is ", B);
-      Vec2D<float> AB = B.GetPosition() - A.GetPosition();
-      A.ZeroActualVelocityInDirection(AB);
-      // handle logic
-      CollisionGameLogic(A, B);
-    }
-
-    // Update entities: this advances animations,
-    // internal timers, and updates positions (with velocity
-    // modified by the collision handling)
-    for (auto &entity : m_Entities) {
-      entity->Update(time_delta);
-    }
-  }
-
-  void CollisionGameLogic(Entity &A, Entity &B) {
-    // not used for path finding demo
-  }
-
-  void ExpiryGameLogic(Entity &entity) {
-    // not used for path finding demo
-  }
-
-  const std::vector<Collision> &GetEntityCollisions() {
-    static std::vector<Collision> m_Collisions;
-
-    m_Collisions.clear();
-    for (const auto &entity_A : m_Entities) {
-      for (const auto &entity_B : m_Entities) {
-        if (entity_A == entity_B)
-          continue;
-        if (!entity_A->IsCollidable() || !entity_B->IsCollidable())
-          continue;
-        // check distance of player to given entity
-        auto position_A = entity_A->GetPosition();
-        auto position_B = entity_B->GetPosition();
-        auto distance_sq = position_A.distance_squared(position_B);
-        auto collision_distance_sq =
-            entity_A->GetCollisionRadiusSquared() +
-            entity_B->GetCollisionRadiusSquared() +
-            2 * entity_A->GetCollisionRadius() * entity_B->GetCollisionRadius();
-        // TODO use vector instructions
-        if (distance_sq < collision_distance_sq) {
-          m_Collisions.emplace_back(Collision(entity_A, entity_B));
-        }
-      }
-    }
-    return m_Collisions;
   }
 
   void HandleActions(const std::vector<UserAction> &actions) {
@@ -688,25 +605,27 @@ public:
         LOG_INFO("Move direction ", action.Argument.position);
         m_Player->SetRequestedVelocity(action.Argument.position * 4.0f);
       } else if (action.type == UserAction::Type::MOVE_TARGET) {
-        TilePos p = m_Map.WorldToTile(action.Argument.position);
+        WorldPos wp = action.Argument.position;
-        LOG_INFO("Move target: ", action.Argument.position, ", tile pos: ", p);
+        TilePos p = m_Map.WorldToTile(wp);
-        // TODO move
+        LOG_INFO("Clearing current move queue and inserting new target: ", wp);
+        std::queue<WorldPos> empty;
+        std::swap(empty, m_MoveQueue);
+        m_MoveQueue.push(wp);
       }
     };
   }
 
   const Map &GetMap() const { return m_Map; }
 
-  bool IsCollisionBoxVisible() const { return m_DrawCollisionBox; }
   bool IsExitRequested() const { return m_ExitRequested; }
 
 private:
   int m_Width;
   int m_Height;
   bool m_ExitRequested = false;
-  bool m_DrawCollisionBox = true;
   std::vector<std::shared_ptr<Entity>> m_Entities;
   std::shared_ptr<Player> m_Player;
+  std::queue<WorldPos> m_MoveQueue;
   Map m_Map;
 };
 
@@ -721,15 +640,15 @@ public:
     LOG_INFO("Running the game");
     while (!m_Game->IsExitRequested()) {
       m_Game->HandleActions(m_UserInput->GetActions());
-      m_Game->UpdateEntities();
+      m_Game->UpdatePlayerVelocity();
 
       m_Window->ClearWindow();
 
       // draw the map (terrain tiles)
       const Map &map = m_Game->GetMap();
       const auto &tiles = map.GetMapTiles();
-      for (int row = 0; row < tiles.size(); row++) {
+      for (size_t row = 0; row < tiles.size(); row++) {
-        for (int col = 0; col < tiles[row].size(); col++) {
+        for (size_t col = 0; col < tiles[row].size(); col++) {
           // LOG_DEBUG("Drawing rect (", row, ", ", col, ")");
           m_Window->DrawRect(
               map.TileToWorld(TilePos{row, col}),
@@ -741,10 +660,6 @@ public:
       // draw all the entities (player etc)
       for (auto &entity : m_Game->GetEntities()) {
         m_Window->DrawSprite(entity->GetPosition(), entity->GetSprite());
-        if (m_Game->IsCollisionBoxVisible()) {
-          m_Window->DrawCircle(entity->GetPosition(),
-                               entity->GetCollisionRadius());
-        }
       }
 
       m_Window->Flush();
@@ -760,15 +675,11 @@ public:
   inline void SetUserInput(std::unique_ptr<UserInput> x) {
     m_UserInput = std::move(x);
   }
-  inline void SetAudioOutput(std::unique_ptr<AudioOutput> x) {
-    m_AudioOutput = std::move(x);
-  }
 
 private:
   std::unique_ptr<PathFindingDemo> m_Game;
   std::unique_ptr<Window> m_Window;
   std::unique_ptr<UserInput> m_UserInput;
-  std::unique_ptr<AudioOutput> m_AudioOutput;
 };
 
 int main(int argc, char **argv) {
@@ -791,12 +702,6 @@ int main(int argc, char **argv) {
     return error;
   }
 
-  auto audio_output = std::make_unique<AudioOutput>();
-  if (auto initialized = audio_output->Init(); !initialized) {
-    LOG_ERROR(initialized.error());
-    return error;
-  }
-
   /*
    * Initialize the map and run the pathfinding demo
    */
@@ -807,7 +712,6 @@ int main(int argc, char **argv) {
   auto game_loop = GameLoop{};
   game_loop.SetWindow(std::move(window));
   game_loop.SetUserInput(std::move(user_input));
-  game_loop.SetAudioOutput(std::move(audio_output));
   game_loop.SetGame(std::move(demo));
   game_loop.Run();
 }

cpp/src/math.hpp

@@ -86,3 +86,6 @@ public:
     return os;
   }
 };
+
+using TilePos = Vec2D<int>;
+using WorldPos = Vec2D<float>;

cpp/src/pathfinder.hpp

@@ -0,0 +1,12 @@
+#pragma once
+
+#include "math.hpp"
+
+namespace pathfinder {
+
+  using Path = std::vector<TilePos>;
+
+  class PathFinderBase {
+
+  };
+}