diff --git a/.gitignore b/.gitignore
index fcf12b2..7f1c441 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,4 @@ tags
 python/.ipynb_checkpoints
 cpp/build
 cpp/pathfinding
+.aider*
diff --git a/README.md b/README.md
index 3c3bdf0..c95561a 100644
--- a/README.md
+++ b/README.md
@@ -52,55 +52,3 @@ make -j $(nproc)
 
 Run the `pathfinding` binary in the [cpp](./cpp/) folder.
 
-## TODO
-
-- [x] python
-    - [x] get jupyter lab running
-    - [x] drawing utility
-    - [x] interface for pathfinding
-    - [x] research methods
-    - [x] implement methods
-        - [x] DFS
-        - [x] BFS
-        - [x] Dijsktra
-        - [x] GBFS
-        - [x] A*
-    - [x] performance measurement: time/visited nodes
-    - [x] finalize the script and copy back to the jupyter notebook
-    - [x] finish text on the page
-    - [x] create a dedicated python script
-- [ ] C++
-    - [x] re-use 2D game engine
-    - [x] add tiles (with cost) to map
-        - [x] conversion functions from tile coords to world coords
-        - [x] drawing tiles
-        - [x] add "terrain tiles" with different costs
-    - [x] add mouse-click action
-    - [x] add direct movement (through mouse click action, no pathfinding)
-    - [x] implement pathfinding
-        - [x] BFS
-        - [x] GBFS
-        - [x] Dijkstra
-        - [ ] A*
-    - [ ] windows build?
-        - [x] VS solution
-        - [ ] merge to master
-        - [ ] cmake?
-    - [x] add screenshot
-    - [ ] zoom + pan of the map
-    - [ ] maze generator?
-    - [ ] collisions
-    - [ ] multiple units
-        - change from single unit (player) to RTS-style multiple units
-    - [ ] unit selection
-        - selection rectangle?
-    - [ ] pathfinding for multiple units
-    - [ ] pathfinding for multiple units
-        - group formation, local cohesion, etc
-    - [ ] cpython interface
-        - control the game through the interpreter
-    - [ ] clang-format config
-    - [ ] git hooks?
-    - [ ] [gcovr](https://gcovr.com/en/stable/)
-    - [ ] [clang-tidy](https://clang.llvm.org/extra/clang-tidy/)
-
diff --git a/cpp/Makefile b/cpp/Makefile
index bb0b33e..eeca937 100644
--- a/cpp/Makefile
+++ b/cpp/Makefile
@@ -9,16 +9,23 @@ LDLIBS   := -lSDL3 -lSDL3_image -lGLEW -lGL
 SRC_DIR  := src
 BUILD_DIR:= build
 TARGET   := pathfinding
+TEST_TARGET := unittest
 
 #----------------------------------
 SOURCES := $(shell find $(SRC_DIR) -name '*.cpp')
 OBJECTS := $(SOURCES:$(SRC_DIR)/%.cpp=$(BUILD_DIR)/%.o)
 
 #----------------------------------
-.PHONY: all clean
+.PHONY: all clean test
 
 all: $(TARGET)
 
+test: $(TEST_TARGET)
+	./$(TEST_TARGET)
+
+$(TEST_TARGET): test/test.cpp
+	$(CXX) -std=c++23 -lgtest -Isrc -o $@ $<
+
 # link step
 $(TARGET): $(OBJECTS)
 	$(CXX) $(LDFLAGS) -o $@ $^ $(LDLIBS)
@@ -32,4 +39,4 @@ $(BUILD_DIR):
 	mkdir -p $@
 
 clean:
-	rm -rf $(BUILD_DIR) $(TARGET)
+	rm -rf $(BUILD_DIR) $(TARGET) $(TEST_TARGET)
diff --git a/cpp/src/camera.cpp b/cpp/src/camera.cpp
new file mode 100644
index 0000000..8e04ae7
--- /dev/null
+++ b/cpp/src/camera.cpp
@@ -0,0 +1,45 @@
+#include "camera.hpp"
+#include "math.hpp"
+#include "log.hpp"
+
+// for now only pass-through placeholder functions,
+// since we draw the whole map 
+
+
+void Camera::Pan(const WorldPos& delta)
+{
+  m_Pan += (delta / m_Zoom);
+}
+
+void Camera::Zoom(float delta)
+{
+  constexpr float ZOOM_SCALE = 0.1f;
+  m_Zoom += delta * ZOOM_SCALE;
+  LOG_DEBUG("Zoom: ", m_Zoom);
+}
+
+WindowPos Camera::WorldToWindow(WorldPos world) const
+{
+  const auto& v = world + m_Pan;
+  return WindowPos{v[0], v[1]} * m_Zoom;
+}
+
+WorldPos Camera::WindowToWorld(WindowPos window) const
+{
+  window /= m_Zoom;
+  return WorldPos{window[0], window[1]} - m_Pan;
+}
+
+
+WindowSize Camera::WorldToWindowSize(WorldSize world) const
+{
+  const auto& v = world;
+  // no zoom yet, just pass-through
+  return WindowSize{v[0], v[1]} * m_Zoom;
+}
+
+WorldSize Camera::WindowToWorldSize(WindowSize window) const
+{
+    window /= m_Zoom;
+    return WorldSize{window[0], window[1]};
+}
diff --git a/cpp/src/camera.hpp b/cpp/src/camera.hpp
new file mode 100644
index 0000000..f57b441
--- /dev/null
+++ b/cpp/src/camera.hpp
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "math.hpp"
+
+class Camera
+{
+public:
+  void Pan(const WorldPos& delta);
+  void Zoom(float delta);
+
+  WorldPos GetPan() const { return m_Pan; }
+  float GetZoom() const { return m_Zoom; }
+
+  WindowPos  WorldToWindow(WorldPos)       const;
+  WorldPos   WindowToWorld(WindowPos)      const;
+  WindowSize WorldToWindowSize(WorldSize)  const;
+  WorldSize  WindowToWorldSize(WindowSize) const;
+
+private:
+  // TODO this should be replaced with a matrix
+  float m_Zoom = 1.0f;
+  WorldPos m_Pan;
+};
diff --git a/cpp/src/entities.cpp b/cpp/src/entities.cpp
index e3e0f50..d8e1ad2 100644
--- a/cpp/src/entities.cpp
+++ b/cpp/src/entities.cpp
@@ -25,13 +25,13 @@ void Entity::ZeroActualVelocityInDirection(WorldPos direction) {
   // 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.
-  WorldPos e1 = direction.normalized();
-  WorldPos e2 = e1.orthogonal();
+  WorldPos e1 = direction.GetNormalized();
+  WorldPos e2 = e1.GetOrthogonal();
 
   // 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;
+  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.
diff --git a/cpp/src/gameloop.cpp b/cpp/src/gameloop.cpp
index 85d825e..3f97015 100644
--- a/cpp/src/gameloop.cpp
+++ b/cpp/src/gameloop.cpp
@@ -26,10 +26,20 @@ void GameLoop::Run() {
     const auto &tiles = map.GetMapTiles();
     for (size_t row = 0; row < tiles.size(); row++) {
       for (size_t col = 0; col < tiles[row].size(); col++) {
+        const auto& camera = m_Game->GetCamera();
+        const auto& position = camera.WorldToWindow(
+          map.TileEdgeToWorld(
+            TilePos{static_cast<int32_t>(row), static_cast<int32_t>(col)}
+          )
+        );
+        const auto& size = camera.WorldToWindowSize(
+              map.GetTileSize()
+        );
         // LOG_DEBUG("Drawing rect (", row, ", ", col, ")");
         m_Window->DrawRect(
-            map.TileEdgeToWorld(TilePos{row, col}),
-            map.GetTileSize(), tiles[row][col]->R, tiles[row][col]->G,
+            position,
+            size,
+            tiles[row][col]->R, tiles[row][col]->G,
             tiles[row][col]->B, tiles[row][col]->A);
       }
     }
@@ -37,13 +47,15 @@ void GameLoop::Run() {
     // draw the path, if it exists
     WorldPos start_pos = m_Game->GetPlayer()->GetPosition();
     for (const auto& next_pos: m_Game->GetPath()) {
-      m_Window->DrawLine(start_pos, next_pos);
+      const auto& camera = m_Game->GetCamera();
+      m_Window->DrawLine(camera.WorldToWindow(start_pos), camera.WorldToWindow(next_pos));
       start_pos = next_pos;
     }
 
     // draw all the entities (player etc)
     for (auto &entity : m_Game->GetEntities()) {
-      m_Window->DrawSprite(entity->GetPosition(), entity->GetSprite());
+      const auto& camera = m_Game->GetCamera();
+      m_Window->DrawSprite(camera.WorldToWindow(entity->GetPosition()), entity->GetSprite(), camera.GetZoom());
     }
     
     m_Window->Flush();
diff --git a/cpp/src/map.cpp b/cpp/src/map.cpp
index aa48775..902880f 100644
--- a/cpp/src/map.cpp
+++ b/cpp/src/map.cpp
@@ -17,23 +17,23 @@ 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}; 
+  return WorldPos{(p.x() + 0.5f) * TILE_SIZE, (p.y() + 0.5f) * TILE_SIZE}; 
 }
 
 WorldPos Map::TileEdgeToWorld(TilePos p) const {
-  return WorldPos{p.x * TILE_SIZE, p.y * TILE_SIZE}; 
+  return WorldPos{p.x() * TILE_SIZE, p.y() * TILE_SIZE}; 
 }
 
 TilePos Map::WorldToTile(WorldPos p) const {
-  return TilePos{p.x / TILE_SIZE, p.y / TILE_SIZE};
+  return TilePos{static_cast<int32_t>(p.x() / TILE_SIZE), static_cast<int32_t>(p.y() / TILE_SIZE)};
 }
 
-WorldPos Map::GetTileSize() const { return WorldPos{TILE_SIZE, TILE_SIZE}; }
+WorldSize Map::GetTileSize() const { return WorldSize{TILE_SIZE, TILE_SIZE}; }
 
 const Tile *Map::GetTileAt(TilePos p) const {
   assert(IsTilePosValid(p));
-  size_t row = p.x;
-  size_t col = p.y;
+  size_t row = p.x();
+  size_t col = p.y();
 
   return m_Tiles[row][col];
 }
@@ -43,10 +43,10 @@ const Tile *Map::GetTileAt(WorldPos p) const {
 }
 
 bool Map::IsTilePosValid(TilePos p) const {
-  if (p.x < 0 || p.y < 0)
+  if (p.x() < 0 || p.y() < 0)
     return false;
-  size_t row = static_cast<size_t>(p.x);
-  size_t col = static_cast<size_t>(p.y);
+  size_t row = static_cast<size_t>(p.x());
+  size_t col = static_cast<size_t>(p.y());
 
   return row < m_Tiles.size() && col < m_Tiles[0].size();
 }
@@ -75,14 +75,14 @@ std::vector<TilePos> Map::GetNeighbors(TilePos center) const
 void Map::PaintCircle(TilePos center, unsigned radius, TileType tile_type)
 {
   // get rectangle that wraps the circle
-  TilePos corner1 = TilePos{center.x - radius, center.y - radius};
-  TilePos corner2 = TilePos{center.x + radius, center.y + radius};
+  TilePos corner1 = TilePos{center.x() - static_cast<int32_t>(radius), center.y() - static_cast<int32_t>(radius)};
+  TilePos corner2 = TilePos{center.x() + static_cast<int32_t>(radius), center.y() + static_cast<int32_t>(radius)};
   // iterate through all valid points, setting the type
   const unsigned radius_squared = radius * radius; 
-  for (int x = corner1.x; x < corner2.x; x++) {
-    for (int y = corner1.y; y < corner2.y; y++) {
+  for (int x = corner1.x(); x < corner2.x(); x++) {
+    for (int y = corner1.y(); y < corner2.y(); y++) {
       TilePos current_tile = {x, y}; 
-      unsigned distance_squared = center.distance_squared(current_tile);
+      unsigned distance_squared = static_cast<unsigned>(center.DistanceTo(current_tile) * center.DistanceTo(current_tile));
       if (IsTilePosValid(current_tile) && distance_squared < radius_squared)
       {
         // y is row, x is col
@@ -94,19 +94,20 @@ void Map::PaintCircle(TilePos center, unsigned radius, TileType tile_type)
 
 void Map::PaintLine(TilePos start_tile, TilePos stop_tile, double width, TileType tile_type)
 {
-  const Vec2D<double> start(start_tile);
-  const Vec2D<double> stop(stop_tile);
-  const double line_length = start.distance(stop);
-  const Vec2D<double> step((stop-start)/line_length);
-  const Vec2D<double> ortho = step.orthogonal();
+  const vec<double, 2> start{static_cast<double>(start_tile.x()), static_cast<double>(start_tile.y())};
+  const vec<double, 2> stop{static_cast<double>(stop_tile.x()), static_cast<double>(stop_tile.y())};
+  const double line_length = start.DistanceTo(stop);
+  const vec<double, 2> step = (stop - start) / line_length;
+  const vec<double, 2> ortho = step.GetOrthogonal();
   LOG_DEBUG("step = ", step, " ortho = ", ortho);
 
   for (double t = 0; t < line_length; t += 1.0) {
     for (double ortho_t = 0; ortho_t < width; ortho_t += 0.1) {
       auto tile_pos = start + step * t + ortho * ortho_t;
-      if (IsTilePosValid(tile_pos)) {
-        size_t row = static_cast<size_t>(tile_pos.x);
-        size_t col = static_cast<size_t>(tile_pos.y);
+      TilePos tile_pos_int{static_cast<int32_t>(tile_pos.x()), static_cast<int32_t>(tile_pos.y())};
+      if (IsTilePosValid(tile_pos_int)) {
+        size_t row = static_cast<size_t>(tile_pos.x());
+        size_t col = static_cast<size_t>(tile_pos.y());
         m_Tiles[row][col] = &tile_types.at(tile_type);
       }
     }
@@ -116,15 +117,15 @@ void Map::PaintLine(TilePos start_tile, TilePos stop_tile, double width, TileTyp
 
 void Map::PaintRectangle(TilePos first_corner, TilePos second_corner, TileType tile_type)
 {
-  std::initializer_list<int> xvals = {first_corner.x, second_corner.x};
-  std::initializer_list<int> yvals = {first_corner.y, second_corner.y};
+  std::initializer_list<int> xvals = {first_corner.x(), second_corner.x()};
+  std::initializer_list<int> yvals = {first_corner.y(), second_corner.y()};
   for (int x = std::min(xvals); x < std::max(xvals); x++) {
     for (int y = std::min(yvals); y < std::max(yvals); y++) {
       TilePos tile_pos{x,y};
       LOG_DEBUG("tile_pos = ", tile_pos); 
       if (IsTilePosValid(tile_pos)) {
-        size_t row = static_cast<size_t>(tile_pos.x);
-        size_t col = static_cast<size_t>(tile_pos.y);
+        size_t row = static_cast<size_t>(tile_pos.x());
+        size_t col = static_cast<size_t>(tile_pos.y());
         m_Tiles[row][col] = &tile_types.at(tile_type);
       }
     }
diff --git a/cpp/src/map.hpp b/cpp/src/map.hpp
index 2be7b64..6b400f6 100644
--- a/cpp/src/map.hpp
+++ b/cpp/src/map.hpp
@@ -25,8 +25,8 @@ public:
   WorldPos TileToWorld(TilePos p) const;
   WorldPos TileEdgeToWorld(TilePos p) const;
   TilePos WorldToTile(WorldPos p) const;
-
-  WorldPos GetTileSize() const;
+  
+  WorldSize GetTileSize() const;
   const Tile *GetTileAt(TilePos p) const;
   const Tile *GetTileAt(WorldPos p) const;
 
diff --git a/cpp/src/math.hpp b/cpp/src/math.hpp
index f7e5f5e..fbd0682 100644
--- a/cpp/src/math.hpp
+++ b/cpp/src/math.hpp
@@ -1,143 +1,397 @@
 #pragma once
 
+#include <algorithm>
 #include <cassert>
 #include <cmath>
 #include <concepts>
 #include <initializer_list>
 #include <iostream>
+#include <numeric>
+#include <ranges>
 #include <utility>
-
-constexpr double EQUALITY_LIMIT = 1e-6;
+#include <functional>
 
 #ifdef _WIN32
 #include <numbers>
 #define M_PI std::numbers::pi
+// TODO use std::numbers::pi instead of M_PI
 #endif
 
-template <typename T> struct Vec2D {
+template <typename T>
+  requires std::floating_point<T>
+static inline bool equalEpsilon(const T &a, const T &b) {
+  constexpr auto epsilon = []() {
+    if constexpr (std::is_same_v<T, float>) {
+      return T{1e-5};
+    } else {
+      return T{1e-12}; // double, long double
+    }
+  }();
+  if (a == b) {
+    // handle special cases: bit equality, Inf...
+    return true;
+  }
+  return std::abs(a - b) < epsilon;
+}
+
+struct Any {};
+
+template <typename T, size_t N, typename Tag = Any> class vec {
 public:
-  Vec2D() = default;
-  ~Vec2D() = default;
+  vec() : m_Array{} {}
 
-  template <typename U>
-  Vec2D(Vec2D<U> other) {
-    this->x = static_cast<T>(other.x);
-    this->y = static_cast<T>(other.y);
+  template <typename... ArgsT>
+    requires(std::same_as<ArgsT, T> && ...) && (sizeof...(ArgsT) == N)
+  vec(ArgsT... args) : m_Array{args...} {}
+
+  //
+  // Access to elements & data
+  //
+
+  const T &operator[](size_t index) const {
+    // we leave run-time checks to the underlying std::array
+    return m_Array[index];
   }
 
-  Vec2D& operator+=(const Vec2D &other) {
-    x += other.x;
-    y += other.y;
-    return *this;
+  T &operator[](size_t index) {
+    // we leave run-time checks to the underlying std::array
+    return m_Array[index];
   }
 
-  template <typename U>
-  requires std::is_arithmetic_v<U>
-  Vec2D& operator/=(U k) {
-    this->x /= static_cast<T>(k);
-    this->y /= static_cast<T>(k);
-    return *this;
-  }
-
-  friend Vec2D operator+(const Vec2D &a, const Vec2D &b) {
-    return Vec2D{a.x + b.x, a.y + b.y};
-  }
-  
-  friend Vec2D operator-(const Vec2D &a, const Vec2D &b) {
-    return Vec2D{a.x - b.x, a.y - b.y};
-  }
-
-  template <typename U>
-    requires std::is_arithmetic_v<U>
-  friend Vec2D operator*(U k, const Vec2D &v)
-  {
-    return Vec2D{k * v.x, k * v.y};
-  }
-
-  template <typename U>
-    requires std::is_arithmetic_v<U>
-  friend Vec2D operator/(const Vec2D &v, U k)
-  {
-    return Vec2D{v.x / k, v.y / k};
-  }
-
-  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");
+  friend std::ostream &operator<<(std::ostream &os, const vec &obj) {
+    os << "( ";
+    for (const auto &element : obj.m_Array) {
+      os << element << " ";
     }
-  }
-
-  Vec2D operator*(float b) const { return Vec2D{b * x, b * y}; }
-
-  T distance_squared(const Vec2D &other) const {
-    T dx = x - other.x;
-    T dy = y - other.y;
-    return dx * dx + dy * dy;
-  }
-
-  T distance(const Vec2D &other) const
-    requires std::floating_point<T>
-  {
-    return sqrt(distance_squared(other));
-  }
-
-  void normalize()
-    requires std::floating_point<T>
-  {
-    auto length = sqrt(x * x + y * y);
-    if (length < EQUALITY_LIMIT) {
-      x = y = 0;
-    } else {
-      x /= length;
-      y /= length;
-    }
-  }
-
-  Vec2D normalized()
-    requires std::floating_point<T>
-  {
-    Vec2D v(*this);
-    v.normalize();
-    return v;
-  }
-
-  Vec2D orthogonal() const
-  {
-    Vec2D v(*this);
-    
-    std::swap(v.x, v.y);
-    v.x = -v.x;
-    return v;
-  }
-
-  template <typename U> Vec2D(std::initializer_list<U> list) {
-    assert(list.size() == 2);
-    auto first_element = *list.begin();
-    auto second_element = *(list.begin() + 1);
-    x = static_cast<T>(first_element);
-    y = static_cast<T>(second_element);
-  }
-
-  T x, y;
-
-  friend std::ostream &operator<<(std::ostream &os, const Vec2D &obj) {
-    os << "( " << obj.x << ", " << obj.y << ")";
+    os << ")";
     return os;
   }
+
+  std::array<T,N>& Data() { return m_Array; }
+
+  //
+  // binary operators
+  //
+
+  friend bool operator==(const vec &a, const vec &b)
+    requires (std::is_integral_v<T>)
+  {
+    return std::ranges::equal(a.m_Array, b.m_Array);
+  }
+
+  friend bool operator==(const vec &a, const vec &b)
+    requires (std::is_floating_point_v<T>)
+  {
+    for (const auto &[u, v] : std::views::zip(a.m_Array, b.m_Array)) {
+      if (!equalEpsilon(u, v)) {
+        return false;
+      }
+    }
+    return true;
+  }
+
+  friend bool operator!=(const vec &a, const vec &b) { return !(a == b); }
+
+  friend vec operator+(const vec &a, const vec &b) {
+    vec<T, N, Tag> c;
+    std::ranges::transform(a.m_Array, b.m_Array, c.m_Array.begin(),
+                           std::plus{});
+    return c;
+  }
+
+  friend vec operator-(const vec &a, const vec &b) {
+    vec<T, N, Tag> c;
+    std::ranges::transform(a.m_Array, b.m_Array, c.m_Array.begin(),
+                           std::minus{});
+    return c;
+  }
+
+  friend vec operator*(const vec &a, const T &scalar) {
+    vec<T, N, Tag> c;
+    std::ranges::transform(a.m_Array, std::views::repeat(scalar),
+                           c.m_Array.begin(), std::multiplies{});
+    return c;
+  }
+
+  friend vec operator*(const T &scalar, const vec &a) { return a * scalar; }
+
+  friend vec operator/(const vec &a, const T &scalar) {
+    vec<T, N, Tag> c;
+    std::ranges::transform(a.m_Array, std::views::repeat(scalar),
+                           c.m_Array.begin(), std::divides{});
+    return c;
+  }
+
+  //
+  // compound-assignment operators
+  //
+
+  vec &operator+=(const vec &b) {
+    vec &a = *this;
+    std::ranges::transform(a.m_Array, b.m_Array, a.m_Array.begin(),
+                           std::plus{});
+    return a;
+  }
+
+  vec &operator-=(const vec &b) {
+    vec &a = *this;
+    std::ranges::transform(a.m_Array, b.m_Array, a.m_Array.begin(),
+                           std::minus{});
+    return a;
+  }
+
+  vec& operator/=(float scalar)
+  {
+    vec& a = *this;
+    auto b = std::views::repeat(scalar);
+    std::ranges::transform(a.m_Array, b, a.m_Array.begin(), std::divides{});
+    // TODO check all of this, could be done better with views instead of ranges?
+    return a;
+  }
+
+  //
+  // Utility functions
+  //
+
+  T LengthSquared() const {
+    return std::transform_reduce(m_Array.begin(), m_Array.end(), T{},
+                                 std::plus{}, [](T x) { return x * x; });
+  }
+
+  T Length() const { return std::sqrt(LengthSquared()); }
+
+  T DistanceTo(const vec &b) const {
+    const vec &a = *this;
+    return (a - b).Length();
+  }
+
+  //
+  // In-place vector operations
+  //
+
+  void Normalize() {
+    T length = Length();
+    if (equalEpsilon(length, T{0}))
+      return;
+    std::ranges::transform(m_Array, std::views::repeat(length), m_Array.begin(),
+                           std::divides{});
+  }
+
+  //
+  // Methods returning new object
+  //
+
+  vec GetNormalized() const {
+    vec tmp = *this;
+    tmp.Normalize();
+    return tmp;
+  }
+
+  vec GetOrthogonal() const
+    requires(N == 2)
+  {
+    vec tmp = *this;
+
+    std::swap(tmp.m_Array[0], tmp.m_Array[1]);
+    tmp.m_Array[0] *= -1;
+
+    return tmp;
+  }
+
+  static T DotProduct(const vec& a, const vec& b)
+  {
+    return std::inner_product(
+        a.m_Array.begin(), a.m_Array.end(), b.m_Array.begin(), 
+        T{}, std::plus{}, std::multiplies{});
+  }
+
+  T DotProduct(const vec& b) const
+  {
+    const auto& a = *this;
+    return DotProduct(a, b);
+  }
+
+  //
+  // Helpers
+  //
+
+  const T &x() const
+    requires(N >= 1)
+  {
+    return m_Array[0];
+  }
+
+  T &x()
+    requires(N >= 1)
+  {
+    return m_Array[0];
+  }
+
+  const T &y() const
+    requires(N >= 2)
+  {
+    return m_Array[1];
+  }
+
+  T &y()
+    requires(N >= 2)
+  {
+    return m_Array[1];
+  }
+
+  const T &z() const
+    requires(N >= 3)
+  {
+    return m_Array[2];
+  }
+
+  T &z()
+    requires(N >= 3)
+  {
+    return m_Array[2];
+  }
+
+private:
+  std::array<T, N> m_Array;
 };
 
-using TilePos = Vec2D<int>;
-using WorldPos = Vec2D<float>;
+//
+// Aliases
+//
+
+using vec2 = vec<float, 2>;
+using vec3 = vec<float, 3>;
+using vec4 = vec<float, 4>;
+using dvec2 = vec<double, 2>;
+using dvec3 = vec<double, 3>;
+using dvec4 = vec<double, 4>;
+using ivec2 = vec<std::int32_t, 2>;
+using ivec3 = vec<std::int32_t, 3>;
+using ivec4 = vec<std::int32_t, 4>;
+using uvec2 = vec<std::uint32_t, 2>;
+using uvec3 = vec<std::uint32_t, 3>;
+using uvec4 = vec<std::uint32_t, 4>;
+
+// tags for differentiating between domains
+struct WorldPosTag {};
+struct WorldSizeTag {};
+struct WindowPosTag {};
+struct WindowSizeTag {};
+struct TilePosTag {};
+struct TileSizeTag {};
+
+// types for each domain
+using WorldPos = vec<float, 2, WorldPosTag>;
+using WindowPos = vec<float, 2, WindowPosTag>;
+using TilePos = vec<int32_t, 2, TilePosTag>;
+// Size
+using WorldSize = vec<float, 2, WorldSizeTag>;
+using WindowSize = vec<float, 2, WindowSizeTag>;
+using TileSize = vec<int32_t, 2, TileSizeTag>;
+
+//
+// Utils
+//
 
 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));
-    }
+  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));
+  }
 };
 
+//
+// Matrix
+//
+
+// Collumn major square matrix
+template <typename T, size_t N, typename Tag = Any>
+class Matrix {
+
+using vec_type = vec<T, N, Tag>;
+
+public:
+    Matrix() = default;
+
+    // Initialization using flat array of N*N elements
+    template <typename Tarr, size_t M>
+    requires (M == N*N && std::same_as<Tarr, T>)
+    Matrix(std::array<Tarr,M> array) : m_Array{}
+    {
+        std::size_t idx = 0;
+        for (auto col : array | std::views::chunk(N))
+        {
+            std::ranges::copy(col, m_Array[idx++].Data().begin());
+        }
+    }
+
+    const vec_type& operator[](size_t index) const { return m_Array[index]; }
+    vec_type& operator[](size_t index) { return m_Array[index]; }
+
+    friend std::ostream &operator<<(std::ostream &os, const Matrix &obj)
+    {
+        os << "( ";
+        for (const auto &element : obj.m_Array) {
+        os << element << " ";
+        }
+        os << ")";
+        return os;
+    }
+
+
+    friend Matrix operator+(const Matrix& A, const Matrix& B)
+    {
+        Matrix C;
+        std::ranges::transform(A.m_Array, B.m_Array, C.m_Array.begin(), std::plus{});
+        return C;
+    }
+
+    friend Matrix operator-(const Matrix& A, const Matrix& B)
+    {
+        Matrix C;
+        std::ranges::transform(A.m_Array, B.m_Array, C.m_Array.begin(), std::minus{});
+        return C;
+    }
+
+    friend Matrix operator*(const Matrix& A, const Matrix& B)
+    {
+        Matrix C;
+
+        for (size_t i = 0; i < N; i++)
+        {
+            for (size_t j = 0; j < N; j++)
+            {
+                T sum = 0;
+                for (size_t k = 0; k < N; ++k) sum += A[i][k] * B[k][j];
+                C[i][j] = sum;
+            }
+        }
+        return C;
+    }
+
+    friend vec_type operator*(const Matrix& A, const vec_type& b)
+    {
+        // we assume that b is row vector
+        vec_type c;
+        for (size_t i = 0; i < N; i++)
+        {
+            c[i] = b.DotProduct(A[i]);
+        }
+        return c;
+    }
+
+    static constexpr Matrix Eye()
+    {
+        Matrix E;
+        for (size_t i = 0; i < N; i++)
+        {
+            E[i][i] = T{1};
+        }
+        return E;
+    }
+
+
+private:
+    std::array<vec_type, N> m_Array;
+};
\ No newline at end of file
diff --git a/cpp/src/pathfinder/base.cpp b/cpp/src/pathfinder/base.cpp
index 5468672..cccaf5b 100644
--- a/cpp/src/pathfinder/base.cpp
+++ b/cpp/src/pathfinder/base.cpp
@@ -13,7 +13,7 @@ PathFinderBase::PathFinderBase(const Map* map) : m_Map(map) {}
 
 // LinearPathFinder also lives here, since it is too small to get it's
 // own implementation file
-Path LinearPathFinder::CalculatePath(WorldPos start, WorldPos end)
+Path LinearPathFinder::CalculatePath(WorldPos, WorldPos end) // first argument (start pos) not used
 {
   auto path = Path{end};
   return path;
diff --git a/cpp/src/pathfinder/gbfs.cpp b/cpp/src/pathfinder/gbfs.cpp
index 8b9bbfd..02684ca 100644
--- a/cpp/src/pathfinder/gbfs.cpp
+++ b/cpp/src/pathfinder/gbfs.cpp
@@ -11,7 +11,7 @@ namespace pathfinder {
 
 float GBFS::Heuristic(const TilePos& a, const TilePos& b)
 {
-    return static_cast<float>(std::abs(a.x- b.x) + std::abs(a.y - b.y));
+    return static_cast<float>(std::abs(a.x() - b.x()) + std::abs(a.y() - b.y()));
 }
 
 Path GBFS::CalculatePath(WorldPos start_world, WorldPos end_world)
diff --git a/cpp/src/pathfindingdemo.cpp b/cpp/src/pathfindingdemo.cpp
index aa2b899..eef2382 100644
--- a/cpp/src/pathfindingdemo.cpp
+++ b/cpp/src/pathfindingdemo.cpp
@@ -63,7 +63,7 @@ void PathFindingDemo::CreateMap() {
 }
 
 WorldPos PathFindingDemo::GetRandomPosition() const {
-  return WorldPos{0.0, 0.0}; // totally random!
+  return WorldPos{0.0f, 0.0f}; // totally random!
 }
 
 std::optional<WorldPos> PathFindingDemo::GetMoveTarget() {
@@ -75,7 +75,7 @@ std::optional<WorldPos> PathFindingDemo::GetMoveTarget() {
 
   WorldPos next_player_pos = m_Path.front();
 
-  if (current_player_pos.distance(next_player_pos) > 1.0) {
+  if (current_player_pos.DistanceTo(next_player_pos) > 1.0) {
     // target not reached yet
     return next_player_pos;
   }
@@ -93,9 +93,10 @@ void PathFindingDemo::UpdatePlayerVelocity() {
   double tile_velocity_coeff = m_Map.GetTileVelocityCoeff(current_pos);
   auto next_pos = GetMoveTarget();
   WorldPos velocity = WorldPos{};
-  if (next_pos) {
+  if (next_pos)
+  {
     velocity = next_pos.value() - current_pos;
-    velocity.normalize();
+    velocity.Normalize();
     //LOG_DEBUG("I want to move to: ", next_pos.value(),
     //          ", velocity: ", velocity);
   }
@@ -104,21 +105,40 @@ void PathFindingDemo::UpdatePlayerVelocity() {
   player->Update(time_delta);
 }
 
-void PathFindingDemo::HandleActions(const std::vector<UserAction> &actions) {
-  for (const auto &action : actions) {
-    if (action.type == UserAction::Type::EXIT) {
+void PathFindingDemo::HandleActions(const std::vector<UserAction> &actions)
+{
+  for (const auto &action : actions)
+  {
+    if (action.type == UserAction::Type::EXIT)
+    {
       LOG_INFO("Exit requested");
       m_ExitRequested = true;
-    } else if (action.type == UserAction::Type::SET_MOVE_TARGET) {
-      WorldPos wp = action.Argument.position;
+    }
+    else if (action.type == UserAction::Type::SET_MOVE_TARGET)
+    {
+      WorldPos wp = m_Camera.WindowToWorld(action.Argument.position);
       LOG_INFO("Calculating path to target: ", 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) {
+    }
+    else if (action.type == UserAction::Type::SELECT_PATHFINDER)
+    {
       using namespace pathfinder;
       PathFinderType type = static_cast<PathFinderType>(action.Argument.number);
       m_PathFinder = pathfinder::utils::create(type, (const Map*)&m_Map);
       LOG_INFO("Switched to path finding method: ", m_PathFinder->GetName());
     }
+    else if (action.type == UserAction::Type::CAMERA_PAN)
+    {
+      const auto& window_pan = action.Argument.position;
+      WorldPos world_pan{window_pan.x(), window_pan.y()};
+      m_Camera.Pan(world_pan);
+      LOG_INFO("Camera pan delta: ", world_pan);
+    }
+    else if (action.type == UserAction::Type::CAMERA_ZOOM)
+    {
+      m_Camera.Zoom(action.Argument.float_number);
+      LOG_INFO("Camera zoom: ", action.Argument.float_number);
+    }
   };
 }
diff --git a/cpp/src/pathfindingdemo.hpp b/cpp/src/pathfindingdemo.hpp
index 20532b3..9743c86 100644
--- a/cpp/src/pathfindingdemo.hpp
+++ b/cpp/src/pathfindingdemo.hpp
@@ -10,6 +10,7 @@
 #include "map.hpp"
 #include "user_input.hpp"
 #include "pathfinder/base.hpp"
+#include "camera.hpp"
 
 class PathFindingDemo {
 public:
@@ -24,6 +25,7 @@ public:
   std::shared_ptr<Player> GetPlayer() { return m_Player; }
   std::vector<std::shared_ptr<Entity>>& GetEntities() { return m_Entities; }
   const Map& GetMap() const { return m_Map; }
+  const Camera& GetCamera() const { return m_Camera; }
   const pathfinder::Path& GetPath() const { return m_Path; }
   bool IsExitRequested() const { return m_ExitRequested; }
 
@@ -37,6 +39,7 @@ public:
 private:
   bool m_ExitRequested = false;
   Map m_Map;
+  Camera m_Camera;
   std::vector<std::shared_ptr<Entity>> m_Entities;
   std::shared_ptr<Player> m_Player;
   pathfinder::Path m_Path;
diff --git a/cpp/src/sprite.cpp b/cpp/src/sprite.cpp
index 41b40ca..be5164f 100644
--- a/cpp/src/sprite.cpp
+++ b/cpp/src/sprite.cpp
@@ -12,13 +12,13 @@
 
 Sprite::Sprite() : m_Texture(nullptr, SDL_DestroyTexture) {}
 
-Sprite::Sprite(std::string path, Vec2D<float> center) : Sprite() {
+Sprite::Sprite(std::string path, WorldPos center) : Sprite() {
   LoadImage(path, center);
 }
 
 Sprite::~Sprite() { LOG_DEBUG("."); }
 
-void Sprite::LoadImage(std::string path, Vec2D<float> image_center) {
+void Sprite::LoadImage(std::string path, WorldPos image_center) {
   LOG_INFO("Loading image ", path);
   assert(m_Renderer != nullptr);
 
diff --git a/cpp/src/sprite.hpp b/cpp/src/sprite.hpp
index fd86167..051d732 100644
--- a/cpp/src/sprite.hpp
+++ b/cpp/src/sprite.hpp
@@ -14,7 +14,7 @@ class Sprite {
 public:
   Sprite();
   ~Sprite();
-  explicit Sprite(std::string path, WorldPos center = {0, 0});
+  explicit Sprite(std::string path, WorldPos center = WorldPos{});
 
   Sprite(const Sprite &) = delete;
   Sprite &operator=(const Sprite &) = delete;
@@ -25,15 +25,15 @@ public:
 
   // GetTexture cannot return pointer to const, as SDL_RenderTexture modifies it
   SDL_Texture *GetTexture() { return m_Texture.get(); }
-  WorldPos GetSize() const { return m_Size; }
+  WorldSize GetSize() const { return m_Size; }
   WorldPos GetCenter() const { return m_ImageCenter; }
 
-  void LoadImage(std::string path, WorldPos image_center = {0.0, 0.0});
+  void LoadImage(std::string path, WorldPos image_center = WorldPos{});
 
 private:
   static std::shared_ptr<SDL_Renderer> m_Renderer;
   std::unique_ptr<SDL_Texture, decltype(&SDL_DestroyTexture)> m_Texture;
-  WorldPos m_Size;
+  WorldSize m_Size;
   WorldPos m_ImageCenter;
   float m_TextureWidth = 0;
   float m_TextureHeight = 0;
diff --git a/cpp/src/user_input.cpp b/cpp/src/user_input.cpp
index 46a6cdb..1604f94 100644
--- a/cpp/src/user_input.cpp
+++ b/cpp/src/user_input.cpp
@@ -3,6 +3,7 @@
 #include <map>
 #include <string>
 #include <vector>
+#include <unordered_set>
 
 #include "user_input.hpp"
 
@@ -19,18 +20,56 @@ UserInput::~UserInput() { LOG_DEBUG("."); };
 
 std::expected<void, std::string> UserInput::Init() { return {}; }
 
-const std::vector<UserAction> &UserInput::GetActions() {
-  m_Actions.clear();
-  static WorldPos move_direction = {0.0f, 0.0f};
-  SDL_Event event;
+void UserInput::GetActions_mouse(const SDL_Event& event)
+{
+  static bool mouse_pan = false;
 
-  while (SDL_PollEvent(&event)) {
-    if (event.type == SDL_EVENT_KEY_DOWN || event.type == SDL_EVENT_KEY_UP) {
+  SDL_MouseButtonEvent mouse_event = event.button;
+  MouseButton button = static_cast<MouseButton>(mouse_event.button);
+
+  if (event.type == SDL_EVENT_MOUSE_BUTTON_DOWN)
+  {
+    if (button == MouseButton::LEFT)
+    {
+      LOG_DEBUG("Mouse down: ", mouse_event.x, ", ", mouse_event.y);
+      m_Actions.emplace_back(UserAction::Type::SET_MOVE_TARGET,
+                             WindowPos{mouse_event.x, mouse_event.y});
+    }
+    else if (button == MouseButton::MIDDLE)
+    {
+      mouse_pan = true;
+    }
+  }
+  else if (event.type == SDL_EVENT_MOUSE_BUTTON_UP)
+  {
+    if (button == MouseButton::MIDDLE)
+    {
+      mouse_pan = false;
+    }
+  }
+  else if (event.type == SDL_EVENT_MOUSE_MOTION)
+  {
+    SDL_MouseMotionEvent motion_event = event.motion;
+    if (mouse_pan)
+    {
+      m_Actions.emplace_back(UserAction::Type::CAMERA_PAN,
+                             WindowPos{motion_event.xrel, motion_event.yrel});  
+    }
+  }
+  else if(event.type == SDL_EVENT_MOUSE_WHEEL)
+  {
+    SDL_MouseWheelEvent mouse_wheel = event.wheel;
+    m_Actions.emplace_back(UserAction::Type::CAMERA_ZOOM, mouse_wheel.y);
+  }
+}
+
+void UserInput::GetActions_keyboard(const SDL_Event& event)
+{
       bool key_down = event.type == SDL_EVENT_KEY_DOWN ? true : false;
       SDL_KeyboardEvent kbd_event = event.key;
       if (kbd_event.repeat) {
         // SDL repeats KEY_DOWN if key is held down, we ignore that
-        continue;
+        return;
       }
       LOG_DEBUG("Key '", static_cast<char>(kbd_event.key),
                 key_down ? "' down" : "' up");
@@ -38,8 +77,7 @@ const std::vector<UserAction> &UserInput::GetActions() {
       switch (kbd_event.key) {
       case 'q':
         m_Actions.emplace_back(UserAction::Type::EXIT);
-        // further processing of inputs is not needed
-        return m_Actions;
+        return;
       case '1':
       case '2':
       case '3':
@@ -54,12 +92,39 @@ const std::vector<UserAction> &UserInput::GetActions() {
         LOG_INFO("Key '", static_cast<char>(kbd_event.key), "' not mapped");
         break;
       }
-    } else if (event.type == SDL_EVENT_MOUSE_BUTTON_DOWN) {
-      SDL_MouseButtonEvent mouse_event = event.button;
-      LOG_DEBUG("Mouse down: ", mouse_event.x, ", ", mouse_event.y);
-      m_Actions.emplace_back(UserAction::Type::SET_MOVE_TARGET,
-                             WorldPos{mouse_event.x, mouse_event.y});
-    } else {
+}
+
+const std::vector<UserAction>& UserInput::GetActions() {
+
+  static std::unordered_set<uint32_t> mouse_events = {
+    SDL_EVENT_MOUSE_MOTION,
+    SDL_EVENT_MOUSE_BUTTON_DOWN,
+    SDL_EVENT_MOUSE_BUTTON_UP,
+    SDL_EVENT_MOUSE_WHEEL,
+    SDL_EVENT_MOUSE_ADDED,
+    SDL_EVENT_MOUSE_REMOVED,
+  };
+  
+  static std::unordered_set<uint32_t> keyboard_events = {
+    SDL_EVENT_KEY_DOWN,
+    SDL_EVENT_KEY_UP,
+  };
+    
+  SDL_Event event;
+  m_Actions.clear();
+
+  while (SDL_PollEvent(&event))
+  {
+    if (keyboard_events.contains(event.type))
+    {
+      GetActions_keyboard(event);
+    }
+    else if (mouse_events.contains(event.type))
+    {
+      GetActions_mouse(event);       
+    }
+    else
+    {
       // TODO uncomment, for now too much noise
       // LOG_WARNING("Action not processed");
     }
diff --git a/cpp/src/user_input.hpp b/cpp/src/user_input.hpp
index 2ea98b5..c32b071 100644
--- a/cpp/src/user_input.hpp
+++ b/cpp/src/user_input.hpp
@@ -7,24 +7,32 @@
 #include "log.hpp"
 #include "math.hpp"
 
+// Seems like SDL doesn't have named constants for mouse button 
+enum class MouseButton { LEFT = 1, MIDDLE, RIGHT };
+
 class UserAction {
 public:
-  enum class Type { NONE, EXIT, SET_MOVE_TARGET, SELECT_PATHFINDER };
+  enum class Type { NONE, EXIT, SET_MOVE_TARGET, SELECT_PATHFINDER, CAMERA_PAN, CAMERA_ZOOM };
 
-  UserAction() = default;
-  UserAction(Type t) : type(t) {}
+  UserAction() : type(Type::NONE), Argument{.number = 0} {}
+  UserAction(Type t) : type(t), Argument{.number = 0} {}
   UserAction(Type t, char key) : type(t), Argument{.key = key} {}
-  UserAction(Type t, WorldPos v) : type(t), Argument{.position = v} {}
-  UserAction(Type t, int arg) : type(t), Argument{.number = arg} {}
+  UserAction(Type t, WindowPos v) : type(t), Argument{.position = v} {}
+  UserAction(Type t, int32_t arg) : type(t), Argument{.number = arg} {}
+  UserAction(Type t, float arg) : type(t), Argument{.float_number = arg} {}
   ~UserAction() = default;
 
   Type type;
 
   union {
-    WorldPos position;
+    WindowPos position;
     char key;
-    int number;
+    int32_t number;
+    float float_number;
   } Argument;
+
+  // TODO use std::variant
+  //std::variant<WindowPos, char, int> Argument;
 };
 
 class UserInput {
@@ -43,4 +51,7 @@ public:
 
 private:
   std::vector<UserAction> m_Actions;
+
+  void GetActions_keyboard(const SDL_Event&);
+  void GetActions_mouse(const SDL_Event&);
 };
diff --git a/cpp/src/window.cpp b/cpp/src/window.cpp
index a488258..91b9ab6 100644
--- a/cpp/src/window.cpp
+++ b/cpp/src/window.cpp
@@ -76,17 +76,17 @@ Window::~Window() {
   LOG_DEBUG(".");
 }
 
-void Window::DrawSprite(const WorldPos &position, Sprite &s) {
-  WorldPos size = s.GetSize();
-  WorldPos img_center = s.GetCenter();
-  SDL_FRect rect = {position.x - img_center.x, position.y - img_center.y,
-                    size.x, size.y};
+void Window::DrawSprite(const WindowPos &position, Sprite &s, float scale) {
+  WorldSize size = s.GetSize() * scale;
+  WorldPos img_center = s.GetCenter() * scale;
+  SDL_FRect rect = {position.x() - img_center.x(), position.y() - img_center.y(),
+                    size.x(), size.y()};
   SDL_RenderTexture(m_Renderer.get(), s.GetTexture(), nullptr, &rect);
 }
 
-void Window::DrawRect(const WorldPos &position, const WorldPos size, uint8_t R,
+void Window::DrawRect(const WindowPos &position, const WindowSize size, uint8_t R,
                       uint8_t G, uint8_t B, uint8_t A) {
-  SDL_FRect rect = {position.x, position.y, size.x, size.y};
+  SDL_FRect rect = {position.x(), position.y(), size.x(), size.y()};
   SDL_SetRenderDrawColor(m_Renderer.get(), R, G, B, A);
   SDL_RenderFillRect(m_Renderer.get(), &rect);
 }
@@ -98,9 +98,9 @@ void Window::ClearWindow() {
 
 void Window::Flush() { SDL_RenderPresent(m_Renderer.get()); }
 
-void Window::DrawCircle(const WorldPos &position, float radius) {
-  int cx = static_cast<int>(position.x);
-  int cy = static_cast<int>(position.y);
+void Window::DrawCircle(const WindowPos &position, float radius) {
+  int cx = static_cast<int>(position.x());
+  int cy = static_cast<int>(position.y());
   SDL_SetRenderDrawColor(m_Renderer.get(), 255, 0, 0, 255);
   for (int i = 0; i < 360; ++i) {
     double a = i * M_PI / 180.0;
@@ -110,9 +110,9 @@ void Window::DrawCircle(const WorldPos &position, float radius) {
   }
 }
 
-void Window::DrawLine(const WorldPos &A, const WorldPos &B)
+void Window::DrawLine(const WindowPos &A, const WindowPos &B)
 {
   SDL_SetRenderDrawColor(m_Renderer.get(), 255, 0, 0, 255);
-  SDL_RenderLine(m_Renderer.get(), A.x, A.y, B.x, B.y);
+  SDL_RenderLine(m_Renderer.get(), A.x(), A.y(), B.x(), B.y());
 }
 
diff --git a/cpp/src/window.hpp b/cpp/src/window.hpp
index b582b53..8453d67 100644
--- a/cpp/src/window.hpp
+++ b/cpp/src/window.hpp
@@ -22,19 +22,19 @@ public:
   Window &operator=(Window &&) = delete;
 
   std::expected<void, std::string> Init();
-  void DrawSprite(const WorldPos &position, Sprite &s);
-  void DrawRect(const WorldPos &position, const WorldPos size, uint8_t R,
+  void DrawSprite(const WindowPos &position, Sprite &s, float scale = 1.0f);
+  void DrawRect(const WindowPos &position, const WindowSize size, uint8_t R,
                 uint8_t G, uint8_t B, uint8_t A);
   void ClearWindow();
   void Flush();
-  void DrawCircle(const WorldPos &position, float radius);
-  void DrawLine(const WorldPos &A, const WorldPos &B);
-
-  std::shared_ptr<SDL_Renderer> m_Renderer = nullptr;
-  SDL_Window *m_Window;
-  SDL_GLContext m_Context;
+  void DrawCircle(const WindowPos &position, float radius);
+  void DrawLine(const WindowPos &A, const WindowPos &B);
 
 private:
   uint32_t m_Width;
   uint32_t m_Height;
+  std::shared_ptr<SDL_Renderer> m_Renderer = nullptr;
+  SDL_Window *m_Window;
+  SDL_GLContext m_Context;
+
 };
diff --git a/cpp/test/test.cpp b/cpp/test/test.cpp
index 403ed83..8b9262b 100644
--- a/cpp/test/test.cpp
+++ b/cpp/test/test.cpp
@@ -5,151 +5,683 @@
 #include <sstream>
 #include <unordered_set>
 
-#include "array.hpp"
 #include "log.hpp"
 #include "math.hpp"
 
-// Vec2D Tests
-TEST(Vec2D, DefaultConstruction) {
-  Vec2D<int> v;
-  // Default values are uninitialized, but we can test basic functionality
-  v.x = 0;
-  v.y = 0;
-  ASSERT_EQ(v.x, 0);
-  ASSERT_EQ(v.y, 0);
+
+TEST(vec, DefaultConstruction) {
+  // Test that default-constucted vector
+  // has all elements equal to zero
+  vec3 v1;
+  ASSERT_EQ(v1[0], 0.0);
+  ASSERT_EQ(v1[1], 0.0);
+  ASSERT_EQ(v1[2], 0.0);
 }
 
-TEST(Vec2D, InitializerListConstruction) {
-  Vec2D<int> v{3, 4};
-  ASSERT_EQ(v.x, 3);
-  ASSERT_EQ(v.y, 4);
-
-  Vec2D<float> vf{1.5f, 2.5f};
-  ASSERT_FLOAT_EQ(vf.x, 1.5f);
-  ASSERT_FLOAT_EQ(vf.y, 2.5f);
-
-  // Test type conversion
-  Vec2D<float> vd{1, 2}; // int to float
-  ASSERT_FLOAT_EQ(vd.x, 1.0f);
-  ASSERT_FLOAT_EQ(vd.y, 2.0f);
+TEST(vec, GetElements) {
+  // Test operator[]
+  ivec3 v1{12, 34, 56};
+  ASSERT_EQ(v1[0], 12);
+  ASSERT_EQ(v1[1], 34);
+  ASSERT_EQ(v1[2], 56);
 }
 
-TEST(Vec2D, Addition) {
-  Vec2D<int> a{1, 2};
-  Vec2D<int> b{3, 4};
 
-  Vec2D<int> c = a + b;
-  ASSERT_EQ(c.x, 4);
-  ASSERT_EQ(c.y, 6);
+TEST(vec, equalEpsilon) {
+  // Test equalEpsilon
+  // TODO just an ad-hoc test,
+  // can possibly fail for other machines.
+  // This needs some work
+  vec3 v1{1.0f, 2.0f, 3.0f};
+  vec3 v2{0.999999f, 1.9999999f, 2.9999999f};
+  ASSERT_EQ(v1, v2);
+}
 
+TEST(vec, equalInt) {
+  ivec2 v1{1,2};
+  ivec2 v2{1,2};
+  ASSERT_EQ(v1, v2);
+}
+
+TEST(vec, nonEqualEpsilon) {
+  // Test operator!=
+  vec3 v1{1.0f, 2.0f, 3.0f};
+  vec3 v2{2.0f, 4.0f, 6.0f};
+  ASSERT_NE(v1, v2);
+}
+
+TEST(vec, LogPrint) {
+  // Test that logger can print the vector of different types
+  // and sizes
+  vec2  v2(1.2f, 3.4f);
+  vec3  v3(1.2f, 3.4f, 5.6f);
+  vec4  v4(1.2f, 3.4f, 5.6f, 7.8f);
+  dvec2 dv2(1.2, 3.4);
+  dvec3 dv3(1.2, 3.4, 5.6);
+  dvec4 dv4(1.2, 3.4, 5.6, 7.8);
+  ivec2 iv2(1, 3);
+  ivec3 iv3(1, 3, 5);
+  ivec4 iv4(1, 3, 5, 7);
+  uvec2 uv2(1u, 3u);
+  uvec3 uv3(1u, 3u, 5u);
+  uvec4 uv4(1u, 3u, 5u, 7u);
+
+  LOG_DEBUG("vec2  ", v2);
+  LOG_DEBUG("vec3  ", v3);
+  LOG_DEBUG("vec4  ", v4);
+  LOG_DEBUG("dvec2 ", dv2);
+  LOG_DEBUG("dvec3 ", dv3);
+  LOG_DEBUG("dvec4 ", dv4);
+  LOG_DEBUG("ivec2 ", iv2);
+  LOG_DEBUG("ivec3 ", iv3);
+  LOG_DEBUG("ivec4 ", iv4);
+  LOG_DEBUG("uvec2 ", uv2);
+  LOG_DEBUG("uvec3 ", uv3);
+  LOG_DEBUG("uvec4 ", uv4);
+}
+
+TEST(vec, Add)
+{
+  // Test operator+ with float vectors
+  vec3 v1{1.0f, 2.0f, 3.0f};
+  vec3 v2{4.0f, 5.0f, 6.0f};
+  vec3 result = v1 + v2;
+  
+  ASSERT_FLOAT_EQ(result[0], 5.0f);
+  ASSERT_FLOAT_EQ(result[1], 7.0f);
+  ASSERT_FLOAT_EQ(result[2], 9.0f);
+  
+  // Test operator+ with integer vectors
+  ivec3 iv1{1, 2, 3};
+  ivec3 iv2{10, 20, 30};
+  ivec3 iresult = iv1 + iv2;
+  
+  ASSERT_EQ(iresult[0], 11);
+  ASSERT_EQ(iresult[1], 22);
+  ASSERT_EQ(iresult[2], 33);
+  
   // Test that original vectors are unchanged
-  ASSERT_EQ(a.x, 1);
-  ASSERT_EQ(a.y, 2);
-  ASSERT_EQ(b.x, 3);
-  ASSERT_EQ(b.y, 4);
+  ASSERT_FLOAT_EQ(v1[0], 1.0f);
+  ASSERT_FLOAT_EQ(v1[1], 2.0f);
+  ASSERT_FLOAT_EQ(v1[2], 3.0f);
 }
 
-TEST(Vec2D, AdditionAssignment) {
-  Vec2D<int> a{1, 2};
-  Vec2D<int> b{3, 4};
-
-  a += b;
-  ASSERT_EQ(a.x, 4);
-  ASSERT_EQ(a.y, 6);
-
-  // Test that b is unchanged
-  ASSERT_EQ(b.x, 3);
-  ASSERT_EQ(b.y, 4);
+TEST(vec, Sub)
+{
+  // Test operator- with float vectors
+  vec3 v1{5.0f, 7.0f, 9.0f};
+  vec3 v2{1.0f, 2.0f, 3.0f};
+  vec3 result = v1 - v2;
+  
+  ASSERT_FLOAT_EQ(result[0], 4.0f);
+  ASSERT_FLOAT_EQ(result[1], 5.0f);
+  ASSERT_FLOAT_EQ(result[2], 6.0f);
+  
+  // Test operator- with integer vectors
+  ivec3 iv1{30, 20, 10};
+  ivec3 iv2{5, 3, 1};
+  ivec3 iresult = iv1 - iv2;
+  
+  ASSERT_EQ(iresult[0], 25);
+  ASSERT_EQ(iresult[1], 17);
+  ASSERT_EQ(iresult[2], 9);
+  
+  // Test that original vectors are unchanged
+  ASSERT_FLOAT_EQ(v1[0], 5.0f);
+  ASSERT_FLOAT_EQ(v1[1], 7.0f);
+  ASSERT_FLOAT_EQ(v1[2], 9.0f);
+  
+  // Test subtraction resulting in negative values
+  vec3 v3{1.0f, 2.0f, 3.0f};
+  vec3 v4{4.0f, 5.0f, 6.0f};
+  vec3 negative_result = v3 - v4;
+  
+  ASSERT_FLOAT_EQ(negative_result[0], -3.0f);
+  ASSERT_FLOAT_EQ(negative_result[1], -3.0f);
+  ASSERT_FLOAT_EQ(negative_result[2], -3.0f);
 }
 
-TEST(Vec2D, ScalarMultiplication) {
-  Vec2D<int> v{2, 3};
-
-  Vec2D<int> result = v * 2.0f;
-  ASSERT_EQ(result.x, 4);
-  ASSERT_EQ(result.y, 6);
-
-  // Test with float vector
-  Vec2D<float> vf{1.5f, 2.5f};
-  Vec2D<float> resultf = vf * 2.0f;
-  ASSERT_FLOAT_EQ(resultf.x, 3.0f);
-  ASSERT_FLOAT_EQ(resultf.y, 5.0f);
+TEST(vec, ScalarMultiplication)
+{
+  // Test scalar * vector with float vectors
+  vec3 v1{2.0f, 3.0f, 4.0f};
+  vec3 result = v1 * 2.5f;
+  
+  ASSERT_FLOAT_EQ(result[0], 5.0f);
+  ASSERT_FLOAT_EQ(result[1], 7.5f);
+  ASSERT_FLOAT_EQ(result[2], 10.0f);
+  
+  // Test scalar * vector with integer vectors
+  ivec3 iv1{3, 5, 7};
+  ivec3 iresult = iv1 * 2;
+  
+  ASSERT_EQ(iresult[0], 6);
+  ASSERT_EQ(iresult[1], 10);
+  ASSERT_EQ(iresult[2], 14);
+  
+  // Test that original vector is unchanged
+  ASSERT_FLOAT_EQ(v1[0], 2.0f);
+  ASSERT_FLOAT_EQ(v1[1], 3.0f);
+  ASSERT_FLOAT_EQ(v1[2], 4.0f);
+  
+  // Test multiplication by zero
+  vec3 v2{1.0f, 2.0f, 3.0f};
+  vec3 zero_result = v2 * 0.0f;
+  
+  ASSERT_FLOAT_EQ(zero_result[0], 0.0f);
+  ASSERT_FLOAT_EQ(zero_result[1], 0.0f);
+  ASSERT_FLOAT_EQ(zero_result[2], 0.0f);
+  
+  // Test multiplication by negative scalar (and different ordering)
+  vec3 v3{1.0f, -2.0f, 3.0f};
+  vec3 negative_result = -2.0f * v3;
+  
+  ASSERT_FLOAT_EQ(negative_result[0], -2.0f);
+  ASSERT_FLOAT_EQ(negative_result[1], 4.0f);
+  ASSERT_FLOAT_EQ(negative_result[2], -6.0f);
 }
 
-TEST(Vec2D, Normalization) {
-  Vec2D<float> v{3.0f, 4.0f}; // Length = 5
-
-  v.normalize();
-  ASSERT_FLOAT_EQ(v.x, 0.6f);
-  ASSERT_FLOAT_EQ(v.y, 0.8f);
-
-  // Check that length is approximately 1
-  float length = sqrt(v.x * v.x + v.y * v.y);
-  ASSERT_NEAR(length, 1.0f, 1e-6f);
+TEST(vec, ScalarDivision)
+{
+  // Test vector / scalar with float vectors
+  vec3 v1{10.0f, 15.0f, 20.0f};
+  vec3 result = v1 / 2.5f;
+  
+  ASSERT_FLOAT_EQ(result[0], 4.0f);
+  ASSERT_FLOAT_EQ(result[1], 6.0f);
+  ASSERT_FLOAT_EQ(result[2], 8.0f);
+  
+  // Test vector / scalar with integer vectors
+  ivec3 iv1{12, 18, 24};
+  ivec3 iresult = iv1 / 2;
+  
+  ASSERT_EQ(iresult[0], 6);
+  ASSERT_EQ(iresult[1], 9);
+  ASSERT_EQ(iresult[2], 12);
+  
+  // Test that original vector is unchanged
+  ASSERT_FLOAT_EQ(v1[0], 10.0f);
+  ASSERT_FLOAT_EQ(v1[1], 15.0f);
+  ASSERT_FLOAT_EQ(v1[2], 20.0f);
+  
+  // Test division by negative scalar
+  vec3 v2{6.0f, -9.0f, 12.0f};
+  vec3 negative_result = v2 / -3.0f;
+  
+  ASSERT_FLOAT_EQ(negative_result[0], -2.0f);
+  ASSERT_FLOAT_EQ(negative_result[1], 3.0f);
+  ASSERT_FLOAT_EQ(negative_result[2], -4.0f);
+  
+  // Test division by fractional scalar
+  vec3 v3{1.0f, 2.0f, 3.0f};
+  vec3 fractional_result = v3 / 0.5f;
+  
+  ASSERT_FLOAT_EQ(fractional_result[0], 2.0f);
+  ASSERT_FLOAT_EQ(fractional_result[1], 4.0f);
+  ASSERT_FLOAT_EQ(fractional_result[2], 6.0f);
 }
 
-TEST(Vec2D, NormalizedCopy) {
-  Vec2D<float> v{3.0f, 4.0f};
-  Vec2D<float> normalized = v.normalized();
+TEST(vec, AdditionAssignment)
+{
+  // Test operator+= with float vectors
+  vec3 v1{1.0f, 2.0f, 3.0f};
+  vec3 v2{4.0f, 5.0f, 6.0f};
+  v1 += v2;
+  
+  ASSERT_FLOAT_EQ(v1[0], 5.0f);
+  ASSERT_FLOAT_EQ(v1[1], 7.0f);
+  ASSERT_FLOAT_EQ(v1[2], 9.0f);
+  
+  // Test that v2 is unchanged
+  ASSERT_FLOAT_EQ(v2[0], 4.0f);
+  ASSERT_FLOAT_EQ(v2[1], 5.0f);
+  ASSERT_FLOAT_EQ(v2[2], 6.0f);
+  
+  // Test operator+= with integer vectors
+  ivec3 iv1{10, 20, 30};
+  ivec3 iv2{1, 2, 3};
+  iv1 += iv2;
+  
+  ASSERT_EQ(iv1[0], 11);
+  ASSERT_EQ(iv1[1], 22);
+  ASSERT_EQ(iv1[2], 33);
+  
+  // Test chaining
+  vec3 v3{1.0f, 1.0f, 1.0f};
+  vec3 v4{2.0f, 2.0f, 2.0f};
+  vec3 v5{3.0f, 3.0f, 3.0f};
+  v3 += v4 += v5;
+  
+  ASSERT_FLOAT_EQ(v3[0], 6.0f);
+  ASSERT_FLOAT_EQ(v3[1], 6.0f);
+  ASSERT_FLOAT_EQ(v3[2], 6.0f);
+  ASSERT_FLOAT_EQ(v4[0], 5.0f);
+  ASSERT_FLOAT_EQ(v4[1], 5.0f);
+  ASSERT_FLOAT_EQ(v4[2], 5.0f);
+}
 
-  // Original should be unchanged
-  ASSERT_FLOAT_EQ(v.x, 3.0f);
-  ASSERT_FLOAT_EQ(v.y, 4.0f);
+TEST(vec, SubtractionAssignment)
+{
+  // Test operator-= with float vectors
+  vec3 v1{10.0f, 15.0f, 20.0f};
+  vec3 v2{3.0f, 5.0f, 7.0f};
+  v1 -= v2;
+  
+  ASSERT_FLOAT_EQ(v1[0], 7.0f);
+  ASSERT_FLOAT_EQ(v1[1], 10.0f);
+  ASSERT_FLOAT_EQ(v1[2], 13.0f);
+  
+  // Test that v2 is unchanged
+  ASSERT_FLOAT_EQ(v2[0], 3.0f);
+  ASSERT_FLOAT_EQ(v2[1], 5.0f);
+  ASSERT_FLOAT_EQ(v2[2], 7.0f);
+  
+  // Test operator-= with integer vectors
+  ivec3 iv1{50, 40, 30};
+  ivec3 iv2{5, 10, 15};
+  iv1 -= iv2;
+  
+  ASSERT_EQ(iv1[0], 45);
+  ASSERT_EQ(iv1[1], 30);
+  ASSERT_EQ(iv1[2], 15);
+  
+  // Test subtraction resulting in negative values
+  vec3 v3{1.0f, 2.0f, 3.0f};
+  vec3 v4{4.0f, 5.0f, 6.0f};
+  v3 -= v4;
+  
+  ASSERT_FLOAT_EQ(v3[0], -3.0f);
+  ASSERT_FLOAT_EQ(v3[1], -3.0f);
+  ASSERT_FLOAT_EQ(v3[2], -3.0f);
+}
 
+TEST(vec, LengthSquared)
+{
+  // Test LengthSquared with float vectors
+  vec3 v1{3.0f, 4.0f, 0.0f};
+  ASSERT_FLOAT_EQ(v1.LengthSquared(), 25.0f); // 3² + 4² + 0² = 25
+  
+  vec2 v2{1.0f, 1.0f};
+  ASSERT_FLOAT_EQ(v2.LengthSquared(), 2.0f); // 1² + 1² = 2
+  
+  // Test with zero vector
+  vec3 zero{0.0f, 0.0f, 0.0f};
+  ASSERT_FLOAT_EQ(zero.LengthSquared(), 0.0f);
+}
+
+TEST(vec, Length)
+{
+  // Test Length with float vectors
+  vec3 v1{3.0f, 4.0f, 0.0f};
+  ASSERT_FLOAT_EQ(v1.Length(), 5.0f); // sqrt(3² + 4² + 0²) = 5
+  
+  vec2 v2{1.0f, 1.0f};
+  ASSERT_NEAR(v2.Length(), 1.414213f, 1e-5f); // sqrt(2) ≈ 1.414213
+  
+  // Test with zero vector
+  vec3 zero{0.0f, 0.0f, 0.0f};
+  ASSERT_FLOAT_EQ(zero.Length(), 0.0f);
+}
+
+TEST(vec, Normalize)
+{
+  // Test Normalize with float vectors
+  vec3 v1{3.0f, 4.0f, 0.0f};
+  v1.Normalize();
+  
+  ASSERT_FLOAT_EQ(v1[0], 0.6f);  // 3/5
+  ASSERT_FLOAT_EQ(v1[1], 0.8f);  // 4/5
+  ASSERT_FLOAT_EQ(v1[2], 0.0f);
+  ASSERT_NEAR(v1.Length(), 1.0f, 1e-6f);
+  
+  // Test with zero vector (may produce NaN - implementation dependent)
+  vec3 zero{0.0f, 0.0f, 0.0f};
+  zero.Normalize();
+  // Check if result is NaN (which is expected for zero vector normalization)
+  ASSERT_TRUE(zero[0] == 0.0f);
+  ASSERT_TRUE(zero[1] == 0.0f);
+  ASSERT_TRUE(zero[2] == 0.0f);
+}
+
+TEST(vec, GetNormalized)
+{
+  // Test GetNormalized with float vectors
+  const vec3 v1{3.0f, 4.0f, 0.0f};
+  vec3 normalized = v1.GetNormalized();
+  
+  // Original vector should be unchanged
+  ASSERT_FLOAT_EQ(v1[0], 3.0f);
+  ASSERT_FLOAT_EQ(v1[1], 4.0f);
+  ASSERT_FLOAT_EQ(v1[2], 0.0f);
+  
   // Normalized copy should be unit length
-  ASSERT_FLOAT_EQ(normalized.x, 0.6f);
-  ASSERT_FLOAT_EQ(normalized.y, 0.8f);
-
-  float length =
-      sqrt(normalized.x * normalized.x + normalized.y * normalized.y);
-  ASSERT_NEAR(length, 1.0f, 1e-6f);
+  ASSERT_FLOAT_EQ(normalized[0], 0.6f);  // 3/5
+  ASSERT_FLOAT_EQ(normalized[1], 0.8f);  // 4/5
+  ASSERT_FLOAT_EQ(normalized[2], 0.0f);
+  ASSERT_NEAR(normalized.Length(), 1.0f, 1e-6f);
+  
+  // Test with zero vector
+  vec3 zero{0.0f, 0.0f, 0.0f};
+  vec3 zero_normalized = zero.GetNormalized();
+  
+  ASSERT_FLOAT_EQ(zero_normalized[0], 0.0f);
+  ASSERT_FLOAT_EQ(zero_normalized[1], 0.0f);
+  ASSERT_FLOAT_EQ(zero_normalized[2], 0.0f);
+  
+  // Original zero vector should be unchanged
+  ASSERT_FLOAT_EQ(zero[0], 0.0f);
+  ASSERT_FLOAT_EQ(zero[1], 0.0f);
+  ASSERT_FLOAT_EQ(zero[2], 0.0f);
 }
 
-TEST(Vec2D, ZeroVectorNormalization) {
-  Vec2D<float> v{0.0f, 0.0f};
-
-  v.normalize();
-  ASSERT_FLOAT_EQ(v.x, 0.0f);
-  ASSERT_FLOAT_EQ(v.y, 0.0f);
-
-  // Test normalized() as well
-  Vec2D<float> v2{0.0f, 0.0f};
-  Vec2D<float> normalized = v2.normalized();
-  ASSERT_FLOAT_EQ(normalized.x, 0.0f);
-  ASSERT_FLOAT_EQ(normalized.y, 0.0f);
+TEST(vec, GetOrthogonal)
+{
+  const vec2 v1{5.0f, 1.0f};
+  auto v2 = v1.GetOrthogonal();
+  ASSERT_FLOAT_EQ(v2[0], -1.0f);
+  ASSERT_FLOAT_EQ(v2[1], 5.0f);
 }
 
-TEST(Vec2D, VerySmallVectorNormalization) {
-  Vec2D<float> v{1e-7f, 1e-7f}; // Very small vector
-
-  v.normalize();
-  // Should be treated as zero vector
-  ASSERT_FLOAT_EQ(v.x, 0.0f);
-  ASSERT_FLOAT_EQ(v.y, 0.0f);
+TEST(vec, DistanceTo)
+{
+  // Test DistanceTo with 3D vectors
+  vec3 v1{0.0f, 0.0f, 0.0f};
+  vec3 v2{3.0f, 4.0f, 0.0f};
+  
+  float distance = v1.DistanceTo(v2);
+  ASSERT_FLOAT_EQ(distance, 5.0f); // 3-4-5 triangle
+  
+  // Distance should be symmetric
+  ASSERT_FLOAT_EQ(v2.DistanceTo(v1), distance);
+  
+  // Test with 2D vectors
+  vec2 a{1.0f, 1.0f};
+  vec2 b{4.0f, 5.0f};
+  
+  float distance_2d = a.DistanceTo(b);
+  ASSERT_FLOAT_EQ(distance_2d, 5.0f); // sqrt((4-1)² + (5-1)²) = sqrt(9+16) = 5
+  
+  // Distance to self should be zero
+  ASSERT_FLOAT_EQ(v1.DistanceTo(v1), 0.0f);
+  ASSERT_FLOAT_EQ(a.DistanceTo(a), 0.0f);
+  
+  // Test that original vectors are unchanged
+  ASSERT_FLOAT_EQ(v1[0], 0.0f);
+  ASSERT_FLOAT_EQ(v1[1], 0.0f);
+  ASSERT_FLOAT_EQ(v1[2], 0.0f);
+  ASSERT_FLOAT_EQ(v2[0], 3.0f);
+  ASSERT_FLOAT_EQ(v2[1], 4.0f);
+  ASSERT_FLOAT_EQ(v2[2], 0.0f);
 }
 
-TEST(Vec2D, OutputOperator) {
-  Vec2D<int> v{42, 24};
-
-  std::ostringstream oss;
-  oss << v;
-  ASSERT_EQ(oss.str(), "( 42, 24)");
-}
-
-TEST(Vec2D, ChainedOperations) {
-  Vec2D<float> a{1.0f, 2.0f};
-  Vec2D<float> b{3.0f, 4.0f};
+TEST(vec, ChainedOperations) {
+  vec2 a{1.0f, 2.0f};
+  vec2 b{3.0f, 4.0f};
 
   // Test chaining: (a + b) * 2.0f
-  Vec2D<float> result = (a + b) * 2.0f;
-  ASSERT_FLOAT_EQ(result.x, 8.0f);
-  ASSERT_FLOAT_EQ(result.y, 12.0f);
+  auto result = (a + b) * 2.0f;
+  ASSERT_FLOAT_EQ(result[0], 8.0f);
+  ASSERT_FLOAT_EQ(result[1], 12.0f);
 
   // Test chaining with assignment
   a += b;
   a = a * 0.5f;
-  ASSERT_FLOAT_EQ(a.x, 2.0f);
-  ASSERT_FLOAT_EQ(a.y, 3.0f);
+  ASSERT_FLOAT_EQ(a[0], 2.0f);
+  ASSERT_FLOAT_EQ(a[1], 3.0f);
+}
+
+TEST(Matrix, DefaultConstruction) {
+  // Test that default-constructed matrix has all elements equal to zero
+  Matrix<float, 2> m1;
+  ASSERT_FLOAT_EQ(m1[0][0], 0.0f);
+  ASSERT_FLOAT_EQ(m1[0][1], 0.0f);
+  ASSERT_FLOAT_EQ(m1[1][0], 0.0f);
+  ASSERT_FLOAT_EQ(m1[1][1], 0.0f);
+}
+
+TEST(Matrix, ArrayConstruction) {
+  // Test construction from array (column major)
+  Matrix<float, 2> m1(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  
+  // Column 0: [1, 2]
+  ASSERT_FLOAT_EQ(m1[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(m1[0][1], 2.0f);
+  
+  // Column 1: [3, 4]
+  ASSERT_FLOAT_EQ(m1[1][0], 3.0f);
+  ASSERT_FLOAT_EQ(m1[1][1], 4.0f);
+  
+  // Test with 3x3 matrix
+  Matrix<int, 3> m2(std::array<int, 9>{1, 2, 3, 4, 5, 6, 7, 8, 9});
+  
+  // Column 0: [1, 2, 3]
+  ASSERT_EQ(m2[0][0], 1);
+  ASSERT_EQ(m2[0][1], 2);
+  ASSERT_EQ(m2[0][2], 3);
+  
+  // Column 1: [4, 5, 6]
+  ASSERT_EQ(m2[1][0], 4);
+  ASSERT_EQ(m2[1][1], 5);
+  ASSERT_EQ(m2[1][2], 6);
+  
+  // Column 2: [7, 8, 9]
+  ASSERT_EQ(m2[2][0], 7);
+  ASSERT_EQ(m2[2][1], 8);
+  ASSERT_EQ(m2[2][2], 9);
+}
+
+TEST(Matrix, ElementAccess) {
+  // Test element access (both const and non-const)
+  Matrix<float, 2> m1(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  
+  // Test const access
+  const Matrix<float, 2>& const_ref = m1;
+  ASSERT_FLOAT_EQ(const_ref[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(const_ref[1][1], 4.0f);
+  
+  // Test non-const access and modification
+  m1[0][0] = 10.0f;
+  m1[1][1] = 40.0f;
+  
+  ASSERT_FLOAT_EQ(m1[0][0], 10.0f);
+  ASSERT_FLOAT_EQ(m1[1][1], 40.0f);
+  
+  // Verify other elements unchanged
+  ASSERT_FLOAT_EQ(m1[0][1], 2.0f);
+  ASSERT_FLOAT_EQ(m1[1][0], 3.0f);
+}
+
+TEST(Matrix, Addition) {
+  // Test matrix addition
+  Matrix<float, 2> m1(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  Matrix<float, 2> m2(std::array<float, 4>{5.0f, 6.0f, 7.0f, 8.0f});
+  
+  Matrix<float, 2> result = m1 + m2;
+  
+  ASSERT_FLOAT_EQ(result[0][0], 6.0f);  // 1 + 5
+  ASSERT_FLOAT_EQ(result[0][1], 8.0f);  // 2 + 6
+  ASSERT_FLOAT_EQ(result[1][0], 10.0f); // 3 + 7
+  ASSERT_FLOAT_EQ(result[1][1], 12.0f); // 4 + 8
+  
+  // Test that original matrices are unchanged
+  ASSERT_FLOAT_EQ(m1[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(m1[1][1], 4.0f);
+  ASSERT_FLOAT_EQ(m2[0][0], 5.0f);
+  ASSERT_FLOAT_EQ(m2[1][1], 8.0f);
+  
+  // Test with integer matrices
+  Matrix<int, 2> im1(std::array<int, 4>{1, 2, 3, 4});
+  Matrix<int, 2> im2(std::array<int, 4>{10, 20, 30, 40});
+  Matrix<int, 2> iresult = im1 + im2;
+  
+  ASSERT_EQ(iresult[0][0], 11);
+  ASSERT_EQ(iresult[0][1], 22);
+  ASSERT_EQ(iresult[1][0], 33);
+  ASSERT_EQ(iresult[1][1], 44);
+}
+
+TEST(Matrix, Subtraction) {
+  // Test matrix subtraction
+  Matrix<float, 2> m1(std::array<float, 4>{10.0f, 8.0f, 6.0f, 4.0f});
+  Matrix<float, 2> m2(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  
+  Matrix<float, 2> result = m1 - m2;
+  
+  ASSERT_FLOAT_EQ(result[0][0], 9.0f);  // 10 - 1
+  ASSERT_FLOAT_EQ(result[0][1], 6.0f);  // 8 - 2
+  ASSERT_FLOAT_EQ(result[1][0], 3.0f);  // 6 - 3
+  ASSERT_FLOAT_EQ(result[1][1], 0.0f);  // 4 - 4
+  
+  // Test that original matrices are unchanged
+  ASSERT_FLOAT_EQ(m1[0][0], 10.0f);
+  ASSERT_FLOAT_EQ(m1[1][1], 4.0f);
+  ASSERT_FLOAT_EQ(m2[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(m2[1][1], 4.0f);
+  
+  // Test subtraction resulting in negative values
+  Matrix<float, 2> m3(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  Matrix<float, 2> m4(std::array<float, 4>{5.0f, 6.0f, 7.0f, 8.0f});
+  Matrix<float, 2> negative_result = m3 - m4;
+  
+  ASSERT_FLOAT_EQ(negative_result[0][0], -4.0f);
+  ASSERT_FLOAT_EQ(negative_result[0][1], -4.0f);
+  ASSERT_FLOAT_EQ(negative_result[1][0], -4.0f);
+  ASSERT_FLOAT_EQ(negative_result[1][1], -4.0f);
+}
+
+TEST(Matrix, MatrixMultiplication) {
+  // Test 2x2 matrix multiplication
+  Matrix<float, 2> m1(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  Matrix<float, 2> m2(std::array<float, 4>{5.0f, 6.0f, 7.0f, 8.0f});
+  
+  Matrix<float, 2> result = m2 * m1;
+  
+  ASSERT_FLOAT_EQ(result[0][0], 23.0f);
+  ASSERT_FLOAT_EQ(result[0][1], 34.0f);
+  ASSERT_FLOAT_EQ(result[1][0], 31.0f);
+  ASSERT_FLOAT_EQ(result[1][1], 46.0f);
+  
+  // Test identity property: I * m = m
+  Matrix<float, 2> identity = Matrix<float, 2>::Eye();
+  Matrix<float, 2> identity_result = identity * m1;
+  
+  ASSERT_FLOAT_EQ(identity_result[0][0], m1[0][0]);
+  ASSERT_FLOAT_EQ(identity_result[0][1], m1[0][1]);
+  ASSERT_FLOAT_EQ(identity_result[1][0], m1[1][0]);
+  ASSERT_FLOAT_EQ(identity_result[1][1], m1[1][1]);
+  
+  // Test with 3x3 matrices
+  Matrix<int, 3> im1(std::array<int, 9>{1, 0, 0, 0, 1, 0, 0, 0, 1}); // Identity
+  Matrix<int, 3> im2(std::array<int, 9>{1, 2, 3, 4, 5, 6, 7, 8, 9});
+  Matrix<int, 3> iresult = im1 * im2;
+  
+  // Identity * matrix = matrix
+  ASSERT_EQ(iresult[0][0], 1);
+  ASSERT_EQ(iresult[0][1], 2);
+  ASSERT_EQ(iresult[0][2], 3);
+  ASSERT_EQ(iresult[1][0], 4);
+  ASSERT_EQ(iresult[1][1], 5);
+  ASSERT_EQ(iresult[1][2], 6);
+  ASSERT_EQ(iresult[2][0], 7);
+  ASSERT_EQ(iresult[2][1], 8);
+  ASSERT_EQ(iresult[2][2], 9);
+}
+
+TEST(Matrix, MatrixVectorMultiplication) {
+  // Test matrix-vector multiplication
+  Matrix<float, 2> m1(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  vec<float, 2> v1(2.0f, 3.0f);
+  
+  vec<float, 2> result = m1 * v1;
+  
+  ASSERT_FLOAT_EQ(result[0], 8.0f);
+  ASSERT_FLOAT_EQ(result[1], 18.0f);
+  
+  // Test with 3x3 matrix and 3D vector
+  Matrix<int, 3> im1(std::array<int, 9>{1, 0, 0, 0, 1, 0, 0, 0, 1}); // Identity
+  vec<int, 3> iv1(5, 10, 15);
+  
+  vec<int, 3> iresult = im1 * iv1;
+  
+  // Identity * vector = vector
+  ASSERT_EQ(iresult[0], 5);
+  ASSERT_EQ(iresult[1], 10);
+  ASSERT_EQ(iresult[2], 15);
+  
+  // Test that original matrix and vector are unchanged
+  ASSERT_FLOAT_EQ(m1[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(v1[0], 2.0f);
+  ASSERT_FLOAT_EQ(v1[1], 3.0f);
+}
+
+TEST(Matrix, EyeIdentityMatrix) {
+  // Test 2x2 identity matrix
+  Matrix<float, 2> eye2 = Matrix<float, 2>::Eye();
+  
+  ASSERT_FLOAT_EQ(eye2[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(eye2[0][1], 0.0f);
+  ASSERT_FLOAT_EQ(eye2[1][0], 0.0f);
+  ASSERT_FLOAT_EQ(eye2[1][1], 1.0f);
+  
+  // Test 3x3 identity matrix
+  Matrix<int, 3> eye3 = Matrix<int, 3>::Eye();
+  
+  for (size_t i = 0; i < 3; ++i) {
+    for (size_t j = 0; j < 3; ++j) {
+      if (i == j) {
+        ASSERT_EQ(eye3[i][j], 1);
+      } else {
+        ASSERT_EQ(eye3[i][j], 0);
+      }
+    }
+  }
+  
+  // Test 4x4 identity matrix
+  Matrix<double, 4> eye4 = Matrix<double, 4>::Eye();
+  
+  for (size_t i = 0; i < 4; ++i) {
+    for (size_t j = 0; j < 4; ++j) {
+      if (i == j) {
+        ASSERT_DOUBLE_EQ(eye4[i][j], 1.0);
+      } else {
+        ASSERT_DOUBLE_EQ(eye4[i][j], 0.0);
+      }
+    }
+  }
+}
+
+TEST(Matrix, LogPrint) {
+  // Test that logger can print matrices of different types and sizes
+  Matrix<float, 2> m2(std::array<float, 4>{1.1f, 2.2f, 3.3f, 4.4f});
+  Matrix<int, 3> m3(std::array<int, 9>{1, 2, 3, 4, 5, 6, 7, 8, 9});
+  Matrix<double, 2> dm2(std::array<double, 4>{1.5, 2.5, 3.5, 4.5});
+  
+  LOG_DEBUG("Matrix<float, 2>  ", m2);
+  LOG_DEBUG("Matrix<int, 3>    ", m3);
+  LOG_DEBUG("Matrix<double, 2> ", dm2);
+}
+
+TEST(Matrix, ChainedOperations) {
+  // Test chaining matrix operations
+  Matrix<float, 2> m1(std::array<float, 4>{1.0f, 2.0f, 3.0f, 4.0f});
+  Matrix<float, 2> m2(std::array<float, 4>{1.0f, 1.0f, 1.0f, 1.0f});
+  Matrix<float, 2> m3(std::array<float, 4>{2.0f, 0.0f, 0.0f, 2.0f});
+  
+  // Test (m1 + m2) * m3
+  Matrix<float, 2> result = (m1 + m2) * m3;
+  
+  // m1 + m2 = [2 4]    m3 = [2 0]    result = [4  8]
+  //           [3 5]          [0 2]             [6 10]
+  
+  ASSERT_FLOAT_EQ(result[0][0], 4.0f);
+  ASSERT_FLOAT_EQ(result[0][1], 6.0f);
+  ASSERT_FLOAT_EQ(result[1][0], 8.0f);
+  ASSERT_FLOAT_EQ(result[1][1], 10.0f);
+  
+  // Test that original matrices are unchanged
+  ASSERT_FLOAT_EQ(m1[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(m2[0][0], 1.0f);
+  ASSERT_FLOAT_EQ(m3[0][0], 2.0f);
 }
 
 int main(int argc, char **argv) {
diff --git a/vs/pathfinding_demo/pathfinding_demo.vcxproj b/vs/pathfinding_demo/pathfinding_demo.vcxproj
index c7828c9..2908d8d 100644
--- a/vs/pathfinding_demo/pathfinding_demo.vcxproj
+++ b/vs/pathfinding_demo/pathfinding_demo.vcxproj
@@ -19,12 +19,44 @@
     </ProjectConfiguration>
   </ItemGroup>
   <ItemGroup>
+    <ClCompile Include="..\..\cpp\src\camera.cpp" />
+    <ClCompile Include="..\..\cpp\src\entities.cpp" />
+    <ClCompile Include="..\..\cpp\src\gameloop.cpp" />
     <ClCompile Include="..\..\cpp\src\main.cpp" />
+    <ClCompile Include="..\..\cpp\src\map.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\astar.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\base.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\bfs.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\dijkstra.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\gbfs.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\linear.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfinder\utils.cpp" />
+    <ClCompile Include="..\..\cpp\src\pathfindingdemo.cpp" />
+    <ClCompile Include="..\..\cpp\src\sprite.cpp" />
+    <ClCompile Include="..\..\cpp\src\tile.cpp" />
+    <ClCompile Include="..\..\cpp\src\user_input.cpp" />
+    <ClCompile Include="..\..\cpp\src\window.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="..\..\cpp\src\array.hpp" />
+    <ClInclude Include="..\..\cpp\src\camera.hpp" />
+    <ClInclude Include="..\..\cpp\src\entities.hpp" />
+    <ClInclude Include="..\..\cpp\src\gameloop.hpp" />
     <ClInclude Include="..\..\cpp\src\log.hpp" />
+    <ClInclude Include="..\..\cpp\src\map.hpp" />
     <ClInclude Include="..\..\cpp\src\math.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\astar.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\base.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\bfs.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\dijkstra.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\gbfs.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\linear.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfinder\utils.hpp" />
+    <ClInclude Include="..\..\cpp\src\pathfindingdemo.hpp" />
+    <ClInclude Include="..\..\cpp\src\sprite.hpp" />
+    <ClInclude Include="..\..\cpp\src\tile.hpp" />
+    <ClInclude Include="..\..\cpp\src\user_input.hpp" />
+    <ClInclude Include="..\..\cpp\src\window.hpp" />
   </ItemGroup>
   <ItemGroup>
     <ProjectReference Include="..\glew\build\vc15\glew_static.vcxproj">
@@ -96,7 +128,7 @@
       <PreprocessorDefinitions>GLEW_STATIC</PreprocessorDefinitions>
       <ConformanceMode>true</ConformanceMode>
       <LanguageStandard>stdcpplatest</LanguageStandard>
-      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;..\..\cpp\src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>
@@ -114,7 +146,7 @@
       <PreprocessorDefinitions>GLEW_STATIC</PreprocessorDefinitions>
       <ConformanceMode>true</ConformanceMode>
       <LanguageStandard>stdcpplatest</LanguageStandard>
-      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;..\..\cpp\src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>
@@ -132,7 +164,7 @@
       <PreprocessorDefinitions>GLEW_STATIC</PreprocessorDefinitions>
       <ConformanceMode>true</ConformanceMode>
       <LanguageStandard>stdcpplatest</LanguageStandard>
-      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;..\..\cpp\src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>
@@ -150,7 +182,7 @@
       <PreprocessorDefinitions>GLEW_STATIC</PreprocessorDefinitions>
       <ConformanceMode>true</ConformanceMode>
       <LanguageStandard>stdcpplatest</LanguageStandard>
-      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+      <AdditionalIncludeDirectories>..\glew\include;..\SDL\include;..\SDL_image\include;..\;..\..\cpp\src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>
diff --git a/vs/pathfinding_demo/pathfinding_demo.vcxproj.filters b/vs/pathfinding_demo/pathfinding_demo.vcxproj.filters
index fca6151..5b5e6b6 100644
--- a/vs/pathfinding_demo/pathfinding_demo.vcxproj.filters
+++ b/vs/pathfinding_demo/pathfinding_demo.vcxproj.filters
@@ -18,6 +18,54 @@
     <ClCompile Include="..\..\cpp\src\main.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="..\..\cpp\src\window.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\user_input.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\tile.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\sprite.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfindingdemo.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\map.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\gameloop.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\entities.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\camera.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\utils.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\linear.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\gbfs.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\dijkstra.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\bfs.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\base.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="..\..\cpp\src\pathfinder\astar.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="..\..\cpp\src\array.hpp">
@@ -29,5 +77,53 @@
     <ClInclude Include="..\..\cpp\src\math.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="..\..\cpp\src\camera.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\entities.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\gameloop.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\map.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfindingdemo.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\sprite.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\tile.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\user_input.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\window.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\astar.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\base.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\bfs.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\dijkstra.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\gbfs.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\linear.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="..\..\cpp\src\pathfinder\utils.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file