A* is only usable for a single path, except with a heuristic function
returning always zero, which turns it into Dijkstra's algorithm.
Distribution uses it that way, because it needs to find multiple paths
from the same source as efficiently as possible. Only the other uses
of path search can profit from A*'s superior efficiency.
I feel the extra complexity is not justified. Besides, it slows down
distribution path assembly a bit, which is the only case where
efficiency really matters. Let's stick to Dijkstra's for now.
#include "file.h"
#include "nat.h"
#include "optlist.h"
#include "file.h"
#include "nat.h"
#include "optlist.h"
#include "path.h"
#include "sect.h"
#include "path.h"
#include "sect.h"
static char *bufrotate(char *buf, size_t bufsz, size_t i);
/*
static char *bufrotate(char *buf, size_t bufsz, size_t i);
/*
- * Dijkstra's algorithm. Refer to your graph algorithm textbook for
- * how it works. Implementation is specialized to hex maps.
+ * A* algorithm. Refer to your graph algorithm textbook for how it
+ * works. Implementation is specialized to hex maps.
*
* Define PATH_FIND_STATS for performance statistics on stdout.
*/
*
* Define PATH_FIND_STATS for performance statistics on stdout.
*/
struct pf_heap {
int uid; /* sector uid and */
coord x, y; /* coordinates, uid == XYOFFSET(x, y) */
struct pf_heap {
int uid; /* sector uid and */
coord x, y; /* coordinates, uid == XYOFFSET(x, y) */
- double cost; /* cost from source */
+ double f; /* estimated cost from source to dest */
};
static int pf_nheap; /* #entries in pf_nheap[] */
};
static int pf_nheap; /* #entries in pf_nheap[] */
static int pf_suid;
static natid pf_actor;
static double (*pf_sct_cost)(natid, int);
static int pf_suid;
static natid pf_actor;
static double (*pf_sct_cost)(natid, int);
+static double (*pf_h)(coord, coord, coord, coord, double);
/*
* Performance statistics
*/
#ifdef PATH_FIND_STATS
/*
* Performance statistics
*/
#ifdef PATH_FIND_STATS
-static unsigned pf_nsearch, pf_nsource, pf_nopen, pf_nclose;
+static unsigned pf_nsearch, pf_nsource, pf_nopen, pf_nreopen, pf_nclose;
static unsigned pf_nheap_max, pf_noway;
static double pf_sumcost;
#define STAT_INC(v) ((void)((v)++))
static unsigned pf_nheap_max, pf_noway;
static double pf_sumcost;
#define STAT_INC(v) ((void)((v)++))
#define STAT_HIMARK(v, h) ((void)0)
#endif /* !PATH_FIND_STATS */
#define STAT_HIMARK(v, h) ((void)0)
#endif /* !PATH_FIND_STATS */
-#ifndef NDEBUG /* silence "not used" warning */
/* Is sector with uid UID open? */
static int
pf_is_open(int uid)
{
return pf_map[uid].visit == pf_visit;
}
/* Is sector with uid UID open? */
static int
pf_is_open(int uid)
{
return pf_map[uid].visit == pf_visit;
}
/* Is sector with uid UID closed? */
static int
/* Is sector with uid UID closed? */
static int
assert(0 <= uid && uid < WORLD_SZ());
assert(pf_map[uid].heapi == i);
assert(pf_map[uid].visit == pf_visit);
assert(0 <= uid && uid < WORLD_SZ());
assert(pf_map[uid].heapi == i);
assert(pf_map[uid].visit == pf_visit);
- assert(pf_map[uid].cost <= pf_heap[i].cost);
+ assert(pf_map[uid].cost <= pf_heap[i].f);
- assert(c >= pf_nheap || pf_heap[i].cost <= pf_heap[c].cost);
+ assert(c >= pf_nheap || pf_heap[i].f <= pf_heap[c].f);
- assert(c >= pf_nheap || pf_heap[i].cost <= pf_heap[c].cost);
+ assert(c >= pf_nheap || pf_heap[i].f <= pf_heap[c].f);
}
for (uid = 0; uid < WORLD_SZ(); uid++) {
}
for (uid = 0; uid < WORLD_SZ(); uid++) {
assert(0 <= n && n < pf_nheap);
for (r = n; (c = 2 * r + 1) < pf_nheap; r = c) {
assert(0 <= n && n < pf_nheap);
for (r = n; (c = 2 * r + 1) < pf_nheap; r = c) {
- if (c + 1 < pf_nheap && pf_heap[c].cost > pf_heap[c + 1].cost)
+ if (c + 1 < pf_nheap && pf_heap[c].f > pf_heap[c + 1].f)
- if (pf_heap[r].cost < pf_heap[c].cost)
+ if (pf_heap[r].f < pf_heap[c].f)
break;
pf_heap_swap(r, c);
}
break;
pf_heap_swap(r, c);
}
assert(0 <= n && n < pf_nheap);
for (c = n; (p = (c - 1) / 2), c > 0; c = p) {
assert(0 <= n && n < pf_nheap);
for (c = n; (p = (c - 1) / 2), c > 0; c = p) {
- if (pf_heap[p].cost < pf_heap[c].cost)
+ if (pf_heap[p].f < pf_heap[c].f)
break;
pf_heap_swap(p, c);
}
break;
pf_heap_swap(p, c);
}
* Open the unvisited sector X,Y.
* UID is sector uid, it equals XYOFFSET(X,Y).
* Cheapest path from source comes from direction DIR and has cost COST.
* Open the unvisited sector X,Y.
* UID is sector uid, it equals XYOFFSET(X,Y).
* Cheapest path from source comes from direction DIR and has cost COST.
+ * H is the estimated cost from X,Y to the destination.
-pf_open(int uid, coord x, coord y, int dir, double cost)
+pf_open(int uid, coord x, coord y, int dir, double cost, double h)
- STAT_INC(pf_nopen);
- i = pf_nheap++;
- STAT_HIMARK(pf_nheap_max, (unsigned)pf_nheap);
- DPRINTF("pf: open %d,%d %g %c %d\n", x, y, cost, dirch[dir], i);
- assert(pf_is_unvisited(uid));
- pf_map[uid].visit = pf_visit;
+ if (pf_is_open(uid)) {
+ STAT_INC(pf_nreopen);
+ i = pf_map[uid].heapi;
+ DPRINTF("pf: reopen %d,%d %g %c %d\n", x, y, cost, dirch[dir], i);
+ } else {
+ assert(pf_is_unvisited(uid));
+ STAT_INC(pf_nopen);
+ i = pf_nheap++;
+ STAT_HIMARK(pf_nheap_max, (unsigned)pf_nheap);
+ DPRINTF("pf: open %d,%d %g %c %d\n", x, y, cost, dirch[dir], i);
+ pf_map[uid].heapi = i;
+ pf_map[uid].visit = pf_visit;
+ pf_heap[i].uid = uid;
+ pf_heap[i].x = x;
+ pf_heap[i].y = y;
+ }
- pf_heap[i].uid = uid;
- pf_heap[i].x = x;
- pf_heap[i].y = y;
- pf_heap[i].cost = cost;
+ pf_heap[i].f = cost + h;
pf_sift_up(i);
pf_check();
pf_sift_up(i);
pf_check();
#ifdef PATH_FIND_DEBUG
/*
* Return cost from source to sector with uid UID.
#ifdef PATH_FIND_DEBUG
/*
* Return cost from source to sector with uid UID.
- * It must be visited, i.e. open or closed.
+ * It must be open (cost is an estimate) or closed (cost is exact).
*/
static double
pf_cost(int uid)
*/
static double
pf_cost(int uid)
* SX,SY is the source.
* The cost to enter the sector with uid u is COST(ACTOR, u).
* Negative value means the sector can't be entered.
* SX,SY is the source.
* The cost to enter the sector with uid u is COST(ACTOR, u).
* Negative value means the sector can't be entered.
+ * Optional H() is the heuristic: the cost from x,y to the destination
+ * dx,dy is at least H(x, y, dx, dy, c1d), where c1d is the cost to
+ * enter dx,dy. The closer H() is to the true cost, the more
+ * efficient this function works.
+ * With a null H(), A* degenerates into Dijkstra's algorithm. You can
+ * then call path_find_to() multiple times for the same source. This
+ * is faster when you need many destinations.
-pf_set_source(coord sx, coord sy, natid actor, double (*cost)(natid, int))
+pf_set_source(coord sx, coord sy, natid actor, double (*cost) (natid, int),
+ double (*h) (coord, coord, coord, coord, double))
{
STAT_INC(pf_nsource);
DPRINTF("pf: source %d,%d\n", sx, sy);
{
STAT_INC(pf_nsource);
DPRINTF("pf: source %d,%d\n", sx, sy);
pf_suid = XYOFFSET(sx, sy);
pf_actor = actor;
pf_sct_cost = cost;
pf_suid = XYOFFSET(sx, sy);
pf_actor = actor;
pf_sct_cost = cost;
if (!pf_map) {
pf_map = calloc(WORLD_SZ(), sizeof(*pf_map));
if (!pf_map) {
pf_map = calloc(WORLD_SZ(), sizeof(*pf_map));
pf_visit += 2;
pf_nheap = 0;
pf_visit += 2;
pf_nheap = 0;
-
- pf_open(pf_suid, pf_sx, pf_sy, DIR_STOP, 0.0);
{
int duid;
int uid, nuid, i;
{
int duid;
int uid, nuid, i;
coord x, y, nx, ny;
STAT_INC(pf_nsearch);
coord x, y, nx, ny;
STAT_INC(pf_nsearch);
return pf_map[duid].cost;
}
return pf_map[duid].cost;
}
+ c1d = pf_sct_cost(pf_actor, duid);
+ if (c1d < 0) {
+ DPRINTF("pf: done new %g\n", -1.0);
+ STAT_INC(pf_noway);
+ return -1;
+ }
+
+ if (pf_is_unvisited(pf_suid))
+ /* first search from this source */
+ pf_open(pf_suid, pf_sx, pf_sy, DIR_STOP, 0.0,
+ pf_h ? pf_h(pf_sx, pf_sy, dx, dy, c1d) : 0.0);
+ else
+ assert(!pf_h); /* multiple searches only w/o heuristic */
+
while (pf_nheap > 0 && (uid = pf_heap[0].uid) != duid) {
x = pf_heap[0].x;
y = pf_heap[0].y;
while (pf_nheap > 0 && (uid = pf_heap[0].uid) != duid) {
x = pf_heap[0].x;
y = pf_heap[0].y;
- cost = pf_heap[0].cost;
+ cost = pf_map[uid].cost;
for (i = 0; i < 6; i++) { /* for all neighbors */
nx = x_in_dir(x, DIR_FIRST + i);
ny = y_in_dir(y, DIR_FIRST + i);
nuid = XYOFFSET(nx, ny);
for (i = 0; i < 6; i++) { /* for all neighbors */
nx = x_in_dir(x, DIR_FIRST + i);
ny = y_in_dir(y, DIR_FIRST + i);
nuid = XYOFFSET(nx, ny);
- /*
- * Cost to enter NX,NY doesn't depend on direction of
- * entry. This X,Y is at least as expensive as any
- * previous one. Therefore, cost to go to NX,NY via X,Y
- * is at least as high as any previously found route.
- * Skip neighbors that have a route already.
- */
- if (!pf_is_unvisited(nuid))
+ if (pf_h ? pf_is_closed(nuid) : !pf_is_unvisited(nuid))
continue;
c1 = pf_sct_cost(pf_actor, nuid);
if (c1 < 0)
continue;
continue;
c1 = pf_sct_cost(pf_actor, nuid);
if (c1 < 0)
continue;
- pf_open(nuid, nx, ny, DIR_FIRST + i, cost + c1);
+ if (pf_is_unvisited(nuid) || cost + c1 < pf_map[nuid].cost)
+ pf_open(nuid, nx, ny, DIR_FIRST + i, cost + c1,
+ pf_h ? pf_h(nx, ny, dx, dy, c1d) : 0);
void
path_find_print_stats(void)
{
void
path_find_print_stats(void)
{
- printf("pathfind %u searches, %u sources, %u opened, %u closed,"
+ printf("pathfind %u searches, %u sources,"
+ " %u opened, %u reopened, %u closed,"
" %u heap max, %zu bytes, %u noway, %g avg cost\n",
" %u heap max, %zu bytes, %u noway, %g avg cost\n",
- pf_nsearch, pf_nsource, pf_nopen, pf_nclose,
+ pf_nsearch, pf_nsource, pf_nopen, pf_nreopen, pf_nclose,
pf_nheap_max,
(WORLD_SZ() * (sizeof(*pf_map) + sizeof(*pf_heap))),
pf_noway, pf_nsearch ? pf_sumcost / pf_nsearch : 0.0);
pf_nheap_max,
(WORLD_SZ() * (sizeof(*pf_map) + sizeof(*pf_heap))),
pf_noway, pf_nsearch ? pf_sumcost / pf_nsearch : 0.0);
return cost_land(actor, uid, MOB_RAIL);
}
return cost_land(actor, uid, MOB_RAIL);
}
+static double
+h_move(coord x, coord y, coord dx, coord dy, double c1d)
+{
+ int md = mapdist(x, y, dx, dy);
+ return md ? c1d + (md - 1) * 0.001 : 0.0;
+}
+
+static double
+h_march_rail(coord x, coord y, coord dx, coord dy, double c1d)
+{
+ int md = mapdist(x, y, dx, dy);
+ return md ? c1d + (md - 1) * 0.02 : 0.0;
+}
+
static double
cost_sail(natid actor, int uid)
{
static double
cost_sail(natid actor, int uid)
{
+static double
+h_sail_fly(coord x, coord y, coord dx, coord dy, double c1d)
+{
+ return mapdist(x, y, dx, dy);
+}
+
static double (*cost_tab[])(natid, int) = {
cost_move, cost_march, cost_rail, cost_sail, cost_fly
};
static double (*cost_tab[])(natid, int) = {
cost_move, cost_march, cost_rail, cost_sail, cost_fly
};
+static double (*h[])(coord, coord, coord, coord, double) = {
+ h_move, h_march_rail, h_march_rail, h_sail_fly, h_sail_fly
+};
+
/*
* Start finding paths from SX,SY.
* Use mobility costs for ACTOR and MOBTYPE.
/*
* Start finding paths from SX,SY.
* Use mobility costs for ACTOR and MOBTYPE.
void
path_find_from(coord sx, coord sy, natid actor, int mobtype)
{
void
path_find_from(coord sx, coord sy, natid actor, int mobtype)
{
- pf_set_source(sx, sy, actor, cost_tab[mobtype]);
+ pf_set_source(sx, sy, actor, cost_tab[mobtype], NULL);
double
path_find(coord sx, coord sy, coord dx, coord dy, natid actor, int mobtype)
{
double
path_find(coord sx, coord sy, coord dx, coord dy, natid actor, int mobtype)
{
- pf_set_source(sx, sy, actor, cost_tab[mobtype]);
+ pf_set_source(sx, sy, actor, cost_tab[mobtype], h[mobtype]);
return path_find_to(dx, dy);
}
return path_find_to(dx, dy);
}