From fc7f0985610afdf38d9bf88de8126788c25e97fa Mon Sep 17 00:00:00 2001 From: Mitya Selivanov Date: Sun, 25 Feb 2024 20:13:38 +0100 Subject: A* search: refactor, add tests --- source/kit/astar.h | 79 ++++++++++++++++++-------------- source/tests/astar.test.c | 114 ++++++++++++++++++++++++++++++++++++++-------- 2 files changed, 141 insertions(+), 52 deletions(-) diff --git a/source/kit/astar.h b/source/kit/astar.h index 554649a..f1758fc 100644 --- a/source/kit/astar.h +++ b/source/kit/astar.h @@ -26,6 +26,7 @@ enum { ASTAR_SUCCESS, ASTAR_FAIL, ASTAR_OUT_OF_MEMORY, + ASTAR_INVALID_ARGUMENT, }; typedef struct { @@ -49,11 +50,11 @@ typedef struct { } astar_set_t; typedef struct { - s32 status; - i64 source; - i64 destination; astar_set_t open; astar_set_t closed; + void *user_data; + i64 source; + i64 destination; } astar_state_t; #ifdef __GNUC__ @@ -65,7 +66,7 @@ typedef struct { #endif static s32 astar_init(astar_state_t *state, astar_set_t open, - astar_set_t closed, i64 source, + 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); @@ -75,16 +76,14 @@ static s32 astar_init(astar_state_t *state, astar_set_t open, memset(state, 0, sizeof *state); - state->status = ASTAR_PROGRESS; state->source = source; state->destination = destination; state->open = open; state->closed = closed; + state->user_data = user_data; - if (state->open.capacity <= 0) { - state->status = ASTAR_OUT_OF_MEMORY; + if (state->open.capacity <= 0) return KIT_ERROR_INVALID_ARGUMENT; - } state->open.values[0] = (astar_node_t) { .id = source, @@ -168,14 +167,14 @@ extern "C" { #endif #ifndef ASTAR_NEIGHBOR -# define ASTAR_NEIGHBOR(a, n) \ - (astar_link_t) { \ - .stop = 1, \ +# define ASTAR_NEIGHBOR(user_data_, node_, index_) \ + (astar_link_t) { \ + .stop = 1, \ } #endif #ifndef ASTAR_HEURISTIC -# define ASTAR_HEURISTIC(a, b) (-1) +# define ASTAR_HEURISTIC(user_data_, node_0_, node_2_) (-1) #endif #ifdef __GNUC__ @@ -194,17 +193,13 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { assert(state != NULL); if (state == NULL) - return KIT_ERROR_INVALID_ARGUMENT; + return ASTAR_INVALID_ARGUMENT; - if (state->open.size == 0) { - state->status = ASTAR_FAIL; - return KIT_OK; - } + if (state->open.size == 0) + return ASTAR_FAIL; - if (state->open.values == NULL || state->closed.values == NULL) { - state->status = ASTAR_OUT_OF_MEMORY; - return KIT_ERROR_BAD_ALLOC; - } + if (state->open.values == NULL || state->closed.values == NULL) + return ASTAR_OUT_OF_MEMORY; // Find a node in the open set that is closest to the destination // @@ -223,14 +218,36 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { --state->open.size; } + // Check if we reached the destination + // + 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.size; + + return ASTAR_OUT_OF_MEMORY; + } + + // Add the nearest node to the closed set + // + state->closed.values[state->closed.size] = nearest_node; + ++state->closed.size; + + // Finish the search + return ASTAR_SUCCESS; + } + // Enumerate all neighbors // for (i64 k = 0;; ++k) { // Get a link to the next neighbor node // + (void) state->user_data; (void) nearest_node.id; (void) k; - astar_link_t link = ASTAR_NEIGHBOR(nearest_node.id, k); + astar_link_t link = ASTAR_NEIGHBOR(state->user_data, + nearest_node.id, k); // If there is no more neighbors, break the loop if (link.stop) @@ -249,10 +266,11 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { // Calculate distance estimations // + (void) state->user_data; (void) neighbor_node.id; (void) state->destination; i64 estimated_node_to_destination = ASTAR_HEURISTIC( - neighbor_node.id, state->destination); + state->user_data, neighbor_node.id, state->destination); neighbor_node.estimated_source_to_destination = neighbor_node.exact_source_to_node + @@ -266,8 +284,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { state->open.values[state->open.size - 1] = nearest_node; ++state->open.size; - state->status = ASTAR_OUT_OF_MEMORY; - return KIT_ERROR_BAD_ALLOC; + return ASTAR_OUT_OF_MEMORY; } // Add the nearest node to the closed set @@ -281,8 +298,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { ++state->closed.size; // Finish the search - state->status = ASTAR_SUCCESS; - return KIT_OK; + return ASTAR_SUCCESS; } // Check if this node is already in the closed set @@ -331,8 +347,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { state->open.values[state->open.size - 1] = nearest_node; ++state->open.size; - state->status = ASTAR_OUT_OF_MEMORY; - return KIT_ERROR_BAD_ALLOC; + return ASTAR_OUT_OF_MEMORY; } // Add this node to the open set @@ -348,8 +363,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { state->open.values[state->open.size - 1] = nearest_node; ++state->open.size; - state->status = ASTAR_OUT_OF_MEMORY; - return KIT_ERROR_BAD_ALLOC; + return ASTAR_OUT_OF_MEMORY; } // Add the nearest node to the closed set @@ -358,8 +372,7 @@ static s32 NAME_(astar_iteration)(astar_state_t *state) { ++state->closed.size; // Continue the search - state->status = ASTAR_PROGRESS; - return KIT_OK; + return ASTAR_PROGRESS; } #undef NAME_ diff --git a/source/tests/astar.test.c b/source/tests/astar.test.c index eeb1100..6be9418 100644 --- a/source/tests/astar.test.c +++ b/source/tests/astar.test.c @@ -10,15 +10,7 @@ enum { 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) { +static astar_link_t neighbor(i8 *grid, i64 a, i64 n) { i64 x = a % WIDTH; i64 y = a / WIDTH; @@ -67,15 +59,15 @@ static i64 heuristic(i64 a, i64 b) { #define ASTAR_TEMPLATE #define ASTAR_SUFFIX _test -#define ASTAR_NEIGHBOR(a, n) neighbor(a, n) -#define ASTAR_HEURISTIC(a, b) heuristic(a, b) +#define ASTAR_NEIGHBOR(data, a, n) neighbor((i8 *) data, a, n) +#define ASTAR_HEURISTIC(data, a, b) heuristic(a, b) #include "../kit/astar.h" static i64 xy(i64 x, i64 y) { return y * WIDTH + x; } -TEST("astar") { +TEST("astar path exists") { astar_node_t buf[200]; astar_set_t sets[2] = { @@ -83,17 +75,28 @@ TEST("astar") { { .capacity = 100, .size = 0, .values = buf + 100 }, }; + 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, // + }; + astar_state_t state; - REQUIRE_EQ(astar_init(&state, sets[0], sets[1], xy(0, 0), xy(5, 2)), - KIT_OK); + REQUIRE_EQ( + astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)), + KIT_OK); - s32 status = KIT_OK; + s32 status; - while (state.status == ASTAR_PROGRESS) - status |= astar_iteration_test(&state); + for (;;) { + status = astar_iteration_test(&state); + if (status != ASTAR_PROGRESS) + break; + } - REQUIRE_EQ(status, KIT_OK); - REQUIRE_EQ(state.status, ASTAR_SUCCESS); + REQUIRE_EQ(status, ASTAR_SUCCESS); i64 path_size = 10; i64 path[10]; @@ -113,4 +116,77 @@ TEST("astar") { } } +TEST("astar no path") { + astar_node_t buf[200]; + + astar_set_t sets[2] = { + { .capacity = 100, .size = 0, .values = buf }, + { .capacity = 100, .size = 0, .values = buf + 100 }, + }; + + i8 grid[WIDTH * HEIGHT] = { + 0, 0, 0, 0, 1, 0, 0, 0, // + 1, 1, 0, 1, 1, 0, 1, 1, // + 1, 1, 0, 0, 1, 0, 1, 0, // + 0, 0, 0, 1, 1, 1, 1, 0, // + 1, 1, 0, 0, 0, 0, 0, 0, // + }; + + astar_state_t state; + REQUIRE_EQ( + astar_init(&state, sets[0], sets[1], grid, xy(0, 0), xy(5, 2)), + KIT_OK); + + s32 status; + + for (;;) { + status = astar_iteration_test(&state); + if (status != ASTAR_PROGRESS) + break; + } + + REQUIRE_EQ(status, ASTAR_FAIL); +} + +TEST("astar empty path") { + astar_node_t buf[200]; + + astar_set_t sets[2] = { + { .capacity = 100, .size = 0, .values = buf }, + { .capacity = 100, .size = 0, .values = buf + 100 }, + }; + + i8 grid[WIDTH * HEIGHT] = { + 0, 0, 0, 0, 1, 0, 0, 0, // + 1, 1, 0, 1, 1, 0, 1, 1, // + 1, 1, 0, 0, 1, 0, 1, 0, // + 0, 0, 0, 1, 1, 1, 1, 0, // + 1, 1, 0, 0, 0, 0, 0, 0, // + }; + + astar_state_t state; + REQUIRE_EQ( + astar_init(&state, sets[0], sets[1], grid, xy(4, 3), xy(4, 3)), + KIT_OK); + + s32 status; + + for (;;) { + status = astar_iteration_test(&state); + if (status != ASTAR_PROGRESS) + break; + } + + REQUIRE_EQ(status, ASTAR_SUCCESS); + + i64 path_size = 10; + i64 path[10]; + + REQUIRE_EQ(astar_path(&state, &path_size, path), KIT_OK); + REQUIRE_EQ(path_size, 1); + + if (path_size == 1) + REQUIRE_EQ(path[0], xy(4, 3)); +} + #undef KIT_TEST_FILE -- cgit v1.2.3