From 1317e9d2334fc9af5055bd8ef6f4064b4469a15b Mon Sep 17 00:00:00 2001 From: Mitya Selivanov Date: Thu, 11 Jul 2024 16:31:24 +0200 Subject: Update and fixes for A* search --- source/kit/astar.h | 163 +++++++++++++++++++++++----------------------- source/tests/astar.test.c | 26 ++++---- 2 files changed, 94 insertions(+), 95 deletions(-) diff --git a/source/kit/astar.h b/source/kit/astar.h index 4a8fb06..b76ef67 100644 --- a/source/kit/astar.h +++ b/source/kit/astar.h @@ -20,10 +20,6 @@ #include #include -#ifdef __cplusplus -extern "C" { -#endif - enum { ASTAR_PROGRESS = 0, ASTAR_SUCCESS, @@ -37,28 +33,28 @@ typedef struct { b8 skip; i64 neighbor; i64 distance; -} astar_link_t; +} Astar_Link; typedef struct { i64 id; i64 previous; - i64 exact_source_to_node; - i64 estimated_source_to_destination; -} astar_node_t; + i64 exact_distance; + i64 estimate; +} Astar_Node; typedef struct { - i64 capacity; - i64 size; - astar_node_t *values; -} astar_set_t; + i64 capacity; + i64 size; + Astar_Node *values; +} Astar_Set; typedef struct { - astar_set_t open; - astar_set_t closed; - void *user_data; - i64 source; - i64 destination; -} astar_state_t; + Astar_Set open; + Astar_Set closed; + void * user_data; + i64 source; + i64 destination; +} Astar_State; #ifdef __GNUC__ # pragma GCC diagnostic push @@ -68,11 +64,25 @@ typedef struct { # pragma GCC optimize("O3") #endif -static s32 astar_init(astar_state_t *state, astar_set_t open, - astar_set_t closed, void *user_data, i64 source, - i64 destination) { - assert(state != NULL && open.capacity > 0 && open.values != NULL && - closed.capacity > 0 && closed.values != NULL); +#ifdef __cplusplus +extern "C" { +#endif + +static s32 astar_init( + Astar_State *state, + Astar_Set open, + Astar_Set closed, + void * user_data, + 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; @@ -88,11 +98,11 @@ static s32 astar_init(astar_state_t *state, astar_set_t open, if (state->open.capacity <= 0) 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.values[0] = (Astar_Node) { + .id = source, + .previous = -1, + .exact_distance = 0, + .estimate = 0x7fffffffffffffffll, }; state->open.size = 1; @@ -101,7 +111,7 @@ static s32 astar_init(astar_state_t *state, astar_set_t open, return KIT_OK; } -static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) { +static s32 astar_path(Astar_State *state, i64 *size, i64 *path) { assert(state != NULL && size != NULL); if (state == NULL || size == NULL) @@ -148,15 +158,15 @@ static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) { return KIT_OK; } +#ifdef __cplusplus +} +#endif + #ifdef __GNUC__ # pragma GCC pop_options # pragma GCC diagnostic pop #endif -#ifdef __cplusplus -} -#endif - #endif // ================================================================ @@ -167,23 +177,20 @@ static s32 astar_path(astar_state_t *state, i64 *size, i64 *path) { #ifdef ASTAR_TEMPLATE -#ifdef __cplusplus -extern "C" { -#endif - #ifndef ASTAR_SUFFIX # define ASTAR_SUFFIX #endif #ifndef ASTAR_NEIGHBOR # define ASTAR_NEIGHBOR(user_data_, node_, index_) \ - (astar_link_t) { \ + (Astar_Link) { \ .stop = 1, \ } #endif #ifndef ASTAR_HEURISTIC -# define ASTAR_HEURISTIC(user_data_, node_0_, node_2_) (-1) +# define ASTAR_HEURISTIC(user_data_, node_0_, node_2_) \ + 0x7fffffffffffffffll #endif #ifdef __GNUC__ @@ -198,7 +205,11 @@ extern "C" { #define CAT2_(x, y) CAT1_(x, y) #define NAME_(x) CAT2_(x, ASTAR_SUFFIX) -static s32 NAME_(astar_iteration)(astar_state_t *state) { +#ifdef __cplusplus +extern "C" { +#endif + +static s32 NAME_(astar_iteration)(Astar_State *state) { assert(state != NULL); if (state == NULL) @@ -212,18 +223,15 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { // Find a node in the open set that is closest to the destination // - astar_node_t nearest_node; + Astar_Node 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) + if (state->open.values[i].estimate < state->open.values[index_in_open].estimate) 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.values[index_in_open] = state->open.values[state->open.size - 1]; --state->open.size; } @@ -232,7 +240,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { if (nearest_node.id == state->destination) { if (state->closed.size + 1 > state->closed.capacity) { // Restore the state - state->open.values[state->open.size - 1] = nearest_node; + state->open.values[state->open.size] = nearest_node; ++state->open.size; return ASTAR_OUT_OF_MEMORY; @@ -253,10 +261,8 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { // Get a link to the next neighbor node // (void) state->user_data; - (void) nearest_node.id; (void) k; - astar_link_t link = ASTAR_NEIGHBOR(state->user_data, - nearest_node.id, k); + Astar_Link link = ASTAR_NEIGHBOR(state->user_data, nearest_node.id, k); // If there is no more neighbors, break the loop if (link.stop) @@ -265,32 +271,24 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { 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) state->user_data; - (void) neighbor_node.id; (void) state->destination; - i64 estimated_node_to_destination = ASTAR_HEURISTIC( - state->user_data, neighbor_node.id, state->destination); - - neighbor_node.estimated_source_to_destination = - neighbor_node.exact_source_to_node + - estimated_node_to_destination; + + Astar_Node neighbor_node = { + .id = link.neighbor, + .previous = nearest_node.id, + .exact_distance = nearest_node.exact_distance + link.distance, + .estimate = ASTAR_HEURISTIC(state->user_data, link.neighbor, state->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.values[state->open.size] = nearest_node; ++state->open.size; return ASTAR_OUT_OF_MEMORY; @@ -320,13 +318,14 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { 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 has a better distance + // + if (neighbor_node.exact_distance < state->closed.values[index_in_closed].exact_distance) + // Replace the node + state->closed.values[index_in_closed] = neighbor_node; + + // Skip this node + continue; } } @@ -340,20 +339,20 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { break; } if (index_in_open != -1) { - // Check if this node has a better estimate + // Check if this node has a better distance // - if (neighbor_node.estimated_source_to_destination < - state->open.values[index_in_open] - .estimated_source_to_destination) + if (neighbor_node.exact_distance < state->open.values[index_in_open].exact_distance) // Replace the node state->open.values[index_in_open] = neighbor_node; + + // Skip this node continue; } } if (state->open.size + 1 > state->open.capacity) { // Restore the state - state->open.values[state->open.size - 1] = nearest_node; + state->open.values[state->open.size] = nearest_node; ++state->open.size; return ASTAR_OUT_OF_MEMORY; @@ -384,6 +383,10 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { return ASTAR_PROGRESS; } +#ifdef __cplusplus +} +#endif + #undef NAME_ #undef CAT1_ #undef CAT2_ @@ -393,9 +396,5 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { # pragma GCC diagnostic pop #endif -#ifdef __cplusplus -} -#endif - #undef ASTAR_TEMPLATE #endif diff --git a/source/tests/astar.test.c b/source/tests/astar.test.c index 6be9418..f037d65 100644 --- a/source/tests/astar.test.c +++ b/source/tests/astar.test.c @@ -10,7 +10,7 @@ enum { HEIGHT = 5, }; -static astar_link_t neighbor(i8 *grid, i64 a, i64 n) { +static Astar_Link neighbor(i8 *grid, i64 a, i64 n) { i64 x = a % WIDTH; i64 y = a / WIDTH; @@ -23,7 +23,7 @@ static astar_link_t neighbor(i8 *grid, i64 a, i64 n) { else if (n == 3) ++y; else - return (astar_link_t) { + return (Astar_Link) { .stop = 1, }; @@ -31,12 +31,12 @@ static astar_link_t neighbor(i8 *grid, i64 a, i64 n) { if (x < 0 || x >= WIDTH || y < 0 || y >= HEIGHT || m < 0 || m >= WIDTH * HEIGHT || grid[m] == 1) - return (astar_link_t) { + return (Astar_Link) { .stop = 0, .skip = 1, }; - return (astar_link_t) { + return (Astar_Link) { .stop = 0, .skip = 0, .neighbor = m, @@ -68,9 +68,9 @@ static i64 xy(i64 x, i64 y) { } TEST("astar path exists") { - astar_node_t buf[200]; + Astar_Node buf[200]; - astar_set_t sets[2] = { + Astar_Set sets[2] = { { .capacity = 100, .size = 0, .values = buf }, { .capacity = 100, .size = 0, .values = buf + 100 }, }; @@ -83,7 +83,7 @@ TEST("astar path exists") { 1, 1, 0, 0, 0, 0, 0, 0, // }; - astar_state_t state; + Astar_State state; REQUIRE_EQ( astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)), KIT_OK); @@ -117,9 +117,9 @@ TEST("astar path exists") { } TEST("astar no path") { - astar_node_t buf[200]; + Astar_Node buf[200]; - astar_set_t sets[2] = { + Astar_Set sets[2] = { { .capacity = 100, .size = 0, .values = buf }, { .capacity = 100, .size = 0, .values = buf + 100 }, }; @@ -132,7 +132,7 @@ TEST("astar no path") { 1, 1, 0, 0, 0, 0, 0, 0, // }; - astar_state_t state; + Astar_State state; REQUIRE_EQ( astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)), KIT_OK); @@ -149,9 +149,9 @@ TEST("astar no path") { } TEST("astar empty path") { - astar_node_t buf[200]; + Astar_Node buf[200]; - astar_set_t sets[2] = { + Astar_Set sets[2] = { { .capacity = 100, .size = 0, .values = buf }, { .capacity = 100, .size = 0, .values = buf + 100 }, }; @@ -164,7 +164,7 @@ TEST("astar empty path") { 1, 1, 0, 0, 0, 0, 0, 0, // }; - astar_state_t state; + Astar_State state; REQUIRE_EQ( astar_init(&state, sets[0], sets[1], grid, xy(4, 3), xy(4, 3)), KIT_OK); -- cgit v1.2.3