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 /*
 171  * find a path from start to end.  we use A*
 172  */
 173 int HL_findpath(struct HL_astar *s, int loops) {
 174         int dir, y;
 175         struct HL_astar_hex *node, *x, *yopen;
 176         int tent_better;
 177         double cost;
 178
 179         s->error = 0;
 180
 181         if (s->start == s->goal) return 0;
 182
 183         node = add_open(s, s->start);
 184         node->g = 0.0;
 185         node->h = s->heuristic(s->start, s->goal);
 186         node->f = node->h;
 187
 188 #ifdef DEBUG_ASTAR
 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");
 194 #endif
 195
 196         while (s->open) {
 197
 198 #ifdef DEBUG_ASTAR
 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));
 202         }
 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));
 207         }
 208         fprintf(stderr, "\n");
 209 #endif
 210
 211                 x = lowrank(s);
 212 #ifdef DEBUG_ASTAR
 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),
 215                                 x->f, x->g, x->h
 216                                 );
 217 #endif
 218                 if (x->hex == s->goal) {
 219                         break;
 220                 }
 221                 closehex(s, x);
 222                 for (dir = 0; y = s->neighbor(x->hex, dir); dir++) {
 223                         if (in_closed(s, y)) {
 224                                 continue;
 225                         }
 226
 227                         cost = x->g + s->metric(x->hex, y);
 228
 229                         yopen = in_open(s, y);
 230                         if (! yopen) {
 231                                 yopen = add_open(s, y);
 232                                 tent_better = 1;
 233                         } else if (cost < yopen->g) {
 234                                 tent_better = 1;
 235                         } else {
 236                                 tent_better = 0;
 237                         }
 238
 239                         if (tent_better) {
 240                                 if (yopen->parent) {
 241                                         if (x->f < yopen->parent->f) {
 242                                                 yopen->parent = x;
 243                                         }
 244                                 } else {
 245                                         yopen->parent = x;
 246                                 }
 247                                 yopen->g = cost;
 248                                 yopen->h = s->heuristic(y, s->goal);
 249                                 yopen->f = yopen->g + yopen->h;
 250 #ifdef DEBUG_ASTAR
 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
 254                                 );
 255 #endif
 256                         }
 257                 }
 258         }
 259
 260         /* error no path ? */
 261         if (x->hex != s->goal) {
 262                 s->error = 1;
 263                 return 0;
 264         }
 265
 266 #ifdef DEBUG_ASTAR
 267         fprintf(stderr, "found path: ");
 268 #endif
 269
 270         /* reconstruct path */
 271         s->to = x;
 272         x->next = 0;
 273         for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
 274                 if (yopen->parent) {
 275                         yopen->parent->next = yopen;
 276                 }
 277                 if (yopen->hex == s->start) s->from = yopen;
 278 #ifdef DEBUG_ASTAR
 279                 fprintf(stderr, "(%p %d %d,%d)",
 280                                 yopen, yopen->hex,
 281                                 HL_cantor_x(yopen->hex),
 282                                 HL_cantor_y(yopen->hex)
 283                                 );
 284 #endif
 285                 s->pathlen++;
 286         };
 287         s->pathlen--;
 288
 289 #ifdef DEBUG_ASTAR
 290         fprintf(stderr, "\n");
 291 #endif
 292
 293         return s->pathlen;
 294 }