1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
|
#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") {
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
|