From 5665439255e5e8ec219b309d455f4da8c960050b Mon Sep 17 00:00:00 2001 From: Mitya Selivanov Date: Sat, 24 Feb 2024 18:28:14 +0100 Subject: TODO A* search fix --- source/kit/astar.inl.h | 251 ++++++++++++++++++++++++++++++++++++++++++++++ source/tests/astar.test.c | 11 ++ 2 files changed, 262 insertions(+) create mode 100644 source/kit/astar.inl.h create mode 100644 source/tests/astar.test.c 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 +#include + +#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 -- cgit v1.2.3