diff options
| -rw-r--r-- | source/kit/astar.h | 379 | ||||
| -rw-r--r-- | source/kit/astar.inl.h | 251 | ||||
| -rw-r--r-- | source/tests/astar.test.c | 111 |
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 |