Update and fixes for A* search

2 files changed, 94 insertions, 95 deletions

diff --git a/source/kit/astar.h b/source/kit/astar.h
index 4a8fb06..b76ef67 100644
--- a/source/kit/astar.h
+++ b/source/kit/astar.h
@@ -20,10 +20,6 @@
 #include <stddef.h>
 #include <string.h>
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 enum {
   ASTAR_PROGRESS = 0,
   ASTAR_SUCCESS,
@@ -37,28 +33,28 @@ typedef struct {
   b8  skip;
   i64 neighbor;
   i64 distance;
-} astar_link_t;
+} Astar_Link;
 
 typedef struct {
   i64 id;
   i64 previous;
-  i64 exact_source_to_node;
-  i64 estimated_source_to_destination;
-} astar_node_t;
+  i64 exact_distance;
+  i64 estimate;
+} Astar_Node;
 
 typedef struct {
-  i64           capacity;
-  i64           size;
-  astar_node_t *values;
-} astar_set_t;
+  i64         capacity;
+  i64         size;
+  Astar_Node *values;
+} Astar_Set;
 
 typedef struct {
-  astar_set_t open;
-  astar_set_t closed;
-  void       *user_data;
-  i64         source;
-  i64         destination;
-} astar_state_t;
+  Astar_Set open;
+  Astar_Set closed;
+  void *    user_data;
+  i64       source;
+  i64       destination;
+} Astar_State;
 
 #ifdef __GNUC__
 #  pragma GCC diagnostic push
@@ -68,11 +64,25 @@ typedef struct {
 #  pragma GCC            optimize("O3")
 #endif
 
-static s32 astar_init(astar_state_t *state, astar_set_t open,
-                      astar_set_t closed, void *user_data, i64 source,
-                      i64 destination) {
-  assert(state != NULL && open.capacity > 0 && open.values != NULL &&
-         closed.capacity > 0 && closed.values != NULL);
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+static s32 astar_init(
+  Astar_State *state,
+  Astar_Set    open,
+  Astar_Set    closed,
+  void *       user_data,
+  i64          source,
+  i64          destination
+) {
+  assert(
+    state != NULL         &&
+    open.capacity > 0     &&
+    open.values != NULL   &&
+    closed.capacity > 0   &&
+    closed.values != NULL
+  );
 
   if (state == NULL)
     return KIT_ERROR_INVALID_ARGUMENT;
@@ -88,11 +98,11 @@ static s32 astar_init(astar_state_t *state, astar_set_t open,
   if (state->open.capacity <= 0)
     return KIT_ERROR_INVALID_ARGUMENT;
 
-  state->open.values[0] = (astar_node_t) {
-    .id                              = source,
-    .previous                        = -1,
-    .exact_source_to_node            = 0,
-    .estimated_source_to_destination = -1,
+  state->open.values[0] = (Astar_Node) {
+    .id             = source,
+    .previous       = -1,
+    .exact_distance = 0,
+    .estimate       = 0x7fffffffffffffffll,
   };
 
   state->open.size   = 1;
@@ -101,7 +111,7 @@ static s32 astar_init(astar_state_t *state, astar_set_t open,
   return KIT_OK;
 }
 
-static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) {
+static s32 astar_path(Astar_State *state, i64 *size, i64 *path) {
   assert(state != NULL && size != NULL);
 
   if (state == NULL || size == NULL)
@@ -148,15 +158,15 @@ static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) {
   return KIT_OK;
 }
 
+#ifdef __cplusplus
+}
+#endif
+
 #ifdef __GNUC__
 #  pragma GCC            pop_options
 #  pragma GCC diagnostic pop
 #endif
 
-#ifdef __cplusplus
-}
-#endif
-
 #endif
 
 //  ================================================================
@@ -167,23 +177,20 @@ static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) {
 
 #ifdef ASTAR_TEMPLATE
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 #ifndef ASTAR_SUFFIX
 #  define ASTAR_SUFFIX
 #endif
 
 #ifndef ASTAR_NEIGHBOR
 #  define ASTAR_NEIGHBOR(user_data_, node_, index_) \
-    (astar_link_t) {                                \
+    (Astar_Link) {                                  \
       .stop = 1,                                    \
     }
 #endif
 
 #ifndef ASTAR_HEURISTIC
-#  define ASTAR_HEURISTIC(user_data_, node_0_, node_2_) (-1)
+#  define ASTAR_HEURISTIC(user_data_, node_0_, node_2_) \
+    0x7fffffffffffffffll
 #endif
 
 #ifdef __GNUC__
@@ -198,7 +205,11 @@ extern "C" {
 #define CAT2_(x, y) CAT1_(x, y)
 #define NAME_(x) CAT2_(x, ASTAR_SUFFIX)
 
-static s32 NAME_(astar_iteration)(astar_state_t *state) {
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+static s32 NAME_(astar_iteration)(Astar_State *state) {
   assert(state != NULL);
 
   if (state == NULL)
@@ -212,18 +223,15 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
 
   //  Find a node in the open set that is closest to the destination
   //
-  astar_node_t nearest_node;
+  Astar_Node nearest_node;
   {
     i64 index_in_open = 0;
     for (i64 i = 1; i < state->open.size; i++)
-      if (state->open.values[i].estimated_source_to_destination <
-          state->open.values[index_in_open]
-              .estimated_source_to_destination)
+      if (state->open.values[i].estimate < state->open.values[index_in_open].estimate)
         index_in_open = i;
     nearest_node = state->open.values[index_in_open];
     if (index_in_open != state->open.size - 1)
-      state->open.values[index_in_open] =
-          state->open.values[state->open.size - 1];
+      state->open.values[index_in_open] = state->open.values[state->open.size - 1];
     --state->open.size;
   }
 
@@ -232,7 +240,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
   if (nearest_node.id == state->destination) {
     if (state->closed.size + 1 > state->closed.capacity) {
       //  Restore the state
-      state->open.values[state->open.size - 1] = nearest_node;
+      state->open.values[state->open.size] = nearest_node;
       ++state->open.size;
 
       return ASTAR_OUT_OF_MEMORY;
@@ -253,10 +261,8 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
     //  Get a link to the next neighbor node
     //
     (void) state->user_data;
-    (void) nearest_node.id;
     (void) k;
-    astar_link_t link = ASTAR_NEIGHBOR(state->user_data,
-                                       nearest_node.id, k);
+    Astar_Link link = ASTAR_NEIGHBOR(state->user_data, nearest_node.id, k);
 
     //  If there is no more neighbors, break the loop
     if (link.stop)
@@ -265,32 +271,24 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
     if (link.skip)
       continue;
 
-    astar_node_t neighbor_node = {
-      .id                   = link.neighbor,
-      .previous             = nearest_node.id,
-      .exact_source_to_node = nearest_node.exact_source_to_node +
-                              link.distance,
-      .estimated_source_to_destination = -1,
-    };
-
     //  Calculate distance estimations
     //
     (void) state->user_data;
-    (void) neighbor_node.id;
     (void) state->destination;
-    i64 estimated_node_to_destination = ASTAR_HEURISTIC(
-        state->user_data, neighbor_node.id, state->destination);
-
-    neighbor_node.estimated_source_to_destination =
-        neighbor_node.exact_source_to_node +
-        estimated_node_to_destination;
+    
+    Astar_Node neighbor_node = {
+      .id             = link.neighbor,
+      .previous       = nearest_node.id,
+      .exact_distance = nearest_node.exact_distance + link.distance,
+      .estimate       = ASTAR_HEURISTIC(state->user_data, link.neighbor, state->destination),
+    };
 
     //  Check if we reached the destination
     //
     if (neighbor_node.id == state->destination) {
       if (state->closed.size + 2 > state->closed.capacity) {
         //  Restore the state
-        state->open.values[state->open.size - 1] = nearest_node;
+        state->open.values[state->open.size] = nearest_node;
         ++state->open.size;
 
         return ASTAR_OUT_OF_MEMORY;
@@ -320,13 +318,14 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
           break;
         }
       if (index_in_closed != -1) {
-        if (state->closed.values[index_in_closed]
-                .exact_source_to_node <
-            neighbor_node.exact_source_to_node)
-          //  Skip this node
-          continue;
-        //  Replace the node
-        state->closed.values[index_in_closed] = neighbor_node;
+        //  Check if this node has a better distance
+        //
+        if (neighbor_node.exact_distance < state->closed.values[index_in_closed].exact_distance)
+          //  Replace the node
+          state->closed.values[index_in_closed] = neighbor_node;
+
+        //  Skip this node
+        continue;
       }
     }
 
@@ -340,20 +339,20 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
           break;
         }
       if (index_in_open != -1) {
-        //  Check if this node has a better estimate
+        //  Check if this node has a better distance
         //
-        if (neighbor_node.estimated_source_to_destination <
-            state->open.values[index_in_open]
-                .estimated_source_to_destination)
+        if (neighbor_node.exact_distance < state->open.values[index_in_open].exact_distance)
           //  Replace the node
           state->open.values[index_in_open] = neighbor_node;
+        
+        //  Skip this node
         continue;
       }
     }
 
     if (state->open.size + 1 > state->open.capacity) {
       //  Restore the state
-      state->open.values[state->open.size - 1] = nearest_node;
+      state->open.values[state->open.size] = nearest_node;
       ++state->open.size;
 
       return ASTAR_OUT_OF_MEMORY;
@@ -384,6 +383,10 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
   return ASTAR_PROGRESS;
 }
 
+#ifdef __cplusplus
+}
+#endif
+
 #undef NAME_
 #undef CAT1_
 #undef CAT2_
@@ -393,9 +396,5 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
 #  pragma GCC diagnostic pop
 #endif
 
-#ifdef __cplusplus
-}
-#endif
-
 #undef ASTAR_TEMPLATE
 #endif
diff --git a/source/tests/astar.test.c b/source/tests/astar.test.c
index 6be9418..f037d65 100644
--- a/source/tests/astar.test.c
+++ b/source/tests/astar.test.c
@@ -10,7 +10,7 @@ enum {
   HEIGHT = 5,
 };
 
-static astar_link_t neighbor(i8 *grid, i64 a, i64 n) {
+static Astar_Link neighbor(i8 *grid, i64 a, i64 n) {
   i64 x = a % WIDTH;
   i64 y = a / WIDTH;
 
@@ -23,7 +23,7 @@ static astar_link_t neighbor(i8 *grid, i64 a, i64 n) {
   else if (n == 3)
     ++y;
   else
-    return (astar_link_t) {
+    return (Astar_Link) {
       .stop = 1,
     };
 
@@ -31,12 +31,12 @@ static astar_link_t neighbor(i8 *grid, i64 a, i64 n) {
 
   if (x < 0 || x >= WIDTH || y < 0 || y >= HEIGHT || m < 0 ||
       m >= WIDTH * HEIGHT || grid[m] == 1)
-    return (astar_link_t) {
+    return (Astar_Link) {
       .stop = 0,
       .skip = 1,
     };
 
-  return (astar_link_t) {
+  return (Astar_Link) {
     .stop     = 0,
     .skip     = 0,
     .neighbor = m,
@@ -68,9 +68,9 @@ static i64 xy(i64 x, i64 y) {
 }
 
 TEST("astar path exists") {
-  astar_node_t buf[200];
+  Astar_Node buf[200];
 
-  astar_set_t sets[2] = {
+  Astar_Set sets[2] = {
     { .capacity = 100, .size = 0, .values = buf },
     { .capacity = 100, .size = 0, .values = buf + 100 },
   };
@@ -83,7 +83,7 @@ TEST("astar path exists") {
     1, 1, 0, 0, 0, 0, 0, 0, //
   };
 
-  astar_state_t state;
+  Astar_State state;
   REQUIRE_EQ(
       astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)),
       KIT_OK);
@@ -117,9 +117,9 @@ TEST("astar path exists") {
 }
 
 TEST("astar no path") {
-  astar_node_t buf[200];
+  Astar_Node buf[200];
 
-  astar_set_t sets[2] = {
+  Astar_Set sets[2] = {
     { .capacity = 100, .size = 0, .values = buf },
     { .capacity = 100, .size = 0, .values = buf + 100 },
   };
@@ -132,7 +132,7 @@ TEST("astar no path") {
     1, 1, 0, 0, 0, 0, 0, 0, //
   };
 
-  astar_state_t state;
+  Astar_State state;
   REQUIRE_EQ(
       astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)),
       KIT_OK);
@@ -149,9 +149,9 @@ TEST("astar no path") {
 }
 
 TEST("astar empty path") {
-  astar_node_t buf[200];
+  Astar_Node buf[200];
 
-  astar_set_t sets[2] = {
+  Astar_Set sets[2] = {
     { .capacity = 100, .size = 0, .values = buf },
     { .capacity = 100, .size = 0, .values = buf + 100 },
   };
@@ -164,7 +164,7 @@ TEST("astar empty path") {
     1, 1, 0, 0, 0, 0, 0, 0, //
   };
 
-  astar_state_t state;
+  Astar_State state;
   REQUIRE_EQ(
       astar_init(&state, sets[0], sets[1], grid, xy(4, 3), xy(4, 3)),
       KIT_OK);