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(struct HL_hex a, struct HL_hex b) {
24 static double default_heuristic(struct HL_hex a, struct HL_hex 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;
43 state->metric = default_metric;
44 state->heuristic = default_heuristic;
45 state->neighbor = HL_adjhexp;
51 * Reset a state to enpty.
53 struct HL_astar *HL_astar_clear(struct HL_astar *s) {
54 struct HL_astar_hex *hex;
62 hex = s->closed->next;
75 void HL_astar_free(struct HL_astar *s) {
76 struct HL_astar_hex *hex;
77 /* free open and closed lists */
84 hex = s->closed->next;
94 static struct HL_astar_hex *add_open(struct HL_astar *s, struct HL_hex hex) {
95 struct HL_astar_hex *node;
97 fprintf(stderr, "add to open %d\n", HL_cantor(hex));
100 if (!s || s->error) {
102 fprintf(stderr, "returning null\n");
108 fprintf(stderr, "mallocing node(%zd)\n", sizeof *node);
111 /* TODO check in open or closed */
112 node = malloc(sizeof *node);
114 fprintf(stderr, "add to open %d = %p\n", HL_cantor(hex), node);
124 s->open->prev = node;
126 node->next = s->open;
133 static struct HL_astar_hex *closehex(struct HL_astar *s, struct HL_astar_hex *h) {
134 if (!s || s->error) return NULL;
141 fprintf(stderr, "Closing hex: (%d,%d)\n", h->hex.x, h->hex.y);
144 if (h->prev) h->prev->next = h->next;
145 if (h->next) h->next->prev = h->prev;
146 if (s->closed) s->closed->prev = h;
147 if (s->open == h) s->open = h->next;
155 struct HL_astar_hex *in_closed(struct HL_astar *s, struct HL_hex hex) {
156 struct HL_astar_hex *h;
159 for (h = s->closed; h; h = h->next) {
160 if (h->hex.x == hex.x && h->hex.y == hex.y) return h;
165 struct HL_astar_hex *in_open(struct HL_astar *s, struct HL_hex hex) {
166 struct HL_astar_hex *h;
169 for (h = s->open; h; h = h->next) {
170 if (h->hex.x == hex.x && h->hex.y == hex.y) return h;
175 static struct HL_astar_hex *lowrank(struct HL_astar *s) {
176 struct HL_astar_hex *n, *r;
178 if (!s || s->open == 0) return 0;
181 for(n = r; n; n = n->next) {
191 void dump_list(char *name, struct HL_astar_hex *x) {
192 fprintf(stderr, "%s list: ", name);
193 for(; x; x = x->next) {
194 fprintf(stderr, "(%d,%d)", x->hex.x, x->hex.y);
196 fprintf(stderr, "\n");
201 * find a path from start to end. we use A*
203 int HL_findpath(struct HL_astar *s, int loops) {
206 struct HL_astar_hex *node, *x, *yopen;
212 if (HL_distance(s->start, s->goal) == 0) return 0;
214 node = add_open(s, s->start);
216 node->h = s->heuristic(s->start, s->goal);
220 fprintf(stderr, "A* findpath: ");
221 fprintf(stderr, "(%d,%d)", s->start.x, s->start.y);
222 fprintf(stderr, " -> ");
223 fprintf(stderr, "(%d,%d)", s->goal.x, s->goal.y);
224 fprintf(stderr, "\n");
230 dump_list("open", s->open);
231 dump_list("closed", s->closed);
236 fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
241 if (HL_distance(x->hex, s->goal) == 0) {
247 for (dir = 0; n = s->neighbor(x->hex, dir, &y),dir<6; dir++) {
248 if (n == 0) continue; /* no hex in that direction */
249 if (in_closed(s, y)) {
253 fprintf(stderr, "checking dir %d, hex (%d, %d)\n", dir,
257 cost = x->g + s->metric(x->hex, y);
259 yopen = in_open(s, y);
261 fprintf(stderr, "checking inopen = %p\n", yopen);
264 yopen = add_open(s, y);
266 } else if (cost < yopen->g) {
274 if (x->f < yopen->parent->f) {
281 yopen->h = s->heuristic(y, s->goal);
282 yopen->f = yopen->g + yopen->h;
284 fprintf(stderr, "maybe better hex = (%d,%d) f = %f, g = %f, h = %f\n",
285 yopen->hex.x, yopen->hex.y,
286 yopen->f, yopen->g, yopen->h
294 /* error no path ? */
295 if (HL_distance(x->hex, s->goal) != 0) {
301 fprintf(stderr, "found path: ");
304 /* reconstruct path */
307 for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
309 yopen->parent->next = yopen;
311 if (HL_distance(yopen->hex, s->start) == 0) s->from = yopen;
313 fprintf(stderr, "(%p %d %d,%d)",
324 fprintf(stderr, "\n");