From a17450dd8197d5924c756be15864c3d3dbaf4d05 Mon Sep 17 00:00:00 2001
From: Klemen Plestenjak <plestenjak.klemen@gmail.com>
Date: Thu, 23 Nov 2023 19:23:06 +0100
Subject: [PATCH] Fix path smoothing bug

---
 game/pathfinding.c | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/game/pathfinding.c b/game/pathfinding.c
index 6149ac0..937ca1f 100644
--- a/game/pathfinding.c
+++ b/game/pathfinding.c
@@ -19,8 +19,8 @@ static i32 dst(TilePosition a, TilePosition b) {
 static PathData *pushPathWaypoint(PathData *pathData, Position waypoint, BzObjectPool *pathPool) {
     if (pathData->numWaypoints + 1 > PATH_DATA_SIZE) {
         PathData *newPathData = bzObjectPool(pathPool);
-        bzMemSet(newPathData, 0, sizeof(*newPathData));
         BZ_ASSERT(newPathData);
+        bzMemSet(newPathData, 0, sizeof(*newPathData));
         newPathData->numWaypoints = 0;
         newPathData->next = pathData;
         pathData = newPathData;
@@ -68,10 +68,14 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) {
     size_t prevIdx = 1;
     PathData *nextPath = pathData;
     size_t nextIdx = 2;
-    Position lastPos;
+    Position lastPos = {INFINITY, INFINITY};
     // Needed, because we overwrite numWaypoints
     size_t currPathLen = prevPath->numWaypoints;
     size_t nextPathLen = nextPath->numWaypoints;
+    if (prevPath->next) {
+        currPathLen = PATH_DATA_SIZE;
+        nextPathLen = PATH_DATA_SIZE;
+    }
     outPath->numWaypoints = 1;
     while (nextPath && nextIdx < nextPathLen) {
         Position currPos = prevPath->waypoints[prevIdx];
@@ -80,6 +84,7 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) {
 
         prevIdx++;
         nextIdx++;
+        bzLogInfo("%llu,%llu", prevIdx, nextIdx);
 
         if (prevIdx >= currPathLen) {
             prevPath = prevPath->next;
@@ -103,6 +108,7 @@ static void smoothPath(BzTileMap *map, PathData *pathData, BzObjectPool *pool) {
 
         }
     }
+    BZ_ASSERT(lastPos.x != INFINITY && lastPos.y != INFINITY);
     outPath->waypoints[outIdx++] = lastPos;
     outPath->numWaypoints = outIdx;
 
@@ -197,6 +203,7 @@ bool findPath(const PathfindingDesc *desc) {
         out->curWaypoint = 0;
         BZ_ASSERT(desc->pool);
         PathData *pathData = bzObjectPool(desc->pool);
+        BZ_ASSERT(pathData);
         bzMemSet(pathData, 0, sizeof(*pathData));
 
         pathData = pushPathWaypoint(pathData, desc->target, desc->pool);