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->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_adjacent_hex;
  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, int hex) {
  95         struct HL_astar_hex *node;
  96 #ifdef DEBUG_ASTAR
  97                 fprintf(stderr, "add to open %d\n", 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", 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",  HL_cantor_x(h->hex), HL_cantor_y(h->hex));
 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, int 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 == hex) return h;
 161         }
 162         return NULL;
 163 }
 164
 165 struct HL_astar_hex *in_open(struct HL_astar *s, int 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 == hex) 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)",  HL_cantor_x(x->hex), HL_cantor_y(x->hex));
 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, y;
 205         struct HL_astar_hex *node, *x, *yopen;
 206         int tent_better;
 207         double cost;
 208
 209         s->error = 0;
 210
 211         if (s->start == s->goal) return 0;
 212
 213         node = add_open(s, s->start);
 214         node->g = 0.0;
 215         node->h = s->heuristic(s->start, s->goal);
 216         node->f = node->h;
 217
 218 #ifdef DEBUG_ASTAR
 219         fprintf(stderr, "A* findpath: ");
 220         fprintf(stderr, "(%d,%d)",  HL_cantor_x(s->start), HL_cantor_y(s->start));
 221         fprintf(stderr, " -> ");
 222         fprintf(stderr, "(%d,%d)",  HL_cantor_x(s->goal), HL_cantor_y(s->goal));
 223         fprintf(stderr, "\n");
 224 #endif
 225
 226         while (s->open) {
 227
 228 #ifdef DEBUG_ASTAR
 229                 dump_list("open", s->open);
 230                 dump_list("closed", s->closed);
 231 #endif
 232
 233                 x = lowrank(s);
 234 #ifdef DEBUG_ASTAR
 235                 fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
 236                         HL_cantor_x(x->hex), HL_cantor_y(x->hex),
 237                                 x->f, x->g, x->h
 238                                 );
 239 #endif
 240                 if (x->hex == s->goal) {
 241                         break;
 242                 }
 243                 closehex(s, x);
 244
 245                 for (dir = 0; y = s->neighbor(x->hex, dir),dir<6; dir++) {
 246                         if (y == 0) continue; /* no hex in that direction */
 247                         if (in_closed(s, y)) {
 248                                 continue;
 249                         }
 250 #ifdef DEBUG_ASTAR
 251                         fprintf(stderr, "checking dir %d, hex (%d, %d)\n", dir,
 252                                         HL_cantor_x(y), HL_cantor_y(y));
 253 #endif
 254
 255                         cost = x->g + s->metric(x->hex, y);
 256
 257                         yopen = in_open(s, y);
 258 #ifdef DEBUG_ASTAR
 259                         fprintf(stderr, "checking inopen = %p\n", yopen);
 260 #endif
 261                         if (!yopen) {
 262                                 yopen = add_open(s, y);
 263                                 tent_better = 1;
 264                         } else if (cost < yopen->g) {
 265                                 tent_better = 1;
 266                         } else {
 267                                 tent_better = 0;
 268                         }
 269
 270                         if (tent_better) {
 271                                 if (yopen->parent) {
 272                                         if (x->f < yopen->parent->f) {
 273                                                 yopen->parent = x;
 274                                         }
 275                                 } else {
 276                                         yopen->parent = x;
 277                                 }
 278                                 yopen->g = cost;
 279                                 yopen->h = s->heuristic(y, s->goal);
 280                                 yopen->f = yopen->g + yopen->h;
 281 #ifdef DEBUG_ASTAR
 282                 fprintf(stderr, "maybe better hex = (%d,%d) f = %f, g = %f, h = %f\n",
 283                         HL_cantor_x(yopen->hex), HL_cantor_y(yopen->hex),
 284                                 yopen->f, yopen->g, yopen->h
 285                                 );
 286 #endif
 287                         }
 288                 }
 289
 290         }
 291
 292         /* error no path ? */
 293         if (x->hex != s->goal) {
 294                 s->error = 1;
 295                 return 0;
 296         }
 297
 298 #ifdef DEBUG_ASTAR
 299         fprintf(stderr, "found path: ");
 300 #endif
 301
 302         /* reconstruct path */
 303         s->to = x;
 304         x->next = 0;
 305         for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
 306                 if (yopen->parent) {
 307                         yopen->parent->next = yopen;
 308                 }
 309                 if (yopen->hex == s->start) s->from = yopen;
 310 #ifdef DEBUG_ASTAR
 311                 fprintf(stderr, "(%p %d %d,%d)",
 312                                 yopen, yopen->hex,
 313                                 HL_cantor_x(yopen->hex),
 314                                 HL_cantor_y(yopen->hex)
 315                                 );
 316 #endif
 317                 s->pathlen++;
 318         };
 319         s->pathlen--;
 320
 321 #ifdef DEBUG_ASTAR
 322         fprintf(stderr, "\n");
 323 #endif
 324
 325         return s->pathlen;
 326 }