--- /dev/null
+#include "hexmap.h"
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <limits.h>
+
+struct astar_hex {
+ int hex;
+ int f, g, h;
+ int parent;
+};
+
+struct astar {
+ int open, closed; /* how many in open or closed */
+ int closedroom, openroom; /* how much is malloced */
+ struct astar_hex *openlist;
+ struct astar_hex *closedlist;
+ int known, index;
+ int error;
+};
+
+static void astar_init(struct astar *state) {
+ state->open = 0;
+ state->closed = 0;
+ state->closedroom = 0;
+ state->openroom = 0;
+ state->openlist = 0;
+ state->closedlist = 0;
+ state->error = 0;
+ state->known = 0;
+ state->index = 0;
+
+ return;
+}
+
+static void removeopen(struct astar *s, int hex) {
+ int index;
+
+ if (s->error) return;
+ if (s->open == 0) return;
+
+ if (s->known != hex) {
+ for (index = 0; index < s->open; index++) {
+ if (s->openlist[index].hex == hex) break;
+ }
+ } else {
+ index = s->index;
+ s->known = 0; s->index = 0;
+ }
+ s->openlist[index] = s->openlist[open];
+ s->open--;
+}
+
+static void addopen(struct astar *s, int hex) {
+ struct astar_hex *mem;
+ if (s->error) return;
+
+ if (s->open + 1 > s->openroom) {
+ mem = realloc(s->openlist, s->openroom * 2 * sizeof *mem);
+ if (mem == NULL) {
+ s->error = errno;
+ return;
+ }
+ state->openlist = mem;
+ s->openroom = s->openroom * 2;
+ }
+
+ s->openlist[s->open].hex = hex;
+ s->openlist[s->open].f = 0;
+ s->openlist[s->open].g = 0;
+ s->openlist[s->open].h = 0;
+ s->known = hex;
+ s->index = s->open;
+ s->open = s->open + 1;
+}
+
+int lowrank(struct astar *s) {
+ int f;
+ int hex;
+ int i;
+
+ if (!s || s->open = 0) return 0;
+
+ hex = s->openlist[0].hex;
+ f = s->openlist[0].f;
+
+ for(i=1; i < s->open; i++) {
+ if (s->openlist[i].f < f) {
+ hex = s->openlist[i].hex;
+ f = s->openlist[i].f;
+ }
+ /* TODO check s->openlist[i].tiebreak */
+ }
+
+ return hex;
+}
+
+static int default_metric(int a, int b) {
+ return 1;
+}
+
+/*
+ * find a path from start to end. we use A*
+ */
+int HL_findpath(int start, int end, int psize, int *path,
+ double (*metric)(int,int)) {
+ int neighbors[6];
+ int count;
+ int cur = 0;
+ struct astar state;
+ int neighbor;
+
+ if (start == end) return 0;
+ if (!metric) metric = default_metric;
+
+ astar_init(&state);
+ addopen(&state, start);
+
+ while (1) {
+ cur = lowrank(&state);
+ if (cur == end) {
+ break;
+ }
+ addclosed(&state, cur);
+ count = HL_hexes_at_range(cur->hex, 1, neighbors, 6);
+ for (i=0;i<count;i++) {
+ neighbor = neighbors[i];
+ cost = g(&state, current) + metric(current, neighbor);
+ if (inopen(&state, neighbor) && cost < g(&state, neighbor)) {
+ removeopen(&state, neighbor);
+ } else
+ if (inclosed(&state, neighbor) && cost < g(&state, neighbor)) {
+ /* should never happen with
+ * admissible heuristic
+ */
+ removeclosed(&state, neighbor);
+ } else
+ if (!inopen(&state, neighbor) && !inclosed(&state, neighbor)) {
+ addopen(&state, neighbor);
+ setg(&state, neighbor, cost);
+ setrank(&state, neighbor, cost + h(neighbor,end));
+ setparent(&state, neighbor, current);
+ } else {
+ abort();
+ }
+ }
+ }
+
+ count = 0;
+ while (parent(&state, cur) != start) {
+ count++;
+ cur = parent(&state, cur);
+ }
+ cur = end;
+ while (parent(&state, cur) != start) {
+ path[count--] = cur;
+ cur = parent(&state, cur);
+ }
+
+}