TODO A* search fix

2 files changed, 262 insertions, 0 deletions

diff --git a/source/kit/astar.inl.h b/source/kit/astar.inl.h
new file mode 100644
index 0000000..7d6d99e
--- /dev/null
+++ b/source/kit/astar.inl.h
@@ -0,0 +1,251 @@
+//  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 erasing
+  //
+
+  //  Remove a node from the open set, the last node is a node with
+  //  the lowest estimated distance from the source to the destination
+  //
+  astar_node_t nearest_node =
+      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
+      //
+      for (i64 i = index_in_open + 1; i < state->open.size; ++i)
+        state->open.values[i - 1] = state->open.values[i];
+      --state->open.size;
+    }
+
+    //  FIXME
+    //  Use abstractions to set element adding
+    //
+
+    //  Add this node to the open set, sorted by the estimated
+    //  distance from the source to the destination, so the last node
+    //  in the set will be a node with the lowest estimate
+    //
+    assert(state->open.size + 1 <= ASTAR_SET_SIZE);
+    i64 index_insert = 0;
+    for (; index_insert < state->open.size; ++index_insert) {
+      if (state->open.values[index_insert]
+              .estimated_source_to_destination >
+          neighbor_node.estimated_source_to_destination)
+        continue;
+    }
+    for (i64 i = state->open.size; i > index_insert; --i)
+      state->open.values[i] = state->open.values[i - 1];
+    state->open.values[index_insert] = 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
new file mode 100644
index 0000000..ae5044c
--- /dev/null
+++ b/source/tests/astar.test.c
@@ -0,0 +1,11 @@
+#include "../kit/astar.inl.h"
+
+#define KIT_TEST_FILE astar
+#include "../kit/test.h"
+
+TEST("astar") {
+  //  TODO
+  //
+}
+
+#undef KIT_TEST_FILE