pd.if.org Git - hexagon/blob - astar.c

   1 #include <math.h>
   2 #include <stdio.h>
   3 #include <stdlib.h>
   4 #include <limits.h>
   5 #include <errno.h>
   6
   7 #include "hexagon.h"
   8
   9 /*
  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
  13  * the hex id is fine
  14  */
  15
  16 #if 0
  17 #define DEBUG_ASTAR 1
  18 #endif
  19
  20 static double default_metric(int a, int b) {
  21         return 1.0;
  22 }
  23
  24 static double default_heuristic(int a, int b) {
  25         return (double)HL_distance(a, b);
  26 }
  27
  28 struct HL_astar *HL_astar_init(struct HL_astar *state) {
  29         if (!state) {
  30                 state = malloc(sizeof *state);
  31                 if (!state) return NULL;
  32                 state->malloced = 1;
  33         }
  34         state->sets = 0;
  35         state->open = 0;
  36         state->closed = 0;
  37         state->nodes = 0;
  38         state->allocated = 0;
  39         state->error = 0;
  40         state->known = 0;
  41         state->index = 0;
  42         state->pathlen = 0;
  43
  44         state->metric = default_metric;
  45         state->heuristic = default_heuristic;
  46         state->neighbor = HL_adjacent_hex;
  47
  48         return state;
  49 }
  50
  51 /*
  52  * Reset a state to enpty.  Doesn't free memory, so it
  53  * can be reused.
  54  */
  55 struct HL_astar *HL_astar_clear(struct HL_astar *s) {
  56         if (!s) return NULL;
  57
  58         s->open = 0;
  59         s->closed = 0;
  60         s->nodes = 0;
  61         s->pathlen = 0;
  62
  63         return s;
  64 }
  65
  66 void HL_astar_free(struct HL_astar *s) {
  67         free(s->sets);
  68         if (s->malloced) {
  69                 free(s);
  70         }
  71 }
  72
  73 static struct HL_astar_hex *add_open(struct HL_astar *s, int hex) {
  74         struct HL_astar_hex *node;
  75
  76         if (!s || s->error) return NULL;
  77
  78         /* TODO check in open or closed */
  79
  80         if (s->nodes == s->allocated) {
  81                 int newalloc;
  82                 struct HL_astar_hex *alloc;
  83                 if (s->allocated) {
  84                         newalloc = s->allocated * 2;
  85                 } else {
  86                         newalloc = 16;
  87                 }
  88                 alloc = realloc(s->sets, newalloc * sizeof *node);
  89                 if (!alloc) {
  90                         s->error = 1;
  91                         return NULL;
  92                 }
  93                 s->sets = alloc;
  94                 s->allocated = newalloc;
  95         }
  96
  97         node = &s->sets[s->nodes++];
  98
  99         node->hex = hex;
 100         node->g = 0.0;
 101         node->f = 0.0;
 102         node->h = 0.0;
 103         node->open = 1;
 104         node->parent = 0;
 105         if (s->open) s->open->prev = node;
 106         node->next = s->open;
 107         node->prev = NULL;
 108         s->open = node;
 109
 110         return node;
 111 }
 112
 113 static struct HL_astar_hex *closehex(struct HL_astar *s, struct HL_astar_hex *h) {
 114         if (!s || s->error) return NULL;
 115         if (!h) {
 116                 s->error = 1;
 117                 return NULL;
 118         }
 119
 120 #ifdef DEBUG_ASTAR
 121         fprintf(stderr, "Closing hex: (%d,%d)\n",  HL_cantor_x(h->hex), HL_cantor_y(h->hex));
 122 #endif
 123         h->open = 0;
 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;
 128         h->next = s->closed;
 129         s->closed = h;
 130         h->prev = NULL;
 131
 132         return h;
 133 }
 134
 135 struct HL_astar_hex *in_closed(struct HL_astar *s, int hex) {
 136         struct HL_astar_hex *h;
 137         if (!s) return 0;
 138
 139         for (h = s->closed; h; h = h->next) {
 140                 if (h->hex == hex) return h;
 141         }
 142         return NULL;
 143 }
 144
 145 struct HL_astar_hex *in_open(struct HL_astar *s, int hex) {
 146         struct HL_astar_hex *h;
 147         if (!s) return 0;
 148
 149         for (h = s->open; h; h = h->next) {
 150                 if (h->hex == hex) return h;
 151         }
 152         return NULL;
 153 }
 154
 155 static struct HL_astar_hex *lowrank(struct HL_astar *s) {
 156         struct HL_astar_hex *n, *r;
 157
 158         if (!s || s->open == 0) return 0;
 159
 160         r = s->open;
 161         for(n = r; n; n = n->next) {
 162                 if (n->f < r->f) {
 163                         r = n;
 164                 }
 165         }
 166
 167         return r;
 168 }
 169
 170 #ifdef DEBUG_ASTAR
 171 void dump_list(char *name, HL_astar_hex *x) {
 172         fprintf(stderr, "%s list: ", name);
 173         for(; x; x = x->next) {
 174                 fprintf(stderr, "(%d,%d)",  HL_cantor_x(x->hex), HL_cantor_y(x->hex));
 175         }
 176         fprintf(stderr, "\n");
 177 }
 178 #endif
 179
 180 /*
 181  * find a path from start to end.  we use A*
 182  */
 183 int HL_findpath(struct HL_astar *s, int loops) {
 184         int dir, y;
 185         struct HL_astar_hex *node, *x, *yopen;
 186         int tent_better;
 187         double cost;
 188
 189         s->error = 0;
 190
 191         if (s->start == s->goal) return 0;
 192
 193         node = add_open(s, s->start);
 194         node->g = 0.0;
 195         node->h = s->heuristic(s->start, s->goal);
 196         node->f = node->h;
 197
 198 #ifdef DEBUG_ASTAR
 199         fprintf(stderr, "A* findpath: ");
 200         fprintf(stderr, "(%d,%d)",  HL_cantor_x(s->start), HL_cantor_y(s->start));
 201         fprintf(stderr, " -> ");
 202         fprintf(stderr, "(%d,%d)",  HL_cantor_x(s->goal), HL_cantor_y(s->goal));
 203         fprintf(stderr, "\n");
 204 #endif
 205
 206         while (s->open) {
 207
 208 #ifdef DEBUG_ASTAR
 209                 dump_list("open", s->open);
 210                 dump_list("closed", s->closed);
 211 #endif
 212
 213                 x = lowrank(s);
 214 #ifdef DEBUG_ASTAR
 215                 fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
 216                         HL_cantor_x(x->hex), HL_cantor_y(x->hex),
 217                                 x->f, x->g, x->h
 218                                 );
 219 #endif
 220                 if (x->hex == s->goal) {
 221                         break;
 222                 }
 223                 closehex(s, x);
 224                 for (dir = 0; y = s->neighbor(x->hex, dir),dir<6; dir++) {
 225                         if (y == 0) continue; /* no hex in that direction */
 226                         if (in_closed(s, y)) {
 227                                 continue;
 228                         }
 229
 230                         cost = x->g + s->metric(x->hex, y);
 231
 232                         yopen = in_open(s, y);
 233                         if (!yopen) {
 234                                 yopen = add_open(s, y);
 235                                 tent_better = 1;
 236                         } else if (cost < yopen->g) {
 237                                 tent_better = 1;
 238                         } else {
 239                                 tent_better = 0;
 240                         }
 241
 242                         if (tent_better) {
 243                                 if (yopen->parent) {
 244                                         if (x->f < yopen->parent->f) {
 245                                                 yopen->parent = x;
 246                                         }
 247                                 } else {
 248                                         yopen->parent = x;
 249                                 }
 250                                 yopen->g = cost;
 251                                 yopen->h = s->heuristic(y, s->goal);
 252                                 yopen->f = yopen->g + yopen->h;
 253 #ifdef DEBUG_ASTAR
 254                 fprintf(stderr, "maybe better hex = (%d,%d) f = %f, g = %f, h = %f\n",
 255                         HL_cantor_x(yopen->hex), HL_cantor_y(yopen->hex),
 256                                 yopen->f, yopen->g, yopen->h
 257                                 );
 258 #endif
 259                         }
 260                 }
 261         }
 262
 263         /* error no path ? */
 264         if (x->hex != s->goal) {
 265                 s->error = 1;
 266                 return 0;
 267         }
 268
 269 #ifdef DEBUG_ASTAR
 270         fprintf(stderr, "found path: ");
 271 #endif
 272
 273         /* reconstruct path */
 274         s->to = x;
 275         x->next = 0;
 276         for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
 277                 if (yopen->parent) {
 278                         yopen->parent->next = yopen;
 279                 }
 280                 if (yopen->hex == s->start) s->from = yopen;
 281 #ifdef DEBUG_ASTAR
 282                 fprintf(stderr, "(%p %d %d,%d)",
 283                                 yopen, yopen->hex,
 284                                 HL_cantor_x(yopen->hex),
 285                                 HL_cantor_y(yopen->hex)
 286                                 );
 287 #endif
 288                 s->pathlen++;
 289         };
 290         s->pathlen--;
 291
 292 #ifdef DEBUG_ASTAR
 293         fprintf(stderr, "\n");
 294 #endif
 295
 296         return s->pathlen;
 297 }