+++ /dev/null
-#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;
-}
projects / hexagon / blobdiff