Path following

game/utils/pathfinding.c

@@ -78,28 +78,46 @@ bool findPath(const PathfindingDesc *desc) {
         }
     }
 
-    DrawRectangle(desc->start.x * 16, desc->start.y * 16, 16, 16, BLUE);
-    Color color = RED;
-    if (foundPath) {
-        color = GREEN;
+    i32 pathLen = 0;
+    if (foundPath && desc->outPath) {
         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;
+            pathLen++;
+        }
+        Path *out = desc->outPath;
+        out->curWaypoint = 0;
+        pos = desc->target;
+        i32 len = pathLen;
+        // Skip positions
+        while (len >= out->maxWaypoints) {
             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++;
+            len--;
         }
-        bzLogInfo("Path length: %d", count);
+        // Write path
+        for (i32 i = 0; i < len; i++) {
+            out->waypoints[len - i - 1] = (Position){
+                pos.x * map->tileWidth + map->tileWidth * 0.5f,
+                pos.y * map->tileHeight + map->tileHeight * 0.5f
+            };
+            out->numWaypoints++;
+            Visited visit = visited[pos.y * map->width + pos.x];
+            pos.x -= visit.x;
+            pos.y -= visit.y;
+        }
+        BZ_ASSERT(len == out->maxWaypoints);
+        out->numWaypoints = len;
     }
-    DrawRectangle(desc->target.x * 16, desc->target.y * 16, 16, 16, color);
 
     if (!desc->heap) {
         bzHeapFree(heap);
         heap = NULL;
     }
 
-    return foundPath;
+    return foundPath ? pathLen : -1;
 }