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

   1 #include "hexmap.h"
   2
   3 #include <math.h>
   4 #include <stdio.h>
   5 #include <stdlib.h>
   6 #include <limits.h>
   7
   8 struct astar_hex {
   9         int hex;
  10         int f, g, h;
  11         int parent;
  12 };
  13
  14 struct astar {
  15         int open, closed; /* how many in open or closed */
  16         int closedroom, openroom; /* how much is malloced */
  17         struct astar_hex *openlist;
  18         struct astar_hex *closedlist;
  19         int known, index;
  20         int error;
  21 };
  22
  23 static void astar_init(struct astar *state) {
  24         state->open = 0;
  25         state->closed = 0;
  26         state->closedroom = 0;
  27         state->openroom = 0;
  28         state->openlist = 0;
  29         state->closedlist = 0;
  30         state->error = 0;
  31         state->known = 0;
  32         state->index = 0;
  33
  34         return;
  35 }
  36
  37 static void removeopen(struct astar *s, int hex) {
  38         int index;
  39
  40         if (s->error) return;
  41         if (s->open == 0) return;
  42
  43         if (s->known != hex) {
  44                 for (index = 0; index < s->open; index++) {
  45                         if (s->openlist[index].hex == hex) break;
  46                 }
  47         } else {
  48                 index = s->index;
  49                 s->known = 0; s->index = 0;
  50         }
  51         s->openlist[index] = s->openlist[open];
  52         s->open--;
  53 }
  54
  55 static void addopen(struct astar *s, int hex) {
  56         struct astar_hex *mem;
  57         if (s->error) return;
  58
  59         if (s->open + 1 > s->openroom) {
  60                 mem = realloc(s->openlist, s->openroom * 2 * sizeof *mem);
  61                 if (mem == NULL) {
  62                         s->error = errno;
  63                         return;
  64                 }
  65                 state->openlist = mem;
  66                 s->openroom = s->openroom * 2;
  67         }
  68
  69         s->openlist[s->open].hex = hex;
  70         s->openlist[s->open].f = 0;
  71         s->openlist[s->open].g = 0;
  72         s->openlist[s->open].h = 0;
  73         s->known = hex;
  74         s->index = s->open;
  75         s->open = s->open + 1;
  76 }
  77
  78 int lowrank(struct astar *s) {
  79         int f;
  80         int hex;
  81         int i;
  82
  83         if (!s || s->open = 0) return 0;
  84
  85         hex = s->openlist[0].hex;
  86         f = s->openlist[0].f;
  87
  88         for(i=1; i < s->open; i++) {
  89                 if (s->openlist[i].f < f) {
  90                         hex = s->openlist[i].hex;
  91                         f = s->openlist[i].f;
  92                 }
  93                 /* TODO check s->openlist[i].tiebreak */
  94         }
  95
  96         return hex;
  97 }
  98
  99 static int default_metric(int a, int b) {
 100         return 1;
 101 }
 102
 103 /*
 104  * find a path from start to end.  we use A*
 105  */
 106 int HL_findpath(int start, int end, int psize, int *path,
 107                 double (*metric)(int,int)) {
 108         int neighbors[6];
 109         int count;
 110         int cur = 0;
 111         struct astar state;
 112         int neighbor;
 113
 114         if (start == end) return 0;
 115         if (!metric) metric = default_metric;
 116
 117         astar_init(&state);
 118         addopen(&state, start);
 119
 120         while (1) {
 121                 cur = lowrank(&state);
 122                 if (cur == end) {
 123                         break;
 124                 }
 125                 addclosed(&state, cur);
 126                 count = HL_hexes_at_range(cur->hex, 1, neighbors, 6);
 127                 for (i=0;i<count;i++) {
 128                         neighbor = neighbors[i];
 129                         cost = g(&state, current) + metric(current, neighbor);
 130                         if (inopen(&state, neighbor) && cost < g(&state, neighbor)) {
 131                                 removeopen(&state, neighbor);
 132                         } else
 133                         if (inclosed(&state, neighbor) && cost < g(&state, neighbor)) {
 134                                 /* should never happen with
 135                                  * admissible heuristic
 136                                  */
 137                                 removeclosed(&state, neighbor);
 138                         } else
 139                         if (!inopen(&state, neighbor) && !inclosed(&state, neighbor)) {
 140                                 addopen(&state, neighbor);
 141                                 setg(&state, neighbor, cost);
 142                                 setrank(&state, neighbor, cost + h(neighbor,end));
 143                                 setparent(&state, neighbor, current);
 144                         } else {
 145                                 abort();
 146                         }
 147                 }
 148         }
 149
 150         count = 0;
 151         while (parent(&state, cur) != start) {
 152                 count++;
 153                 cur = parent(&state, cur);
 154         }
 155         cur = end;
 156         while (parent(&state, cur) != start) {
 157                 path[count--] = cur;
 158                 cur = parent(&state, cur);
 159         }
 160
 161 }