10 * TODO use binary heap for the openlist and closed list.
11 * open list should be a min-heap on the f value
12 * closed list is just tested for existence, so a sort on
20 static double default_metric(int a, int b) {
24 static double default_heuristic(int a, int b) {
25 return (double)HL_distance(a, b);
28 struct HL_astar *HL_astar_init(struct HL_astar *state) {
30 state = malloc(sizeof *state);
31 if (!state) return NULL;
44 state->metric = default_metric;
45 state->heuristic = default_heuristic;
46 state->neighbor = HL_adjacent_hex;
52 * Reset a state to enpty. Doesn't free memory, so it
55 struct HL_astar *HL_astar_clear(struct HL_astar *s) {
66 void HL_astar_free(struct HL_astar *s) {
73 static struct HL_astar_hex *add_open(struct HL_astar *s, int hex) {
74 struct HL_astar_hex *node;
76 if (!s || s->error) return NULL;
78 /* TODO check in open or closed */
80 if (s->nodes == s->allocated) {
82 struct HL_astar_hex *alloc;
84 newalloc = s->allocated * 2;
88 alloc = realloc(s->sets, newalloc * sizeof *node);
94 s->allocated = newalloc;
97 node = &s->sets[s->nodes++];
105 if (s->open) s->open->prev = node;
106 node->next = s->open;
113 static struct HL_astar_hex *closehex(struct HL_astar *s, struct HL_astar_hex *h) {
114 if (!s || s->error) return NULL;
121 fprintf(stderr, "Closing hex: (%d,%d)\n", HL_cantor_x(h->hex), HL_cantor_y(h->hex));
124 if (h->prev) h->prev->next = h->next;
125 if (h->next) h->next->prev = h->prev;
126 if (s->closed) s->closed->prev = h;
127 if (s->open == h) s->open = h->next;
135 struct HL_astar_hex *in_closed(struct HL_astar *s, int hex) {
136 struct HL_astar_hex *h;
139 for(h = s->closed; h; h = h->next) {
140 if (h->hex == hex) return h;
145 struct HL_astar_hex *in_open(struct HL_astar *s, int hex) {
146 struct HL_astar_hex *h;
149 for(h = s->open; h; h = h->next) {
150 if (h->hex == hex) return h;
155 static struct HL_astar_hex *lowrank(struct HL_astar *s) {
156 struct HL_astar_hex *n, *r;
158 if (!s || s->open == 0) return 0;
161 for(n = r; n; n = n->next) {
171 * find a path from start to end. we use A*
173 int HL_findpath(struct HL_astar *s, int loops) {
175 struct HL_astar_hex *node, *x, *yopen;
181 if (s->start == s->goal) return 0;
183 node = add_open(s, s->start);
185 node->h = s->heuristic(s->start, s->goal);
189 fprintf(stderr, "A* findpath: ");
190 fprintf(stderr, "(%d,%d)", HL_cantor_x(s->start), HL_cantor_y(s->start));
191 fprintf(stderr, " -> ");
192 fprintf(stderr, "(%d,%d)", HL_cantor_x(s->goal), HL_cantor_y(s->goal));
193 fprintf(stderr, "\n");
199 fprintf(stderr, "open list: ");
200 for(x = s->open; x; x = x->next) {
201 fprintf(stderr, "(%d,%d)", HL_cantor_x(x->hex), HL_cantor_y(x->hex));
203 fprintf(stderr, "\n");
204 fprintf(stderr, "closed list: ");
205 for(x = s->closed; x; x = x->next) {
206 fprintf(stderr, "(%d,%d)", HL_cantor_x(x->hex), HL_cantor_y(x->hex));
208 fprintf(stderr, "\n");
213 fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
214 HL_cantor_x(x->hex), HL_cantor_y(x->hex),
218 if (x->hex == s->goal) {
222 for (dir = 0; y = s->neighbor(x->hex, dir); dir++) {
223 if (in_closed(s, y)) {
227 cost = x->g + s->metric(x->hex, y);
229 yopen = in_open(s, y);
231 yopen = add_open(s, y);
233 } else if (cost < yopen->g) {
241 if (x->f < yopen->parent->f) {
248 yopen->h = s->heuristic(y, s->goal);
249 yopen->f = yopen->g + yopen->h;
251 fprintf(stderr, "maybe better hex = (%d,%d) f = %f, g = %f, h = %f\n",
252 HL_cantor_x(yopen->hex), HL_cantor_y(yopen->hex),
253 yopen->f, yopen->g, yopen->h
260 /* error no path ? */
261 if (x->hex != s->goal) {
267 fprintf(stderr, "found path: ");
270 /* reconstruct path */
273 for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
275 yopen->parent->next = yopen;
277 if (yopen->hex == s->start) s->from = yopen;
279 fprintf(stderr, "(%p %d %d,%d)",
281 HL_cantor_x(yopen->hex),
282 HL_cantor_y(yopen->hex)
290 fprintf(stderr, "\n");