Basic pathfinding

game/main.c

@@ -8,6 +8,7 @@
 #include "map_layers.h"
 #include "buildings.h"
 
+#include "utils/pathfinding.h"
 
 Game *GAME = NULL;
 
@@ -152,6 +153,22 @@ void render(float dt, Game *game) {
     bzTileMapDraw(&game->map);
     bzTileMapDrawColliders(&game->map);
 
+
+    Vector2 worldPos = GetScreenToWorld2D(GetMousePosition(), game->camera);
+    int tileX = (int) worldPos.x / 16;
+    int tileY = (int) worldPos.y / 16;
+
+
+    static PathNode *heap = NULL;
+    if (!heap)
+        heap = bzHeapNew(PathNode, game->map.width * game->map.height);
+    findPath(&(PathfindingDesc) {
+        .start=(TilePosition){57, 24},
+        .target=(TilePosition){tileX, tileY},
+        .map=&game->map,
+        .heap=heap,
+    });
+
     ecs_progress(ECS, dt);
 
     EndMode2D();

game/utils/pathfinding.c

@@ -0,0 +1,105 @@
+#include "pathfinding.h"
+
+static i32 dst(TilePosition a, TilePosition b) {
+    //i32 dX = a.x - b.x;
+    //i32 dY = a.y - b.y;
+    //return dX * dX + dY * dY;
+    int dstX = a.x - b.x;
+    int dstY = a.y - b.y;
+    if (dstX < 0) dstX = -dstX;
+    if (dstY < 0) dstY = -dstY;
+
+    if (dstX > dstY)
+        return 14 * dstY + 10 * (dstX - dstY);
+    return 14 * dstX + 10 * (dstY - dstX);
+}
+
+bool findPath(const PathfindingDesc *desc) {
+    BZ_ASSERT(desc->map);
+    BzTileMap *map = desc->map;
+    typedef struct Visited {
+        bool visited : 1;
+        i8 x : 3;
+        i8 y : 3;
+    } Visited;
+    Visited visited[map->width * map->height] = {};
+    bzMemSet(visited, 0, sizeof(visited));
+
+    PathNode *heap = desc->heap;
+    if (!heap) heap = bzHeapNew(PathNode, map->width * map->height);
+    else bzHeapReset(heap);
+
+    i32 toTargetCost = dst(desc->start, desc->target);
+    bzHeapPush(heap, (PathNode) {toTargetCost, 0, toTargetCost, desc->start});
+
+    bool foundPath = false;
+
+    while (!bzHeapIsEmpty(heap)) {
+        PathNode node = bzHeapPop(heap);
+        if (node.pos.x == desc->target.x &&
+            node.pos.y == desc->target.y) {
+            // Found path
+            foundPath = true;
+            break;
+        }
+
+        visited[node.pos.y * map->width + node.pos.x].visited = true;
+
+        for (int y = node.pos.y - 1; y <= node.pos.y + 1; y++) {
+            for (int x = node.pos.x - 1; x <= node.pos.x + 1; x++) {
+                if (x == node.pos.x && y == node.pos.y)
+                    continue;
+                if (y < 0 || y >= map->height ||
+                    x < 0 || x >= map->width)
+                    continue;
+                if (bzTileMapGetCollider(map, x, y).shapes[0].type != BZ_TILE_SHAPE_NONE)
+                    continue;
+                Visited *curVisited = &visited[y * map->width + x];
+                if (curVisited->visited)
+                    continue;
+                curVisited->visited = true;
+
+                TilePosition curPos = {x, y};
+                i32 gCost = node.gCost + dst(node.pos, curPos);
+                //if (gCost >= node.gCost) continue;
+                toTargetCost = dst(curPos, desc->target);
+
+                curVisited->x = (i8) (curPos.x - node.pos.x);
+                curVisited->y = (i8) (curPos.y - node.pos.y);
+
+                bzHeapPush(heap, (PathNode) {
+                    .weight = gCost + toTargetCost,
+                    .gCost = gCost,
+                    .hCost = toTargetCost,
+                    .pos = (TilePosition) {x, y}
+                });
+
+            }
+        }
+    }
+
+    DrawRectangle(desc->start.x * 16, desc->start.y * 16, 16, 16, BLUE);
+    Color color = RED;
+    if (foundPath) {
+        color = GREEN;
+        TilePosition pos = desc->target;
+        int count = 0;
+        while (pos.x != desc->start.x || pos.y != desc->start.y) {
+            Visited visit = visited[pos.y * map->width + pos.x];
+            BZ_ASSERT(visit.x != 0 && visit.y != 0);
+            pos.x -= visit.x;
+            pos.y -= visit.y;
+            DrawRectangle(pos.x * 16, pos.y * 16, 16, 16, GREEN);
+            count++;
+        }
+        bzLogInfo("Path length: %d", count);
+    }
+    DrawRectangle(desc->target.x * 16, desc->target.y * 16, 16, 16, color);
+
+    if (!desc->heap) {
+        bzHeapFree(heap);
+        heap = NULL;
+    }
+
+    return foundPath;
+}

game/utils/pathfinding.h

@@ -0,0 +1,29 @@
+#ifndef PIXELDEFENSE_PATHFINDING_H
+#define PIXELDEFENSE_PATHFINDING_H
+
+#include <breeze.h>
+
+#include "../components.h"
+
+typedef struct PathMove {
+    i8 x;
+    i8 y;
+} PathMove;
+
+typedef struct PathNode {
+    i32 weight; // g + h
+    i32 gCost; // from start cost
+    i32 hCost; // to target cost
+    TilePosition pos;
+} PathNode;
+
+typedef struct PathfindingDesc {
+    TilePosition start;
+    TilePosition target;
+    BzTileMap *map;
+    PathNode *heap;
+} PathfindingDesc;
+
+bool findPath(const PathfindingDesc *desc);
+
+#endif //PIXELDEFENSE_PATHFINDING_H