summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorMitya Selivanov <automainint@guattari.tech>2024-02-25 20:13:38 +0100
committerMitya Selivanov <automainint@guattari.tech>2024-02-25 20:13:38 +0100
commitfc7f0985610afdf38d9bf88de8126788c25e97fa (patch)
treeca180415729d93a956a047e38fb116bf73feaa3d
parentb64c2aedabb70b9761e1e9e6a72a2d576c30bf4a (diff)
downloadkit-fc7f0985610afdf38d9bf88de8126788c25e97fa.zip
A* search: refactor, add tests
-rw-r--r--source/kit/astar.h79
-rw-r--r--source/tests/astar.test.c114
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