pd.if.org Git - hexagon/blob - lib/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(struct HL_hex a, struct HL_hex b) {
  21         return 1.0;
  22 }
  23
  24 static double default_heuristic(struct HL_hex a, struct HL_hex 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->open = 0;
  35         state->closed = 0;
  36         state->nodes = 0;
  37         state->allocated = 0;
  38         state->error = 0;
  39         state->known = 0;
  40         state->index = 0;
  41         state->pathlen = 0;
  42
  43         state->metric = default_metric;
  44         state->heuristic = default_heuristic;
  45         state->neighbor = HL_adjhexp;
  46
  47         return state;
  48 }
  49
  50 /*
  51  * Reset a state to enpty.
  52  */
  53 struct HL_astar *HL_astar_clear(struct HL_astar *s) {
  54         struct HL_astar_hex *hex;
  55         if (!s) return NULL;
  56         while (s->open) {
  57                 hex = s->open->next;
  58                 free(s->open);
  59                 s->open = hex;
  60         }
  61         while (s->closed) {
  62                 hex = s->closed->next;
  63                 free(s->closed);
  64                 s->closed = hex;
  65         }
  66
  67         s->open = 0;
  68         s->closed = 0;
  69         s->nodes = 0;
  70         s->pathlen = 0;
  71
  72         return s;
  73 }
  74
  75 void HL_astar_free(struct HL_astar *s) {
  76         struct HL_astar_hex *hex;
  77         /* free open and closed lists */
  78         while (s->open) {
  79                 hex = s->open->next;
  80                 free(s->open);
  81                 s->open = hex;
  82         }
  83         while (s->closed) {
  84                 hex = s->closed->next;
  85                 free(s->closed);
  86                 s->closed = hex;
  87         }
  88
  89         if (s->malloced) {
  90                 free(s);
  91         }
  92 }
  93
  94 static struct HL_astar_hex *add_open(struct HL_astar *s, struct HL_hex hex) {
  95         struct HL_astar_hex *node;
  96 #ifdef DEBUG_ASTAR
  97                 fprintf(stderr, "add to open %d\n", HL_cantor(hex));
  98 #endif
  99
 100         if (!s || s->error) {
 101 #ifdef DEBUG_ASTAR
 102                 fprintf(stderr, "returning null\n");
 103 #endif
 104
 105                 return NULL;
 106         }
 107 #ifdef DEBUG_ASTAR
 108                 fprintf(stderr, "mallocing node(%zd)\n", sizeof *node);
 109 #endif
 110
 111         /* TODO check in open or closed */
 112         node = malloc(sizeof *node);
 113 #ifdef DEBUG_ASTAR
 114                 fprintf(stderr, "add to open %d = %p\n", HL_cantor(hex), node);
 115 #endif
 116
 117         node->hex = hex;
 118         node->g = 0.0;
 119         node->f = 0.0;
 120         node->h = 0.0;
 121         node->open = 1;
 122         node->parent = 0;
 123         if (s->open) {
 124                 s->open->prev = node;
 125         }
 126         node->next = s->open;
 127         node->prev = NULL;
 128         s->open = node;
 129
 130         return node;
 131 }
 132
 133 static struct HL_astar_hex *closehex(struct HL_astar *s, struct HL_astar_hex *h) {
 134         if (!s || s->error) return NULL;
 135         if (!h) {
 136                 s->error = 1;
 137                 return NULL;
 138         }
 139
 140 #ifdef DEBUG_ASTAR
 141         fprintf(stderr, "Closing hex: (%d,%d)\n",  h->hex.x, h->hex.y);
 142 #endif
 143         h->open = 0;
 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;
 148         h->next = s->closed;
 149         s->closed = h;
 150         h->prev = NULL;
 151
 152         return h;
 153 }
 154
 155 struct HL_astar_hex *in_closed(struct HL_astar *s, struct HL_hex hex) {
 156         struct HL_astar_hex *h;
 157         if (!s) return 0;
 158
 159         for (h = s->closed; h; h = h->next) {
 160                 if (h->hex.x == hex.x && h->hex.y == hex.y) return h;
 161         }
 162         return NULL;
 163 }
 164
 165 struct HL_astar_hex *in_open(struct HL_astar *s, struct HL_hex hex) {
 166         struct HL_astar_hex *h;
 167         if (!s) return 0;
 168
 169         for (h = s->open; h; h = h->next) {
 170                 if (h->hex.x == hex.x && h->hex.y == hex.y) return h;
 171         }
 172         return NULL;
 173 }
 174
 175 static struct HL_astar_hex *lowrank(struct HL_astar *s) {
 176         struct HL_astar_hex *n, *r;
 177
 178         if (!s || s->open == 0) return 0;
 179
 180         r = s->open;
 181         for(n = r; n; n = n->next) {
 182                 if (n->f < r->f) {
 183                         r = n;
 184                 }
 185         }
 186
 187         return r;
 188 }
 189
 190 #ifdef DEBUG_ASTAR
 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);
 195         }
 196         fprintf(stderr, "\n");
 197 }
 198 #endif
 199
 200 /*
 201  * find a path from start to end.  we use A*
 202  */
 203 int HL_findpath(struct HL_astar *s, int loops) {
 204         int dir;
 205         struct HL_hex y;
 206         struct HL_astar_hex *node, *x, *yopen;
 207         int tent_better;
 208         double cost;
 209
 210         s->error = 0;
 211
 212         if (HL_distance(s->start, s->goal) == 0) return 0;
 213
 214         node = add_open(s, s->start);
 215         node->g = 0.0;
 216         node->h = s->heuristic(s->start, s->goal);
 217         node->f = node->h;
 218
 219 #ifdef DEBUG_ASTAR
 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");
 225 #endif
 226
 227         while (s->open) {
 228
 229 #ifdef DEBUG_ASTAR
 230                 dump_list("open", s->open);
 231                 dump_list("closed", s->closed);
 232 #endif
 233
 234                 x = lowrank(s);
 235 #ifdef DEBUG_ASTAR
 236                 fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
 237                         x->hex.x, x->hex.y,
 238                                 x->f, x->g, x->h
 239                                 );
 240 #endif
 241                 if (HL_distance(x->hex, s->goal) == 0) {
 242                         break;
 243                 }
 244                 closehex(s, x);
 245
 246                 int n;
 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)) {
 250                                 continue;
 251                         }
 252 #ifdef DEBUG_ASTAR
 253                         fprintf(stderr, "checking dir %d, hex (%d, %d)\n", dir,
 254                                         y.x, y.y);
 255 #endif
 256
 257                         cost = x->g + s->metric(x->hex, y);
 258
 259                         yopen = in_open(s, y);
 260 #ifdef DEBUG_ASTAR
 261                         fprintf(stderr, "checking inopen = %p\n", yopen);
 262 #endif
 263                         if (!yopen) {
 264                                 yopen = add_open(s, y);
 265                                 tent_better = 1;
 266                         } else if (cost < yopen->g) {
 267                                 tent_better = 1;
 268                         } else {
 269                                 tent_better = 0;
 270                         }
 271
 272                         if (tent_better) {
 273                                 if (yopen->parent) {
 274                                         if (x->f < yopen->parent->f) {
 275                                                 yopen->parent = x;
 276                                         }
 277                                 } else {
 278                                         yopen->parent = x;
 279                                 }
 280                                 yopen->g = cost;
 281                                 yopen->h = s->heuristic(y, s->goal);
 282                                 yopen->f = yopen->g + yopen->h;
 283 #ifdef DEBUG_ASTAR
 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
 287                                 );
 288 #endif
 289                         }
 290                 }
 291
 292         }
 293
 294         /* error no path ? */
 295         if (HL_distance(x->hex, s->goal) != 0) {
 296                 s->error = 1;
 297                 return 0;
 298         }
 299
 300 #ifdef DEBUG_ASTAR
 301         fprintf(stderr, "found path: ");
 302 #endif
 303
 304         /* reconstruct path */
 305         s->to = x;
 306         x->next = 0;
 307         for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
 308                 if (yopen->parent) {
 309                         yopen->parent->next = yopen;
 310                 }
 311                 if (HL_distance(yopen->hex, s->start) == 0) s->from = yopen;
 312 #ifdef DEBUG_ASTAR
 313                 fprintf(stderr, "(%p %d %d,%d)",
 314                                 yopen, yopen->hex,
 315                                 yopen->hex.x,
 316                                 yopen->hex.y
 317                                 );
 318 #endif
 319                 s->pathlen++;
 320         };
 321         s->pathlen--;
 322
 323 #ifdef DEBUG_ASTAR
 324         fprintf(stderr, "\n");
 325 #endif
 326
 327         return s->pathlen;
 328 }