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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
|
//
// -- An A* pathfinder inspired by the excellent tutorial at Red Blob Games --
//
// This is a reimplementation of the CTL example to STC:
// https://github.com/glouw/ctl/blob/master/examples/astar.c
// https://www.redblobgames.com/pathfinding/a-star/introduction.html
#include <stc/cstr.h>
#include <stdio.h>
typedef struct
{
int x;
int y;
int priorty;
int width;
}
point;
point
point_init(int x, int y, int width)
{
return (point) { x, y, 0, width };
}
int
point_cmp_priority(const point* a, const point* b)
{
return c_default_cmp(&a->priorty, &b->priorty);
}
int
point_equal(const point* a, const point* b)
{
return a->x == b->x && a->y == b->y;
}
point
point_from(const cstr* maze, const char* c, int width)
{
int index = (int)cstr_find(maze, c);
return point_init(index % width, index / width, width);
}
int
point_index(const point* p)
{
return p->x + p->width * p->y;
}
int
point_key_cmp(const point* a, const point* b)
{
int i = point_index(a);
int j = point_index(b);
return (i == j) ? 0 : (i < j) ? -1 : 1;
}
#define i_val point
#define i_cmp point_cmp_priority
#include <stc/cpque.h>
#define i_val point
#define i_opt c_no_cmp
#include <stc/cdeq.h>
#define i_key point
#define i_val int
#define i_cmp point_key_cmp
#define i_tag pcost
#include <stc/csmap.h>
#define i_key point
#define i_val point
#define i_cmp point_key_cmp
#define i_tag pstep
#include <stc/csmap.h>
cdeq_point
astar(cstr* maze, int width)
{
cdeq_point ret_path = {0};
cpque_point front = {0};
csmap_pstep from = {0};
csmap_pcost costs = {0};
c_defer(
cpque_point_drop(&front),
csmap_pstep_drop(&from),
csmap_pcost_drop(&costs)
){
point start = point_from(maze, "@", width);
point goal = point_from(maze, "!", width);
csmap_pcost_insert(&costs, start, 0);
cpque_point_push(&front, start);
while (!cpque_point_empty(&front))
{
point current = *cpque_point_top(&front);
cpque_point_pop(&front);
if (point_equal(¤t, &goal))
break;
point deltas[] = {
{ -1, +1, 0, width }, { 0, +1, 0, width }, { 1, +1, 0, width },
{ -1, 0, 0, width }, /* ~ ~ ~ ~ ~ ~ ~ */ { 1, 0, 0, width },
{ -1, -1, 0, width }, { 0, -1, 0, width }, { 1, -1, 0, width },
};
for (size_t i = 0; i < c_arraylen(deltas); i++)
{
point delta = deltas[i];
point next = point_init(current.x + delta.x, current.y + delta.y, width);
int new_cost = *csmap_pcost_at(&costs, current);
if (cstr_str(maze)[point_index(&next)] != '#')
{
const csmap_pcost_value *cost = csmap_pcost_get(&costs, next);
if (cost == NULL || new_cost < cost->second)
{
csmap_pcost_insert(&costs, next, new_cost);
next.priorty = new_cost + abs(goal.x - next.x) + abs(goal.y - next.y);
cpque_point_push(&front, next);
csmap_pstep_insert(&from, next, current);
}
}
}
}
point current = goal;
while (!point_equal(¤t, &start))
{
cdeq_point_push_front(&ret_path, current);
current = *csmap_pstep_at(&from, current);
}
cdeq_point_push_front(&ret_path, start);
}
return ret_path;
}
int
main(void)
{
cstr maze = cstr_lit(
"#########################################################################\n"
"# # # # # # #\n"
"# # ######### # ##### ######### ##### ##### ##### # ! #\n"
"# # # # # # # # # #\n"
"######### # ######### ######### ##### # # # ######### #\n"
"# # # # # # # # # # #\n"
"# # ############# # # ######### ##### # ######### # #\n"
"# # # # # # # # # #\n"
"# ############# ##### ##### # ##### ######### # ##### #\n"
"# # # # # # # # # #\n"
"# ##### ##### # ##### # ######### # # # #############\n"
"# # # # # # # # # # # #\n"
"############# # # # ######### # ##### # ##### ##### #\n"
"# # # # # # # # # #\n"
"# ##### # ######### ##### # ##### ##### ############# #\n"
"# # # # # # # # # #\n"
"# # ######### # ##### ######### # # ############# # #\n"
"# # # # # # # # # # #\n"
"# ######### # # # ##### ######### ######### # #########\n"
"# # # # # # # # # #\n"
"# @ # ##### ##### ##### ######### ##### # ######### # #\n"
"# # # # # # #\n"
"#########################################################################\n"
);
int width = (int)cstr_find(&maze, "\n") + 1;
cdeq_point ret_path = astar(&maze, width);
c_foreach (it, cdeq_point, ret_path)
cstr_data(&maze)[point_index(it.ref)] = 'x';
printf("%s", cstr_str(&maze));
cdeq_point_drop(&ret_path);
cstr_drop(&maze);
}
|
