From e7e4d1e4ce3aef8e4c8236ccb0f8fb0d0373b876 Mon Sep 17 00:00:00 2001
From: Klemen Plestenjak <plestenjak.klemen@gmail.com>
Date: Thu, 7 Dec 2023 11:35:35 +0100
Subject: [PATCH] Fix bug with follow path system

---
 game/main.c           |  4 ++--
 game/pathfinding.c    | 11 +++++++++++
 game/systems.h        |  4 ++--
 game/systems_entity.c | 12 +++++++-----
 4 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/game/main.c b/game/main.c
index fb0dfc3..721b776 100644
--- a/game/main.c
+++ b/game/main.c
@@ -135,8 +135,8 @@ bool init(void *userData) {
         .userDataSize=sizeof(ecs_entity_t)
     });
 
-    ECS_OBSERVER(ECS, entitySpatialRemoved, EcsOnRemove, Position, SpatialGridID);
-    ECS_OBSERVER(ECS, entityPathRemoved, EcsOnRemove, Path);
+    ECS_OBSERVER(ECS, entitySpatialRemove, EcsOnRemove, Position, SpatialGridID);
+    ECS_OBSERVER(ECS, entityPathRemove, EcsOnRemove, Path);
 
     ECS_OBSERVER(ECS, entitySetAnimationState, EcsOnSet, Animation, AnimationType);
 
diff --git a/game/pathfinding.c b/game/pathfinding.c
index 61d8e9c..4dd7824 100644
--- a/game/pathfinding.c
+++ b/game/pathfinding.c
@@ -132,6 +132,17 @@ bool pathfindAStar(const PathfindingDesc *desc) {
     BZ_ASSERT(start.x >= 0 && start.x < map->width);
     BZ_ASSERT(start.y >= 0 && start.y < map->height);
 
+    // Perform very cheap ray cast check
+    if (bzTileMapCanRayCastLine(map, desc->start, desc->target)) {
+        PathData *pathData = bzObjectPool(desc->pool);
+        BZ_ASSERT(pathData);
+        pathData->waypoints[0] = desc->target;
+        pathData->numWaypoints = 1;
+        pathData->next         = NULL;
+        *desc->outPath = (Path) {pathData, 0};
+        return true;
+    }
+
     i32 numTiles = map->width * map->height;
 
     PathNodeRecord *closedSet = bzStackAlloc(desc->alloc, sizeof(*closedSet) * numTiles);
diff --git a/game/systems.h b/game/systems.h
index 2fe9c9b..ea6e457 100644
--- a/game/systems.h
+++ b/game/systems.h
@@ -15,13 +15,13 @@
  * 1: Position
  * 3: SpatialGridID
  */
-void entitySpatialRemoved(ecs_iter_t *it);
+void entitySpatialRemove(ecs_iter_t *it);
 
 /* Observer (for releasing path objects)
  * 0: Game (singleton) for object pool
  * 1: Path
  */
-void entityPathRemoved(ecs_iter_t *it);
+void entityPathRemove(ecs_iter_t *it);
 
 /* Observer (for updating animation state)
  * 1: Animation
diff --git a/game/systems_entity.c b/game/systems_entity.c
index 818fcd5..d334414 100644
--- a/game/systems_entity.c
+++ b/game/systems_entity.c
@@ -9,7 +9,7 @@ static Position getBottomLeftPos(Position pos, Size size) {
     return (Position) {pos.x - size.x * 0.5f, pos.y - size.y * 0.5f};
 }
 
-void entitySpatialRemoved(ecs_iter_t *it) {
+void entitySpatialRemove(ecs_iter_t *it) {
     Game *game = ecs_singleton_get_mut(ECS, Game);
     Position *pos = ecs_field(it, Position, 1);
     SpatialGridID *spatialID = ecs_field(it, SpatialGridID , 2);
@@ -20,7 +20,7 @@ void entitySpatialRemoved(ecs_iter_t *it) {
 
 }
 
-void entityPathRemoved(ecs_iter_t *it) {
+void entityPathRemove(ecs_iter_t *it) {
     Game *game = ecs_singleton_get_mut(ECS, Game);
     BzObjectPool *pool = game->pools.pathData;
     Path *path = ecs_field(it, Path, 1);
@@ -154,11 +154,11 @@ void entityFollowPath(ecs_iter_t *it) {
 
     for (i32 i = 0; i < it->count; i++) {
         ecs_entity_t   entity = it->entities[i];
-        TargetPosition target = path[i].paths->waypoints[path[i].curWaypoint];
+
+        Path *entityPath = &path[i];
+        TargetPosition target = entityPath->paths->waypoints[entityPath->curWaypoint];
 
         if (!ecs_has(ECS, entity, TargetPosition)) {
-            ecs_set_ptr(ECS, entity, TargetPosition, &target);
-            path[i].curWaypoint++;
             if (path[i].curWaypoint >= path[i].paths->numWaypoints) {
                 path[i].curWaypoint = 0;
                 PathData *oldPath = path[i].paths;
@@ -166,6 +166,8 @@ void entityFollowPath(ecs_iter_t *it) {
                 path[i].paths = path[i].paths->next;
                 if (!path[i].paths) ecs_remove(ECS, it->entities[i], Path);
             }
+            ecs_set_ptr(ECS, entity, TargetPosition, &target);
+            path[i].curWaypoint++;
         }
     }
 }