X-Git-Url: https://pd.if.org/git/?a=blobdiff_plain;ds=sidebyside;f=lib%2Fastar.c;fp=lib%2Fastar.c;h=67889c24b0e29402054ea4aa604712e2d1c28eb1;hb=480bd9cf1c1111a9d5ae636c29df1f4a0f8d98ca;hp=0000000000000000000000000000000000000000;hpb=857d4a226763dd6adf42dbd6ca47ee9df679eaac;p=hexagon

diff --git a/lib/astar.c b/lib/astar.c
new file mode 100644
index 0000000..67889c2
--- /dev/null
+++ b/lib/astar.c
@@ -0,0 +1,328 @@
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <limits.h>
+#include <errno.h>
+
+#include "hexagon.h"
+
+/*
+ * TODO use binary heap for the openlist and closed list.
+ * open list should be a min-heap on the f value
+ * closed list is just tested for existence, so a sort on
+ * the hex id is fine
+ */
+
+#if 0
+#define DEBUG_ASTAR 1 
+#endif
+
+static double default_metric(struct HL_hex a, struct HL_hex b) {
+	return 1.0;
+}
+
+static double default_heuristic(struct HL_hex a, struct HL_hex b) {
+	return (double)HL_distance(a, b);
+}
+
+struct HL_astar *HL_astar_init(struct HL_astar *state) {
+	if (!state) {
+		state = malloc(sizeof *state);
+		if (!state) return NULL;
+		state->malloced = 1;
+	}
+	state->open = 0;
+	state->closed = 0;
+	state->nodes = 0;
+	state->allocated = 0;
+	state->error = 0;
+	state->known = 0;
+	state->index = 0;
+	state->pathlen = 0;
+
+	state->metric = default_metric;
+	state->heuristic = default_heuristic;
+	state->neighbor = HL_adjhexp;
+
+	return state;
+}
+
+/*
+ * Reset a state to enpty.
+ */
+struct HL_astar *HL_astar_clear(struct HL_astar *s) {
+	struct HL_astar_hex *hex;
+	if (!s) return NULL;
+	while (s->open) {
+		hex = s->open->next;
+		free(s->open);
+		s->open = hex;
+	}
+	while (s->closed) {
+		hex = s->closed->next;
+		free(s->closed);
+		s->closed = hex;
+	}
+
+	s->open = 0;
+	s->closed = 0;
+	s->nodes = 0;
+	s->pathlen = 0;
+
+	return s;
+}
+
+void HL_astar_free(struct HL_astar *s) {
+	struct HL_astar_hex *hex;
+	/* free open and closed lists */
+	while (s->open) {
+		hex = s->open->next;
+		free(s->open);
+		s->open = hex;
+	}
+	while (s->closed) {
+		hex = s->closed->next;
+		free(s->closed);
+		s->closed = hex;
+	}
+
+	if (s->malloced) {
+		free(s);
+	}
+}
+
+static struct HL_astar_hex *add_open(struct HL_astar *s, struct HL_hex hex) {
+	struct HL_astar_hex *node;
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "add to open %d\n", HL_cantor(hex));
+#endif
+
+	if (!s || s->error) {
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "returning null\n");
+#endif
+
+		return NULL;
+	}
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "mallocing node(%zd)\n", sizeof *node);
+#endif
+
+	/* TODO check in open or closed */
+	node = malloc(sizeof *node);
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "add to open %d = %p\n", HL_cantor(hex), node);
+#endif
+
+	node->hex = hex;
+	node->g = 0.0;
+	node->f = 0.0;
+	node->h = 0.0;
+	node->open = 1;
+	node->parent = 0;
+	if (s->open) {
+		s->open->prev = node;
+	}
+	node->next = s->open;
+	node->prev = NULL;
+	s->open = node;
+
+	return node;
+}
+
+static struct HL_astar_hex *closehex(struct HL_astar *s, struct HL_astar_hex *h) {
+	if (!s || s->error) return NULL;
+	if (!h) {
+		s->error = 1;
+		return NULL;
+	}
+
+#ifdef DEBUG_ASTAR
+	fprintf(stderr, "Closing hex: (%d,%d)\n",  h->hex.x, h->hex.y);
+#endif
+	h->open = 0;
+	if (h->prev) h->prev->next = h->next;
+	if (h->next) h->next->prev = h->prev;
+	if (s->closed) s->closed->prev = h;
+	if (s->open == h) s->open = h->next;
+	h->next = s->closed;
+	s->closed = h;
+	h->prev = NULL;
+
+	return h;
+}
+
+struct HL_astar_hex *in_closed(struct HL_astar *s, struct HL_hex hex) {
+	struct HL_astar_hex *h;
+	if (!s) return 0;
+
+	for (h = s->closed; h; h = h->next) {
+		if (h->hex.x == hex.x && h->hex.y == hex.y) return h;
+	}
+	return NULL;
+}
+
+struct HL_astar_hex *in_open(struct HL_astar *s, struct HL_hex hex) {
+	struct HL_astar_hex *h;
+	if (!s) return 0;
+
+	for (h = s->open; h; h = h->next) {
+		if (h->hex.x == hex.x && h->hex.y == hex.y) return h;
+	}
+	return NULL;
+}
+
+static struct HL_astar_hex *lowrank(struct HL_astar *s) {
+	struct HL_astar_hex *n, *r;
+
+	if (!s || s->open == 0) return 0;
+
+	r = s->open;
+	for(n = r; n; n = n->next) {
+		if (n->f < r->f) {
+			r = n;
+		}
+	}
+
+	return r;
+}
+
+#ifdef DEBUG_ASTAR
+void dump_list(char *name, struct HL_astar_hex *x) {
+	fprintf(stderr, "%s list: ", name);
+	for(; x; x = x->next) {
+		fprintf(stderr, "(%d,%d)",  x->hex.x, x->hex.y);
+	}
+	fprintf(stderr, "\n");
+}
+#endif
+
+/*
+ * find a path from start to end.  we use A*
+ */
+int HL_findpath(struct HL_astar *s, int loops) {
+	int dir;
+	struct HL_hex y;
+	struct HL_astar_hex *node, *x, *yopen;
+	int tent_better;
+	double cost;
+
+	s->error = 0;
+
+	if (HL_distance(s->start, s->goal) == 0) return 0;
+
+	node = add_open(s, s->start);
+	node->g = 0.0;
+	node->h = s->heuristic(s->start, s->goal);
+	node->f = node->h;
+
+#ifdef DEBUG_ASTAR
+	fprintf(stderr, "A* findpath: ");
+	fprintf(stderr, "(%d,%d)",  s->start.x, s->start.y);
+	fprintf(stderr, " -> ");
+	fprintf(stderr, "(%d,%d)",  s->goal.x, s->goal.y);
+	fprintf(stderr, "\n");
+#endif
+
+	while (s->open) {
+
+#ifdef DEBUG_ASTAR
+		dump_list("open", s->open);
+		dump_list("closed", s->closed);
+#endif
+
+		x = lowrank(s);
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "checking lowest f hex = (%d,%d) f = %f, g = %f, h = %f\n",
+			x->hex.x, x->hex.y,
+				x->f, x->g, x->h
+				);
+#endif
+		if (HL_distance(x->hex, s->goal) == 0) {
+			break;
+		}
+		closehex(s, x);
+
+		int n;
+		for (dir = 0; n = s->neighbor(x->hex, dir, &y),dir<6; dir++) {
+			if (n == 0) continue; /* no hex in that direction */
+			if (in_closed(s, y)) {
+				continue;
+			}
+#ifdef DEBUG_ASTAR
+			fprintf(stderr, "checking dir %d, hex (%d, %d)\n", dir,
+					y.x, y.y);
+#endif
+
+			cost = x->g + s->metric(x->hex, y);
+
+			yopen = in_open(s, y);
+#ifdef DEBUG_ASTAR
+			fprintf(stderr, "checking inopen = %p\n", yopen);
+#endif
+			if (!yopen) {
+				yopen = add_open(s, y);
+				tent_better = 1;
+			} else if (cost < yopen->g) {
+				tent_better = 1;
+			} else {
+				tent_better = 0;
+			}
+
+			if (tent_better) {
+				if (yopen->parent) {
+					if (x->f < yopen->parent->f) {
+						yopen->parent = x;
+					}
+				} else {
+					yopen->parent = x;
+				}
+				yopen->g = cost;
+				yopen->h = s->heuristic(y, s->goal);
+				yopen->f = yopen->g + yopen->h;
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "maybe better hex = (%d,%d) f = %f, g = %f, h = %f\n",
+			yopen->hex.x, yopen->hex.y,
+				yopen->f, yopen->g, yopen->h
+				);
+#endif
+			}
+		}
+
+	}
+
+	/* error no path ? */
+	if (HL_distance(x->hex, s->goal) != 0) {
+		s->error = 1;
+		return 0;
+	}
+
+#ifdef DEBUG_ASTAR
+	fprintf(stderr, "found path: ");
+#endif
+
+	/* reconstruct path */
+	s->to = x;
+	x->next = 0;
+	for (s->pathlen = 0, yopen = x; yopen; yopen = yopen->parent) {
+		if (yopen->parent) {
+			yopen->parent->next = yopen;
+		}
+		if (HL_distance(yopen->hex, s->start) == 0) s->from = yopen;
+#ifdef DEBUG_ASTAR
+		fprintf(stderr, "(%p %d %d,%d)",
+				yopen, yopen->hex,
+				yopen->hex.x,
+				yopen->hex.y
+				);
+#endif
+		s->pathlen++;
+	};
+	s->pathlen--;
+
+#ifdef DEBUG_ASTAR
+	fprintf(stderr, "\n");
+#endif
+
+	return s->pathlen;
+}