A* search: refactor, add tests

2 files changed, 141 insertions, 52 deletions

diff --git a/source/kit/astar.h b/source/kit/astar.h
index 554649a..f1758fc 100644
--- a/source/kit/astar.h
+++ b/source/kit/astar.h
@@ -26,6 +26,7 @@ enum {
   ASTAR_SUCCESS,
   ASTAR_FAIL,
   ASTAR_OUT_OF_MEMORY,
+  ASTAR_INVALID_ARGUMENT,
 };
 
 typedef struct {
@@ -49,11 +50,11 @@ typedef struct {
 } astar_set_t;
 
 typedef struct {
-  s32         status;
-  i64         source;
-  i64         destination;
   astar_set_t open;
   astar_set_t closed;
+  void       *user_data;
+  i64         source;
+  i64         destination;
 } astar_state_t;
 
 #ifdef __GNUC__
@@ -65,7 +66,7 @@ typedef struct {
 #endif
 
 static s32 astar_init(astar_state_t *state, astar_set_t open,
-                      astar_set_t closed, i64 source,
+                      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);
@@ -75,16 +76,14 @@ static s32 astar_init(astar_state_t *state, astar_set_t open,
 
   memset(state, 0, sizeof *state);
 
-  state->status      = ASTAR_PROGRESS;
   state->source      = source;
   state->destination = destination;
   state->open        = open;
   state->closed      = closed;
+  state->user_data   = user_data;
 
-  if (state->open.capacity <= 0) {
-    state->status = ASTAR_OUT_OF_MEMORY;
+  if (state->open.capacity <= 0)
     return KIT_ERROR_INVALID_ARGUMENT;
-  }
 
   state->open.values[0] = (astar_node_t) {
     .id                              = source,
@@ -168,14 +167,14 @@ extern "C" {
 #endif
 
 #ifndef ASTAR_NEIGHBOR
-#  define ASTAR_NEIGHBOR(a, n) \
-    (astar_link_t) {           \
-      .stop = 1,               \
+#  define ASTAR_NEIGHBOR(user_data_, node_, index_) \
+    (astar_link_t) {                                \
+      .stop = 1,                                    \
     }
 #endif
 
 #ifndef ASTAR_HEURISTIC
-#  define ASTAR_HEURISTIC(a, b) (-1)
+#  define ASTAR_HEURISTIC(user_data_, node_0_, node_2_) (-1)
 #endif
 
 #ifdef __GNUC__
@@ -194,17 +193,13 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
   assert(state != NULL);
 
   if (state == NULL)
-    return KIT_ERROR_INVALID_ARGUMENT;
+    return ASTAR_INVALID_ARGUMENT;
 
-  if (state->open.size == 0) {
-    state->status = ASTAR_FAIL;
-    return KIT_OK;
-  }
+  if (state->open.size == 0)
+    return ASTAR_FAIL;
 
-  if (state->open.values == NULL || state->closed.values == NULL) {
-    state->status = ASTAR_OUT_OF_MEMORY;
-    return KIT_ERROR_BAD_ALLOC;
-  }
+  if (state->open.values == NULL || state->closed.values == NULL)
+    return ASTAR_OUT_OF_MEMORY;
 
   //  Find a node in the open set that is closest to the destination
   //
@@ -223,14 +218,36 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
     --state->open.size;
   }
 
+  //  Check if we reached the destination
+  //
+  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.size;
+
+      return ASTAR_OUT_OF_MEMORY;
+    }
+
+    //  Add the nearest node to the closed set
+    //
+    state->closed.values[state->closed.size] = nearest_node;
+    ++state->closed.size;
+
+    //  Finish the search
+    return ASTAR_SUCCESS;
+  }
+
   //  Enumerate all neighbors
   //
   for (i64 k = 0;; ++k) {
     //  Get a link to the next neighbor node
     //
+    (void) state->user_data;
     (void) nearest_node.id;
     (void) k;
-    astar_link_t link = ASTAR_NEIGHBOR(nearest_node.id, k);
+    astar_link_t link = ASTAR_NEIGHBOR(state->user_data,
+                                       nearest_node.id, k);
 
     //  If there is no more neighbors, break the loop
     if (link.stop)
@@ -249,10 +266,11 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
 
     //  Calculate distance estimations
     //
+    (void) state->user_data;
     (void) neighbor_node.id;
     (void) state->destination;
     i64 estimated_node_to_destination = ASTAR_HEURISTIC(
-        neighbor_node.id, state->destination);
+        state->user_data, neighbor_node.id, state->destination);
 
     neighbor_node.estimated_source_to_destination =
         neighbor_node.exact_source_to_node +
@@ -266,8 +284,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
         state->open.values[state->open.size - 1] = nearest_node;
         ++state->open.size;
 
-        state->status = ASTAR_OUT_OF_MEMORY;
-        return KIT_ERROR_BAD_ALLOC;
+        return ASTAR_OUT_OF_MEMORY;
       }
 
       //  Add the nearest node to the closed set
@@ -281,8 +298,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
       ++state->closed.size;
 
       //  Finish the search
-      state->status = ASTAR_SUCCESS;
-      return KIT_OK;
+      return ASTAR_SUCCESS;
     }
 
     //  Check if this node is already in the closed set
@@ -331,8 +347,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
       state->open.values[state->open.size - 1] = nearest_node;
       ++state->open.size;
 
-      state->status = ASTAR_OUT_OF_MEMORY;
-      return KIT_ERROR_BAD_ALLOC;
+      return ASTAR_OUT_OF_MEMORY;
     }
 
     //  Add this node to the open set
@@ -348,8 +363,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
     state->open.values[state->open.size - 1] = nearest_node;
     ++state->open.size;
 
-    state->status = ASTAR_OUT_OF_MEMORY;
-    return KIT_ERROR_BAD_ALLOC;
+    return ASTAR_OUT_OF_MEMORY;
   }
 
   //  Add the nearest node to the closed set
@@ -358,8 +372,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) {
   ++state->closed.size;
 
   //  Continue the search
-  state->status = ASTAR_PROGRESS;
-  return KIT_OK;
+  return ASTAR_PROGRESS;
 }
 
 #undef NAME_
diff --git a/source/tests/astar.test.c b/source/tests/astar.test.c
index eeb1100..6be9418 100644
--- a/source/tests/astar.test.c
+++ b/source/tests/astar.test.c
@@ -10,15 +10,7 @@ enum {
   HEIGHT = 5,
 };
 
-i8 grid[WIDTH * HEIGHT] = {
-  0, 0, 0, 0, 0, 0, 0, 0, //
-  1, 1, 0, 1, 1, 0, 1, 0, //
-  1, 1, 0, 0, 1, 0, 1, 0, //
-  0, 0, 0, 1, 1, 1, 1, 0, //
-  1, 1, 0, 0, 0, 0, 0, 0, //
-};
-
-static astar_link_t neighbor(i64 a, i64 n) {
+static astar_link_t neighbor(i8 *grid, i64 a, i64 n) {
   i64 x = a % WIDTH;
   i64 y = a / WIDTH;
 
@@ -67,15 +59,15 @@ static i64 heuristic(i64 a, i64 b) {
 
 #define ASTAR_TEMPLATE
 #define ASTAR_SUFFIX _test
-#define ASTAR_NEIGHBOR(a, n) neighbor(a, n)
-#define ASTAR_HEURISTIC(a, b) heuristic(a, b)
+#define ASTAR_NEIGHBOR(data, a, n) neighbor((i8 *) data, a, n)
+#define ASTAR_HEURISTIC(data, a, b) heuristic(a, b)
 #include "../kit/astar.h"
 
 static i64 xy(i64 x, i64 y) {
   return y * WIDTH + x;
 }
 
-TEST("astar") {
+TEST("astar path exists") {
   astar_node_t buf[200];
 
   astar_set_t sets[2] = {
@@ -83,17 +75,28 @@ TEST("astar") {
     { .capacity = 100, .size = 0, .values = buf + 100 },
   };
 
+  i8 grid[WIDTH * HEIGHT] = {
+    0, 0, 0, 0, 0, 0, 0, 0, //
+    1, 1, 0, 1, 1, 0, 1, 0, //
+    1, 1, 0, 0, 1, 0, 1, 0, //
+    0, 0, 0, 1, 1, 1, 1, 0, //
+    1, 1, 0, 0, 0, 0, 0, 0, //
+  };
+
   astar_state_t state;
-  REQUIRE_EQ(astar_init(&state, sets[0], sets[1], xy(0, 0), xy(5, 2)),
-             KIT_OK);
+  REQUIRE_EQ(
+      astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)),
+      KIT_OK);
 
-  s32 status = KIT_OK;
+  s32 status;
 
-  while (state.status == ASTAR_PROGRESS)
-    status |= astar_iteration_test(&state);
+  for (;;) {
+    status = astar_iteration_test(&state);
+    if (status != ASTAR_PROGRESS)
+      break;
+  }
 
-  REQUIRE_EQ(status, KIT_OK);
-  REQUIRE_EQ(state.status, ASTAR_SUCCESS);
+  REQUIRE_EQ(status, ASTAR_SUCCESS);
 
   i64 path_size = 10;
   i64 path[10];
@@ -113,4 +116,77 @@ TEST("astar") {
   }
 }
 
+TEST("astar no path") {
+  astar_node_t buf[200];
+
+  astar_set_t sets[2] = {
+    { .capacity = 100, .size = 0, .values = buf },
+    { .capacity = 100, .size = 0, .values = buf + 100 },
+  };
+
+  i8 grid[WIDTH * HEIGHT] = {
+    0, 0, 0, 0, 1, 0, 0, 0, //
+    1, 1, 0, 1, 1, 0, 1, 1, //
+    1, 1, 0, 0, 1, 0, 1, 0, //
+    0, 0, 0, 1, 1, 1, 1, 0, //
+    1, 1, 0, 0, 0, 0, 0, 0, //
+  };
+
+  astar_state_t state;
+  REQUIRE_EQ(
+      astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)),
+      KIT_OK);
+
+  s32 status;
+
+  for (;;) {
+    status = astar_iteration_test(&state);
+    if (status != ASTAR_PROGRESS)
+      break;
+  }
+
+  REQUIRE_EQ(status, ASTAR_FAIL);
+}
+
+TEST("astar empty path") {
+  astar_node_t buf[200];
+
+  astar_set_t sets[2] = {
+    { .capacity = 100, .size = 0, .values = buf },
+    { .capacity = 100, .size = 0, .values = buf + 100 },
+  };
+
+  i8 grid[WIDTH * HEIGHT] = {
+    0, 0, 0, 0, 1, 0, 0, 0, //
+    1, 1, 0, 1, 1, 0, 1, 1, //
+    1, 1, 0, 0, 1, 0, 1, 0, //
+    0, 0, 0, 1, 1, 1, 1, 0, //
+    1, 1, 0, 0, 0, 0, 0, 0, //
+  };
+
+  astar_state_t state;
+  REQUIRE_EQ(
+      astar_init(&state, sets[0], sets[1], grid, xy(4, 3), xy(4, 3)),
+      KIT_OK);
+
+  s32 status;
+
+  for (;;) {
+    status = astar_iteration_test(&state);
+    if (status != ASTAR_PROGRESS)
+      break;
+  }
+
+  REQUIRE_EQ(status, ASTAR_SUCCESS);
+
+  i64 path_size = 10;
+  i64 path[10];
+
+  REQUIRE_EQ(astar_path(&state, &path_size, path), KIT_OK);
+  REQUIRE_EQ(path_size, 1);
+
+  if (path_size == 1)
+    REQUIRE_EQ(path[0], xy(4, 3));
+}
+
 #undef KIT_TEST_FILE