fix: add costs to pathfinding

docs/artificialintelligence/06-pathfinding/README.md

@@ -11,6 +11,116 @@ In order to build an A-star pathfinding algorithm, we need to define some data s
 - Priority Queue to store the frontier of visitable buckets;
 - Vector of Indexes to store the path;
 
+## Index and Position
+
+In order to A-star to work in a continuous space, we should quantize the space position into indexes.
+
+```c++
+// generic vector2 struct to work with floats and ints
+template <typename T>
+// requires T to be int32_t or float_t
+requires std::is_same<T, int32_t>::value || std::is_same<T, float_t>::value // C++20
+struct Vector2 {
+    // data
+    T x, y;
+    // constructors
+    Vector2() : x(0), y(0) {}
+    Vector2(T x, T y) : x(x), y(y) {}
+    // copy constructor
+    Vector2(const Vector2& v) : x(v.x), y(v.y) {}
+    // assignment operator
+    Vector2& operator=(const Vector2& v) {
+        x = v.x;
+        y = v.y;
+        return *this;
+    }
+    // operators
+    Vector2 operator+(const Vector2& v) const {
+        return Vector2(x + v.x, y + v.y);
+    }
+    Vector2 operator-(const Vector2& v) const {
+        return Vector2(x - v.x, y - v.y);
+    }
+    // distance
+    float distance(const Vector2& v) const {
+        return sqrt((x - v.x) * (x - v.x) + (y - v.y) * (y - v.y));
+    }
+    // distance squared
+    float distanceSquared(const Vector2& v) const {
+        return (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y);
+    }
+    // quantize to index2
+    Vector2<int32_t> quantized(float scale=1) const {
+        return {(int32_t)std::round(x / scale), (int32_t)std::round(y / scale)};
+    }
+    // operator < for std::map
+    bool operator<(const Vector2& v) const {
+        return x < v.x || (x == v.x && y < v.y);
+    }
+    // operator == for std::map
+    bool operator==(const Vector2& v) const {
+        return x == v.x && y == v.y;
+    }
+};
+```
+
+- The operators `<` and `==` are required to use the Vector2 as a key in a std::map.
+- The `quantized` method is used to convert a position into an index.
+- The `distance` and `distanceSquared` methods are used to calculate the distance between two positions. Is used on A-star to calculate the cost to reach a neighbor or the distance to the goal.
+
+```c++
+using Index2 = Vector2<int32_t>;
+using Position2 = Vector2<float_t>;
+```
+
+I am going to use `Index2` to store the quantized index in the grid and `Position2` to store the continuous position.
+
+```c++
+// hash function for std::unordered_map
+template <>
+struct std::hash<Index2> {
+    size_t operator()(const Index2 &v) const {
+        return (((size_t)v.x) << 32) ^ (size_t)v.y;
+    }
+};
+```
+
+This hash function is for the `std::unordered_map` and `std::unordered_set` to work with `Index2`.
+
+## Bucket
+
+In order to have an easy way to query if a game object is in a bucket, we need to use an `std::unordered_set` of pointers to the game objects. In order to index them, we will use an `std::unordered_map` from `Index2` to `std::unordered_set`.
+
+```c++
+std::unordered_map<Index2, std::unordered_set<GameObject*>> quantizedMap;
+```
+
+## Costs
+
+Your scenario might have different costs to reach a bucket. You can use an `std::unordered_map` to store the cost of each bucket.
+
+```c++
+std::unordered_map<Index2, float> costMap;
+```
+
+## Walls
+
+You might want to avoid some buckets. You can use an `std::unordered_map` to store the walls.
+
+```c++
+std::unordered_map<Index2, bool> isWall;
+```
+
+## Priority Queue
+
+In order to store the frontier of visitable buckets, we need to use a `std::priority_queue` of pairs of `float` and `Index2`.
+
+```c++
+std::priority_queue<std::pair<float, Index2>> frontier;
+```
+
+## Implementation
+
 ```c++
 /**
 In order to build an A-star pathfinding algorithm, we need to define some data structures. We need:
@@ -29,10 +139,21 @@ In order to build an A-star pathfinding algorithm, we need to define some data s
 #include <vector>
 #include <queue>
 
+using std::pair;
+
+template<typename K, typename V>
+using umap = std::unordered_map<K, V>;
+
+template<typename T>
+using uset = std::unordered_set<T>;
+
+template<typename T>
+using pqueue = std::priority_queue<T>;
+
 // generic vector2 struct to work with floats and ints
 template <typename T>
 // requires T to be int32_t or float_t
-requires std::is_same<T, int32_t>::value || std::is_same<T, float_t>::value
+requires std::is_same<T, int32_t>::value || std::is_same<T, float_t>::value // C++20
 struct Vector2 {
     // data
     T x, y;
@@ -60,7 +181,7 @@ struct Vector2 {
     }
     // distance squared
     float distanceSquared(const Vector2& v) const {
-        return (x - v.x) * (x - v.x) + (y - v.y) * (y - v.y);
+        return (float)(x - v.x) * (x - v.x) + (float)(y - v.y) * (y - v.y);
     }
     // quantize to index2
     Vector2<int32_t> quantized(float scale=1) const {
@@ -79,19 +200,8 @@ struct Vector2 {
 using Index2 = Vector2<int32_t>;
 using Position2 = Vector2<float_t>;
 
-struct uid_type {
-private:
-    static inline size_t nextId = 0; // to be used as a counter
-    size_t uid; // to be used as a unique identifier
-public:
-    // not thread safe, but it is not a problem for this example
-    uid_type(): uid(nextId++) {}
-    inline size_t getUid() const { return uid; }
-};
-
-
 // implement this struct to store game objects by yourself
-struct GameObject: public uid_type {
+struct GameObject {
     Position2 position;
     // add here your other data
 
@@ -100,39 +210,21 @@ struct GameObject: public uid_type {
 };
 
 // hash function for std::unordered_map
-template <typename T>
-struct std::hash<Vector2<T>> {
-    size_t operator()(const Vector2<T> &v) const {
-        return hash<T>()(v.x) ^ hash<T>()(v.y);
+template <>
+struct std::hash<Index2> {
+    size_t operator()(const Index2 &v) const {
+        return (((size_t)v.x) << 32) | (size_t)v.y;
     }
 };
 
-using Bucket = std::unordered_set<GameObject*>;
-using QuantizedMap = std::unordered_map<Index2, Bucket>;
-using CostMap = std::unordered_map<Index2, float>;
-using BlockMap = std::unordered_map<Index2, bool>;
-using FlowField = std::unordered_map<Index2, Index2>;
-
 // The game objects organized into buckets
-QuantizedMap quantizedMap;
+umap<Index2, uset<GameObject*>> quantizedMap;
 // all game objects
-Bucket gameObjects;
+uset<GameObject*> gameObjects;
 // The cost of each bucket
-CostMap costMap;
+umap<Index2, float> costMap;
 // The walls
-BlockMap isWall;
-
-std::vector<Index2> getUnblockedNeighbors(const Index2& v) {
-    std::vector<Index2> neighbors;
-    auto candidates = {Index2(1, 0), Index2(-1, 0), Index2(0, 1), Index2(0, -1)};
-    for (const auto& c : candidates) {
-        Index2 n = v + c;
-        if (!isWall.contains(n)) {
-            neighbors.push_back(n);
-        }
-    }
-    return neighbors;
-}
+umap<Index2, bool> isWall;
 
 // Pathfinding algorithm from position A to position B
 std::vector<Index2> findPath(const Position2& startPos, const Position2& endPos) {
@@ -141,55 +233,48 @@ std::vector<Index2> findPath(const Position2& startPos, const Position2& endPos)
     Index2 end = endPos.quantized();
 
     // datastructures
-    BlockMap visited; // to store if a bucket has been visited
-    CostMap accumulatedCosts; // to store the cost to reach a bucket
-    std::priority_queue<std::pair<float, Index2>> frontier;
-    BlockMap isOnFrontier; // to store if a bucket is on the frontier
-    FlowField cameFrom; // to easily reconstruct the path
+    pqueue<pair<float, Index2>> frontier; // to store the frontier of visitable buckets
+    umap<Index2, float> accumulatedCosts; // to store the cost to reach a bucket
 
     // initialize
-    visited[start] = false;
     accumulatedCosts[start] = 0;
     frontier.emplace(0, start);
-    isOnFrontier[start] = true;
-    cameFrom[start] = start;
 
     // main loop
     while (!frontier.empty()) {
         // consume first element from the frontier
         auto current = frontier.top().second;
         frontier.pop();
-        isOnFrontier.erase(current);
-
-        // mark it as visited to avoid revisiting
-        visited[current] = true;
 
         // quit early
         if (current == end)
             break;
 
         // iterate over neighbors
-        for (const auto& next : getUnblockedNeighbors(current)) {
+        auto candidates = {
+                current + Index2(1, 0),
+                current + Index2(-1, 0),
+                current + Index2(0, 1),
+                current + Index2(0, -1)
+        };
+        for (const auto& next : candidates) {
+            // skip walls
+            if(isWall.contains(current))
+                continue;
             // if the neighbor has not been visited and is not on frontier
-            if (!visited[next] && !isOnFrontier.contains(next)) {
-                // calculate the cost to reach the neighbor
-                float newCost =
-                        accumulatedCosts[current] + // cost so far
-                        current.distance(next) + // cost to reach the neighbor
-                        (costMap.contains(next) ? costMap[next] : 0); // cost of the neighbor
-                // if the cost is lower than the previous cost
-                if (!accumulatedCosts.contains(next) || newCost < accumulatedCosts[next]) {
-                    // update the cost
-                    accumulatedCosts[next] = newCost;
-                    // calculate the priority
-                    float priority = newCost + next.distance(end);
-                    // push the neighbor to the frontier
-                    frontier.emplace(-priority, next);
-                    // mark the neighbor as on frontier
-                    isOnFrontier[next] = true;
-                    // store the flow field
-                    cameFrom[next] = current;
-                }
+            // calculate the cost to reach the neighbor
+            float newCost =
+                    accumulatedCosts[current] + // cost so far
+                    current.distance(next) + // cost to reach the neighbor
+                    (costMap.contains(next) ? costMap[next] : 0); // cost of the neighbor
+            // if the cost is lower than the previous cost
+            if (!accumulatedCosts.contains(next) || newCost < accumulatedCosts[next]) {
+                // update the cost
+                accumulatedCosts[next] = newCost;
+                // calculate the priority
+                float priority = newCost + next.distance(end);
+                // push the neighbor to the frontier
+                frontier.emplace(-priority, next);
             }
         }
     }
@@ -199,22 +284,32 @@ std::vector<Index2> findPath(const Position2& startPos, const Position2& endPos)
     Index2 current = end;
     while (current != start) {
         path.push_back(current);
-        current = cameFrom[current];
+        auto candidates = {
+                current + Index2(1, 0),
+                current + Index2(-1, 0),
+                current + Index2(0, 1),
+                current + Index2(0, -1)
+        };
+        for (const auto& next : candidates) {
+            if (accumulatedCosts.contains(next) && accumulatedCosts[next] < accumulatedCosts[current]) {
+                current = next;
+                break;
+            }
+        }
     }
     path.push_back(start);
     std::reverse(path.begin(), path.end());
     return path;
 }
 
-
-
 int main() {
 /*
 map. numbers are bucket cost, letters are objects, x is wall
-A 0 5 0 0
-0 X X 0 0
-5 X 0 0 0
-0 0 0 0 B
+A 0 5 0 0 0
+0 X X 0 0 0
+5 X 0 0 5 0
+0 0 0 5 B 5
+0 0 0 0 5 0
  */
 
     // Create 2 Game Objects
@@ -227,8 +322,14 @@ A 0 5 0 0
     isWall[Index2(2, 1)] = true;
 
     // add cost to some buckets
+    // should avoid these:
     costMap[Index2(2, 0)] = 5;
     costMap[Index2(0, 2)] = 5;
+    // should pass-through these:
+    costMap[Index2(5, 4)] = 5;
+    costMap[Index2(3, 4)] = 5;
+    costMap[Index2(4, 3)] = 5;
+    costMap[Index2(4, 5)] = 5;
 
     // add game objects to the set
     gameObjects.insert(&a);
@@ -251,5 +352,4 @@ A 0 5 0 0
 
     return 0;
 }
-
-```
+```