summaryrefslogtreecommitdiff
path: root/source
diff options
context:
space:
mode:
Diffstat (limited to 'source')
-rw-r--r--source/kit/astar.h379
-rw-r--r--source/kit/astar.inl.h251
-rw-r--r--source/tests/astar.test.c111
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
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-22 23:06:08 +0000