A* search test

3 files changed, 487 insertions, 254 deletions

diff --git a/source/kit/astar.h b/source/kit/astar.h
new file mode 100644
index 0000000..b7f3837
--- /dev/null
+++ b/source/kit/astar.h
@@ -0,0 +1,379 @@
+//  A* graph search algorithm
+//
+
+//  TODO
+//  - Nearest: save the nearest node when the search is failed or not
+//    yet finished.
+//  - Sight: when two nodes are in direct sight of each other, skip
+//    nodes between them (Thera*).
+
+#ifndef KIT_ASTAR_INL_H
+#define KIT_ASTAR_INL_H
+
+#include "types.h"
+#include "status.h"
+
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+enum {
+  ASTAR_PROGRESS = 0,
+  ASTAR_SUCCESS,
+  ASTAR_FAIL,
+  ASTAR_OUT_OF_MEMORY,
+};
+
+typedef struct {
+  b8  stop;
+  b8  skip;
+  i64 neighbor;
+  i64 distance;
+} astar_link_t;
+
+typedef struct {
+  i64 id;
+  i64 previous;
+  i64 exact_source_to_node;
+  i64 estimated_source_to_destination;
+} astar_node_t;
+
+typedef struct {
+  i64           capacity;
+  i64           size;
+  astar_node_t *values;
+} astar_set_t;
+
+typedef struct {
+  s32         status;
+  i64         source;
+  i64         destination;
+  astar_set_t open;
+  astar_set_t closed;
+} astar_state_t;
+
+#ifdef __GNUC__
+#  pragma GCC diagnostic push
+#  pragma GCC diagnostic ignored "-Wunused-function"
+#  pragma GCC diagnostic ignored "-Wunknown-pragmas"
+#  pragma GCC            push_options
+#  pragma GCC            optimize("O3")
+#endif
+
+static s32 astar_init(astar_state_t *state, astar_set_t open,
+                      astar_set_t closed, 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;
+
+  memset(state, 0, sizeof *state);
+
+  state->status      = ASTAR_PROGRESS;
+  state->source      = source;
+  state->destination = destination;
+  state->open        = open;
+  state->closed      = closed;
+
+  if (state->open.capacity <= 0) {
+    state->status = ASTAR_OUT_OF_MEMORY;
+    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.size   = 1;
+  state->closed.size = 0;
+
+  return KIT_OK;
+}
+
+static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) {
+  assert(state != NULL && size != NULL);
+
+  if (state == NULL || size == NULL)
+    return KIT_ERROR_INVALID_ARGUMENT;
+
+  i64 path_size = *size;
+
+  *size = 1;
+
+  i64 id = state->destination;
+
+  while (id != state->source) {
+    if (*size > state->closed.size)
+      return KIT_ERROR_INTERNAL;
+
+    i64 index = 0;
+    for (; index < state->closed.size; index++)
+      if (state->closed.values[index].id == id)
+        break;
+    if (index == state->closed.size)
+      return KIT_ERROR_INTERNAL;
+
+    if (path != NULL && *size <= path_size)
+      path[*size - 1] = id;
+
+    id = state->closed.values[index].previous;
+    ++*size;
+  }
+
+  if (path != NULL && *size <= path_size)
+    path[*size - 1] = id;
+
+  if (path != NULL) {
+    if (*size < path_size)
+      path_size = *size;
+
+    for (i64 i = 0; i < path_size / 2; ++i) {
+      i64 z                   = path[i];
+      path[i]                 = path[path_size - 1 - i];
+      path[path_size - 1 - i] = z;
+    }
+  }
+
+  return KIT_OK;
+}
+
+#ifdef __GNUC__
+#  pragma GCC            pop_options
+#  pragma GCC diagnostic pop
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+#ifdef ASTAR_TEMPLATE
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef ASTAR_SUFFIX
+#  define ASTAR_SUFFIX
+#endif
+
+#ifndef ASTAR_NEIGHBOR
+#  define ASTAR_NEIGHBOR(a, n) \
+    (astar_link_t) {           \
+      .stop = 1,               \
+    }
+#endif
+
+#ifndef ASTAR_HEURISTIC
+#  define ASTAR_HEURISTIC(a, b) (-1)
+#endif
+
+#ifdef __GNUC__
+#  pragma GCC diagnostic push
+#  pragma GCC diagnostic ignored "-Wunused-function"
+#  pragma GCC diagnostic ignored "-Wunknown-pragmas"
+#  pragma GCC            push_options
+#  pragma GCC            optimize("O3")
+#endif
+
+#define CAT1_(x, y) x##y
+#define CAT2_(x, y) CAT1_(x, y)
+#define NAME_(x) CAT2_(x, ASTAR_SUFFIX)
+
+static s32 NAME_(astar_iteration)(astar_state_t *state) {
+  assert(state != NULL);
+
+  if (state == NULL)
+    return KIT_ERROR_INVALID_ARGUMENT;
+
+  if (state->open.size == 0) {
+    state->status = ASTAR_FAIL;
+    return KIT_OK;
+  }
+
+  if (state->open.values == NULL || state->closed.values == NULL) {
+    state->status = ASTAR_OUT_OF_MEMORY;
+    return KIT_ERROR_BAD_ALLOC;
+  }
+
+  //  Find a node in the open set that is closest to the destination
+  //
+  astar_node_t 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)
+        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.size;
+  }
+
+  //  Enumerate all neighbors
+  //
+  for (i64 k = 0;; ++k) {
+    //  Get a link to the next neighbor node
+    //
+    (void) nearest_node.id;
+    (void) k;
+    astar_link_t link = ASTAR_NEIGHBOR(nearest_node.id, k);
+
+    //  If there is no more neighbors, break the loop
+    if (link.stop)
+      break;
+    //  If there is no link, proceed to the next link
+    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) neighbor_node.id;
+    (void) state->destination;
+    i64 estimated_node_to_destination = ASTAR_HEURISTIC(
+        neighbor_node.id, state->destination);
+
+    neighbor_node.estimated_source_to_destination =
+        neighbor_node.exact_source_to_node +
+        estimated_node_to_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.size;
+
+        state->status = ASTAR_OUT_OF_MEMORY;
+        return KIT_ERROR_BAD_ALLOC;
+      }
+
+      //  Add the nearest node to the closed set
+      //
+      state->closed.values[state->closed.size] = nearest_node;
+      ++state->closed.size;
+
+      //  Add the neighbor node to the closed set
+      //
+      state->closed.values[state->closed.size] = neighbor_node;
+      ++state->closed.size;
+
+      //  Finish the search
+      state->status = ASTAR_SUCCESS;
+      return KIT_OK;
+    }
+
+    //  Check if this node is already in the closed set
+    //
+    {
+      i64 index_in_closed = -1;
+      for (i64 i = 0; i < state->closed.size; ++i)
+        if (state->closed.values[i].id == neighbor_node.id) {
+          index_in_closed = i;
+          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 is already in the open set
+    //
+    {
+      i64 index_in_open = -1;
+      for (i64 i = 0; i < state->closed.size; ++i)
+        if (state->open.values[i].id == neighbor_node.id) {
+          index_in_open = i;
+          break;
+        }
+      if (index_in_open != -1) {
+        //  Check if this node has a better estimate
+        //
+        if (neighbor_node.estimated_source_to_destination <
+            state->open.values[index_in_open]
+                .estimated_source_to_destination)
+          //  Replace the node
+          state->open.values[index_in_open] = neighbor_node;
+        continue;
+      }
+    }
+
+    if (state->open.size + 1 > state->open.capacity) {
+      //  Restore the state
+      state->open.values[state->open.size - 1] = nearest_node;
+      ++state->open.size;
+
+      state->status = ASTAR_OUT_OF_MEMORY;
+      return KIT_ERROR_BAD_ALLOC;
+    }
+
+    //  Add this node to the open set
+    //
+    state->open.values[state->open.size] = neighbor_node;
+    ++state->open.size;
+  }
+
+  //  Check if we can add a node to the closed set
+  //
+  if (state->closed.size + 1 > state->closed.capacity) {
+    //  Restore the state
+    state->open.values[state->open.size - 1] = nearest_node;
+    ++state->open.size;
+
+    state->status = ASTAR_OUT_OF_MEMORY;
+    return KIT_ERROR_BAD_ALLOC;
+  }
+
+  //  Add the nearest node to the closed set
+  //
+  state->closed.values[state->closed.size] = nearest_node;
+  ++state->closed.size;
+
+  //  Continue the search
+  state->status = ASTAR_PROGRESS;
+  return KIT_OK;
+}
+
+#undef NAME_
+#undef CAT1_
+#undef CAT2_
+
+#ifdef __GNUC__
+#  pragma GCC            pop_options
+#  pragma GCC diagnostic pop
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#undef ASTAR_TEMPLATE
+#endif
diff --git a/source/kit/astar.inl.h b/source/kit/astar.inl.h
deleted file mode 100644
index a844fb2..0000000
--- a/source/kit/astar.inl.h
+++ /dev/null
@@ -1,251 +0,0 @@
-//  A* graph search algorithm
-//
-
-//  TODO
-//  - Set: abstraction for the node set data type.
-//  - Nearest: save the nearest node when the search is failed or not
-//    yet finished.
-//  - Sight: when two nodes are in direct sight of each other, skip
-//    nodes between them (Thera*).
-
-#include "types.h"
-
-#include <assert.h>
-#include <stddef.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#ifndef ASTAR_SET_SIZE
-#  define ASTAR_SET_SIZE 1024
-#endif
-
-enum {
-  ASTAR_PROGRESS = 0,
-  ASTAR_SUCCESS,
-  ASTAR_FAIL,
-};
-
-typedef struct {
-  i64 id;
-  i64 parent;
-  i64 exact_source_to_node;
-  i64 estimated_node_to_destination;
-  i64 estimated_source_to_destination;
-} astar_node_t;
-
-typedef struct {
-  b8  stop;
-  b8  skip;
-  i64 neighbor;
-  i64 distance;
-} astar_link_t;
-
-typedef struct {
-  i64          size;
-  astar_node_t values[ASTAR_SET_SIZE];
-} astar_set_t;
-
-typedef struct {
-  i64         status;
-  i64         source;
-  i64         destination;
-  astar_set_t open;
-  astar_set_t closed;
-} astar_state_t;
-
-#ifndef ASTAR_NEIGHBOR
-#  define ASTAR_NEIGHBOR(x, n) \
-    (astar_link_t) {           \
-      .stop = 1,               \
-    }
-#endif
-
-#ifndef ASTAR_HEURISTIC
-#  define ASTAR_HEURISTIC(x, y) (-1)
-#endif
-
-#ifdef __GNUC__
-#  pragma GCC diagnostic push
-#  pragma GCC diagnostic ignored "-Wunused-function"
-#  pragma GCC diagnostic ignored "-Wunknown-pragmas"
-#  pragma GCC            push_options
-#  pragma GCC            optimize("O3")
-#endif
-
-static void astar_iteration(astar_state_t *state) {
-  assert(state != NULL);
-
-  if (state == NULL)
-    return;
-
-  if (state->open.size == 0) {
-    state->status = ASTAR_FAIL;
-    return;
-  }
-
-  assert(state->open.values != NULL);
-  assert(state->closed.values != NULL);
-
-  //  FIXME
-  //  Use abstractions for set element search
-  //
-
-  //  Find a node that is closest to the destination
-  //
-  astar_node_t nearest_node;
-  {
-    i64 index_nearest = 0;
-    for (i64 i = 1; i < state->open.size; i++)
-      if (state->open.values[i].estimated_source_to_destination <
-          state->open.values[index_nearest]
-              .estimated_source_to_destination)
-        index_nearest = i;
-    nearest_node = state->open.values[index_nearest];
-    if (index_nearest != state->open.size - 1)
-      state->open.values[index_nearest] =
-          state->open.values[state->open.size - 1];
-    --state->open.size;
-  }
-
-  //  Enumerate all neighbors
-  //
-  for (i64 k = 0;; ++k) {
-    //  Get a link to the next neighbor node
-    //
-    (void) nearest_node.id;
-    (void) k;
-    astar_link_t link = ASTAR_NEIGHBOR(nearest_node.id, k);
-
-    //  If there is no more neighbors, break the loop
-    if (link.stop)
-      break;
-    //  If there is no link, proceed to the next link
-    if (link.skip)
-      continue;
-
-    astar_node_t neighbor_node = {
-      .id                   = link.neighbor,
-      .parent               = nearest_node.id,
-      .exact_source_to_node = nearest_node.exact_source_to_node +
-                              link.distance,
-      .estimated_node_to_destination   = -1,
-      .estimated_source_to_destination = -1,
-    };
-
-    //  Calculate distance estimations
-    //
-    (void) neighbor_node.id;
-    (void) state->destination;
-    neighbor_node.estimated_node_to_destination = ASTAR_HEURISTIC(
-        neighbor_node.id, state->destination);
-    neighbor_node.estimated_source_to_destination =
-        neighbor_node.exact_source_to_node +
-        neighbor_node.estimated_node_to_destination;
-
-    //  Check if we reached the destination
-    //
-    if (neighbor_node.id == state->destination) {
-      assert(state->closed.size + 2 <= ASTAR_SET_SIZE);
-
-      //  FIXME
-      //  Use abstractions for set element inserting
-      //
-
-      //  Add the nearest node to the closed set
-      //
-      state->closed.values[state->closed.size] = nearest_node;
-      ++state->closed.size;
-
-      //  Add the neighbor node to the closed set
-      //
-      state->closed.values[state->closed.size] = neighbor_node;
-      ++state->closed.size;
-
-      //  Finish the search
-      state->status = ASTAR_SUCCESS;
-      return;
-    }
-
-    //  FIXME
-    //  Use abstractions for the set search
-    //
-
-    //  Check if a node with a better estimate is already in the
-    //  closed set
-    //
-    i64 index_in_closed = -1;
-    for (i64 i = 0; i < state->closed.size; ++i)
-      if (state->closed.values[i].id == neighbor_node.id) {
-        index_in_closed = i;
-        break;
-      }
-    if (index_in_closed != -1 &&
-        state->closed.values[index_in_closed]
-                .estimated_source_to_destination <
-            neighbor_node.estimated_source_to_destination)
-      //  Skip this node
-      continue;
-
-    //  Check if this node is in the open set
-    //
-    i64 index_in_open = -1;
-    for (i64 i = 0; i < state->closed.size; ++i)
-      if (state->open.values[i].id == neighbor_node.id) {
-        index_in_open = i;
-        break;
-      }
-    if (index_in_open != -1) {
-      if (state->open.values[index_in_open]
-              .estimated_source_to_destination <
-          neighbor_node.estimated_source_to_destination)
-        // Skip this node
-        continue;
-
-      //  FIXME
-      //  Use abstractions for set element erasing
-      //
-
-      //  Remove this node from the open set
-      //
-      if (index_in_open != state->open.size - 1)
-        state->open.values[index_in_open] =
-            state->open.values[state->open.size - 1];
-      --state->open.size;
-    }
-
-    //  FIXME
-    //  Use abstractions to set element adding
-    //
-
-    //  Add this node to the open set
-    //
-    assert(state->open.size + 1 <= ASTAR_SET_SIZE);
-    state->open.values[state->open.size] = neighbor_node;
-    ++state->open.size;
-  }
-
-  //  FIXME
-  //  Use abstractions to set element adding
-  //
-
-  //  Add the nearest node to the closed set
-  //
-  assert(state->closed.size + 1 <= ASTAR_SET_SIZE);
-  state->closed.values[state->closed.size] = nearest_node;
-  ++state->closed.size;
-
-  //  Continue the search
-  state->status = ASTAR_PROGRESS;
-  return;
-}
-
-#ifdef __GNUC__
-#  pragma GCC            pop_options
-#  pragma GCC diagnostic pop
-#endif
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/source/tests/astar.test.c b/source/tests/astar.test.c
index ae5044c..eeb1100 100644
--- a/source/tests/astar.test.c
+++ b/source/tests/astar.test.c
@@ -1,11 +1,116 @@
-#include "../kit/astar.inl.h"
+#include "../kit/astar.h"
 
 #define KIT_TEST_FILE astar
 #include "../kit/test.h"
 
+#include <stdio.h>
+
+enum {
+  WIDTH  = 8,
+  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) {
+  i64 x = a % WIDTH;
+  i64 y = a / WIDTH;
+
+  if (n == 0)
+    --x;
+  else if (n == 1)
+    ++x;
+  else if (n == 2)
+    --y;
+  else if (n == 3)
+    ++y;
+  else
+    return (astar_link_t) {
+      .stop = 1,
+    };
+
+  i64 m = (y * WIDTH) + x;
+
+  if (x < 0 || x >= WIDTH || y < 0 || y >= HEIGHT || m < 0 ||
+      m >= WIDTH * HEIGHT || grid[m] == 1)
+    return (astar_link_t) {
+      .stop = 0,
+      .skip = 1,
+    };
+
+  return (astar_link_t) {
+    .stop     = 0,
+    .skip     = 0,
+    .neighbor = m,
+    .distance = 1,
+  };
+}
+
+static i64 heuristic(i64 a, i64 b) {
+  i64 x0 = a % WIDTH;
+  i64 y0 = a / WIDTH;
+
+  i64 x1 = b % WIDTH;
+  i64 y1 = b / WIDTH;
+
+  i64 dx = x0 < x1 ? x1 - x0 : x0 - x1;
+  i64 dy = y0 < y1 ? y1 - y0 : y0 - y1;
+
+  return dx + dy;
+}
+
+#define ASTAR_TEMPLATE
+#define ASTAR_SUFFIX _test
+#define ASTAR_NEIGHBOR(a, n) neighbor(a, n)
+#define ASTAR_HEURISTIC(a, b) heuristic(a, b)
+#include "../kit/astar.h"
+
+static i64 xy(i64 x, i64 y) {
+  return y * WIDTH + x;
+}
+
 TEST("astar") {
-  //  TODO
-  //
+  astar_node_t buf[200];
+
+  astar_set_t sets[2] = {
+    { .capacity = 100, .size = 0, .values = buf },
+    { .capacity = 100, .size = 0, .values = buf + 100 },
+  };
+
+  astar_state_t state;
+  REQUIRE_EQ(astar_init(&state, sets[0], sets[1], xy(0, 0), xy(5, 2)),
+             KIT_OK);
+
+  s32 status = KIT_OK;
+
+  while (state.status == ASTAR_PROGRESS)
+    status |= astar_iteration_test(&state);
+
+  REQUIRE_EQ(status, KIT_OK);
+  REQUIRE_EQ(state.status, ASTAR_SUCCESS);
+
+  i64 path_size = 10;
+  i64 path[10];
+
+  REQUIRE_EQ(astar_path(&state, &path_size, path), KIT_OK);
+  REQUIRE_EQ(path_size, 8);
+
+  if (path_size == 8) {
+    REQUIRE_EQ(path[0], xy(0, 0));
+    REQUIRE_EQ(path[1], xy(1, 0));
+    REQUIRE_EQ(path[2], xy(2, 0));
+    REQUIRE_EQ(path[3], xy(3, 0));
+    REQUIRE_EQ(path[4], xy(4, 0));
+    REQUIRE_EQ(path[5], xy(5, 0));
+    REQUIRE_EQ(path[6], xy(5, 1));
+    REQUIRE_EQ(path[7], xy(5, 2));
+  }
 }
 
 #undef KIT_TEST_FILE