git.pond.sub.org Git - empserver/blob - src/lib/common/pathfind.c

   1 /*
   2  *  Empire - A multi-player, client/server Internet based war game.
   3  *  Copyright (C) 1986-2011, Dave Pare, Jeff Bailey, Thomas Ruschak,
   4  *                Ken Stevens, Steve McClure, Markus Armbruster
   5  *
   6  *  Empire is free software: you can redistribute it and/or modify
   7  *  it under the terms of the GNU General Public License as published by
   8  *  the Free Software Foundation, either version 3 of the License, or
   9  *  (at your option) any later version.
  10  *
  11  *  This program is distributed in the hope that it will be useful,
  12  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  *  GNU General Public License for more details.
  15  *
  16  *  You should have received a copy of the GNU General Public License
  17  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18  *
  19  *  ---
  20  *
  21  *  See files README, COPYING and CREDITS in the root of the source
  22  *  tree for related information and legal notices.  It is expected
  23  *  that future projects/authors will amend these files as needed.
  24  *
  25  *  ---
  26  *
  27  *  pathfind.c: Find cheapest paths
  28  *
  29  *  Known contributors to this file:
  30  *     Markus Armbruster, 2011
  31  */
  32
  33 #include <config.h>
  34
  35 #include <assert.h>
  36 #include <stdio.h>
  37 #include <stdlib.h>
  38 #include <string.h>
  39 #include "file.h"
  40 #include "optlist.h"
  41 #include "path.h"
  42 #include "sect.h"
  43
  44 #ifdef PATH_FIND_DEBUG
  45 #define DPRINTF(fmt, ...) ((void)printf(fmt , ## __VA_ARGS__))
  46 #else
  47 #define DPRINTF(fmt, ...) ((void)0)
  48 #endif
  49
  50 static char *bufrotate(char *buf, size_t bufsz, size_t i);
  51
  52 /*
  53  * Dijkstra's algorithm.  Refer to your graph algorithm textbook for
  54  * how it works.  Implementation is specialized to hex maps.
  55  *
  56  * Define PATH_FIND_STATS for performance statistics on stdout.
  57  */
  58
  59 /*
  60  * Array of sector data, indexed by sector uid
  61  *
  62  * We need to store a few values per sector visited by the path
  63  * search.  An array is the stupidest data structure that could
  64  * possibly work.
  65  *
  66  * Extra benefit: it works really well for distribution in a
  67  * continental game, where we visit most sectors.  That's our most
  68  * demanding use of path search, and its performance has noticable
  69  * impact on the update.
  70  *
  71  * Island game distribution is much less demanding.  The array may not
  72  * be the best choice here, but it's plainly good enough.  Same for
  73  * path searches outside the update.
  74  */
  75
  76 struct pf_map {
  77     /*
  78      * visit < pf_visit      : unvisited, remaining members invalid
  79      * visit == pf_visit     : open, dir & cost tentative, heapi used
  80      * visit == pf_visit + 1 : closed, dir & cost final, heapi unused
  81      */
  82     unsigned short visit;
  83     signed char dir;            /* cheapest direction to source */
  84     int heapi;                  /* index in heap, valid if open */
  85     double cost;                /* cost from source */
  86 };
  87
  88 static unsigned short pf_visit;
  89 static struct pf_map *pf_map;
  90
  91 /*
  92  * Binary heap, cost priority queue of all open sectors
  93  *
  94  * Again, we use the stupidest data structure that could possibly
  95  * work: an array.  And we make it so large it can hold *all* sectors.
  96  * In practice, we need much less space, but a tighter upper bound is
  97  * not obvious to me right now.
  98  */
  99
 100 struct pf_heap {
 101     int uid;                    /* sector uid and */
 102     coord x, y;                 /* coordinates, uid == XYOFFSET(x, y) */
 103     double cost;                /* cost from source */
 104 };
 105
 106 static int pf_nheap;            /* #entries in pf_nheap[] */
 107 static struct pf_heap *pf_heap;
 108
 109 /*
 110  * Source and costs
 111  */
 112 static coord pf_sx, pf_sy;
 113 static int pf_suid;
 114 static natid pf_actor;
 115 static double (*pf_sct_cost)(natid, int);
 116
 117 /*
 118  * Performance statistics
 119  */
 120 #ifdef PATH_FIND_STATS
 121 static unsigned pf_nsearch, pf_nsource, pf_nopen, pf_nclose;
 122 static unsigned pf_nheap_max, pf_noway;
 123 static double pf_sumcost;
 124 #define STAT_INC(v) ((void)((v)++))
 125 #define STAT_INCBY(v, i) ((void)((v) += i))
 126 #define STAT_HIMARK(v, h) ((void)((v) < (h) ? (v) = (h) : (h)))
 127 #else   /* !PATH_FIND_STATS */
 128 #define STAT_INC(v) ((void)0)
 129 #define STAT_INCBY(v, i) ((void)0)
 130 #define STAT_HIMARK(v, h) ((void)0)
 131 #endif  /* !PATH_FIND_STATS */
 132
 133 #ifndef NDEBUG                  /* silence "not used" warning */
 134 /* Is sector with uid UID open?  */
 135 static int
 136 pf_is_open(int uid)
 137 {
 138     return pf_map[uid].visit == pf_visit;
 139 }
 140 #endif
 141
 142 /* Is sector with uid UID closed?  */
 143 static int
 144 pf_is_closed(int uid)
 145 {
 146     /*
 147      * optimization: check > pf_visit instead of == pf_visit + 1
 148      * works because pf_map[uid].visit <= pf_visit + 1
 149      */
 150     return pf_map[uid].visit > pf_visit;
 151 }
 152
 153 /* Is sector with uid UID unvisited?  */
 154 static int
 155 pf_is_unvisited(int uid)
 156 {
 157     return pf_map[uid].visit < pf_visit;
 158 }
 159
 160 #ifdef PATH_FIND_DEBUG
 161 static void
 162 pf_check(void)
 163 {
 164     int i, uid, c;
 165
 166     for (i = 0; i < pf_nheap; i++) {
 167         uid = pf_heap[i].uid;
 168         assert(0 <= uid && uid < WORLD_SZ());
 169         assert(pf_map[uid].heapi == i);
 170         assert(pf_map[uid].visit == pf_visit);
 171         assert(pf_map[uid].cost <= pf_heap[i].cost);
 172         c = 2 * i + 1;
 173         assert(c >= pf_nheap || pf_heap[i].cost <= pf_heap[c].cost);
 174         c++;
 175         assert(c >= pf_nheap || pf_heap[i].cost <= pf_heap[c].cost);
 176     }
 177
 178     for (uid = 0; uid < WORLD_SZ(); uid++) {
 179         assert(pf_map[uid].visit <= pf_visit + 1);
 180         if (pf_map[uid].visit == pf_visit) {
 181             i = pf_map[uid].heapi;
 182             assert(0 <= i && i < pf_nheap && pf_heap[i].uid == uid);
 183         }
 184     }
 185 }
 186 #else
 187 #define pf_check() ((void)0)
 188 #endif
 189
 190 /* Swap pf_heap's I-th and J-th elements.  */
 191 static void
 192 pf_heap_swap(int i, int j)
 193 {
 194     struct pf_heap tmp;
 195
 196     assert(0 <= i && i < pf_nheap);
 197     assert(0 <= j && j < pf_nheap);
 198     tmp = pf_heap[i];
 199     pf_heap[i] = pf_heap[j];
 200     pf_heap[j] = tmp;
 201     pf_map[pf_heap[i].uid].heapi = i;
 202     pf_map[pf_heap[j].uid].heapi = j;
 203 }
 204
 205 /* Restore heap property after N-th element's cost increased.  */
 206 static void
 207 pf_sift_down(int n)
 208 {
 209     int r, c;
 210
 211     assert(0 <= n && n < pf_nheap);
 212     for (r = n; (c = 2 * r + 1) < pf_nheap; r = c) {
 213         if (c + 1 < pf_nheap && pf_heap[c].cost > pf_heap[c + 1].cost)
 214             c++;
 215         if (pf_heap[r].cost < pf_heap[c].cost)
 216             break;
 217         pf_heap_swap(r, c);
 218     }
 219 }
 220
 221 /* Restore heap property after N-th element's cost decreased.  */
 222 static void
 223 pf_sift_up(int n)
 224 {
 225     int c, p;
 226
 227     assert(0 <= n && n < pf_nheap);
 228     for (c = n; (p = (c - 1) / 2), c > 0; c = p) {
 229         if (pf_heap[p].cost < pf_heap[c].cost)
 230             break;
 231         pf_heap_swap(p, c);
 232     }
 233 }
 234
 235 /*
 236  * Open the unvisited sector X,Y.
 237  * UID is sector uid, it equals XYOFFSET(X,Y).
 238  * Cheapest path from source comes from direction DIR and has cost COST.
 239  */
 240 static void
 241 pf_open(int uid, coord x, coord y, int dir, double cost)
 242 {
 243     int i;
 244
 245     STAT_INC(pf_nopen);
 246     i = pf_nheap++;
 247     STAT_HIMARK(pf_nheap_max, (unsigned)pf_nheap);
 248     DPRINTF("pf: open %d,%d %g %c %d\n", x, y, cost, dirch[dir], i);
 249     assert(pf_is_unvisited(uid));
 250     pf_map[uid].visit = pf_visit;
 251     pf_map[uid].dir = dir;
 252     pf_map[uid].heapi = i;
 253     pf_map[uid].cost = cost;
 254     pf_heap[i].uid = uid;
 255     pf_heap[i].x = x;
 256     pf_heap[i].y = y;
 257     pf_heap[i].cost = cost;
 258
 259     pf_sift_up(i);
 260     pf_check();
 261 }
 262
 263 /*
 264  * Close the sector at the top of the heap.
 265  */
 266 static void
 267 pf_close(void)
 268 {
 269     int uid = pf_heap[0].uid;
 270
 271     STAT_INC(pf_nclose);
 272     DPRINTF("pf: close %d,%d %d\n", pf_heap[0].x, pf_heap[0].y, pf_nheap);
 273     assert(pf_is_open(uid));
 274     if (--pf_nheap) {
 275         pf_heap[0] = pf_heap[pf_nheap];
 276         pf_map[pf_heap[0].uid].heapi = 0;
 277         pf_sift_down(0);
 278     }
 279     pf_map[uid].visit = pf_visit + 1;
 280     pf_check();
 281 }
 282
 283 /* silence "not used" warning */
 284 #ifdef PATH_FIND_DEBUG
 285 /*
 286  * Return cost from source to sector with uid UID.
 287  * It must be visited, i.e. open or closed.
 288  */
 289 static double
 290 pf_cost(int uid)
 291 {
 292     assert(!pf_is_unvisited(uid));
 293     return pf_map[uid].cost;
 294 }
 295 #endif
 296
 297 static coord
 298 x_in_dir(coord x, int dir)
 299 {
 300     int xx;
 301
 302     assert(0 <= x && x < WORLD_X);
 303     assert(0 <= dir && dir <= DIR_LAST);
 304     xx = x + diroff[dir][0];
 305     if (xx < 0)
 306         return xx + WORLD_X;
 307     if (xx >= WORLD_X)
 308         return xx - WORLD_X;
 309     return xx;
 310 }
 311
 312 static coord
 313 y_in_dir(coord y, int dir)
 314 {
 315     int yy;
 316
 317     assert(0 <= y && y < WORLD_Y);
 318     assert(0 <= dir && dir <= DIR_LAST);
 319     yy = y + diroff[dir][1];
 320     if (yy < 0)
 321         return yy + WORLD_Y;
 322     if (yy >= WORLD_Y)
 323         return yy - WORLD_Y;
 324     return yy;
 325 }
 326
 327 static int
 328 rev_dir(int dir)
 329 {
 330     assert(DIR_FIRST <= dir && dir <= DIR_LAST);
 331     return dir >= DIR_FIRST + 3 ? dir - 3 : dir + 3;
 332 }
 333
 334 /*
 335  * Set the current source and cost function.
 336  * SX,SY is the source.
 337  * The cost to enter the sector with uid u is COST(ACTOR, u).
 338  * Negative value means the sector can't be entered.
 339  */
 340 static void
 341 pf_set_source(coord sx, coord sy, natid actor, double (*cost)(natid, int))
 342 {
 343     STAT_INC(pf_nsource);
 344     DPRINTF("pf: source %d,%d\n", sx, sy);
 345     pf_sx = sx;
 346     pf_sy = sy;
 347     pf_suid = XYOFFSET(sx, sy);
 348     pf_actor = actor;
 349     pf_sct_cost = cost;
 350
 351     if (!pf_map) {
 352         pf_map = calloc(WORLD_SZ(), sizeof(*pf_map));
 353         pf_heap = malloc(WORLD_SZ() * sizeof(*pf_heap));
 354         pf_visit = 1;
 355     } else if ((unsigned short)(pf_visit + 3) < pf_visit) {
 356         DPRINTF("pf: visit wrap-around\n");
 357         memset(pf_map, 0, WORLD_SZ() * sizeof(*pf_map));
 358         pf_visit = 1;
 359     } else
 360         pf_visit += 2;
 361
 362     pf_nheap = 0;
 363
 364     pf_open(pf_suid, pf_sx, pf_sy, DIR_STOP, 0.0);
 365 }
 366
 367 /*
 368  * Find cheapest path from current source to DX,DY, return its cost.
 369  */
 370 double
 371 path_find_to(coord dx, coord dy)
 372 {
 373     int duid;
 374     int uid, nuid, i;
 375     double cost, c1;
 376     coord x, y, nx, ny;
 377
 378     STAT_INC(pf_nsearch);
 379     DPRINTF("pf: dest %d,%d\n", dx, dy);
 380     duid = XYOFFSET(dx, dy);
 381     if (pf_is_closed(duid)) {
 382         DPRINTF("pf: done old %g\n", pf_map[duid].cost);
 383         STAT_INCBY(pf_sumcost, pf_map[duid].cost);
 384         return pf_map[duid].cost;
 385     }
 386
 387     while (pf_nheap > 0 && (uid = pf_heap[0].uid) != duid) {
 388         x = pf_heap[0].x;
 389         y = pf_heap[0].y;
 390         cost = pf_heap[0].cost;
 391         pf_close();
 392
 393         for (i = 0; i < 6; i++) { /* for all neighbors */
 394             nx = x_in_dir(x, DIR_FIRST + i);
 395             ny = y_in_dir(y, DIR_FIRST + i);
 396             nuid = XYOFFSET(nx, ny);
 397             /*
 398              * Cost to enter NX,NY doesn't depend on direction of
 399              * entry.  This X,Y is at least as expensive as any
 400              * previous one.  Therefore, cost to go to NX,NY via X,Y
 401              * is at least as high as any previously found route.
 402              * Skip neighbors that have a route already.
 403              */
 404             if (!pf_is_unvisited(nuid))
 405                 continue;
 406             c1 = pf_sct_cost(pf_actor, nuid);
 407             if (c1 < 0)
 408                 continue;
 409             pf_open(nuid, nx, ny, DIR_FIRST + i, cost + c1);
 410         }
 411     }
 412
 413     DPRINTF("pf: done new %g\n", !pf_nheap ? -1.0 : pf_map[duid].cost);
 414     if (!pf_nheap) {
 415         STAT_INC(pf_noway);
 416         return -1.0;
 417     }
 418     STAT_INCBY(pf_sumcost, pf_map[duid].cost);
 419     return pf_map[duid].cost;
 420 }
 421
 422 /*
 423  * Write route from SX,SY to DX,DY to BUF[BUFSIZ], return its length.
 424  * If the route is longer than BUFSIZ-1 characters, it's truncated.
 425  * You must compute path cost first, with path_find_to().
 426  * SX,SY must be on a shortest path from the current source to DX,DY.
 427  */
 428 size_t
 429 path_find_route(char *buf, size_t bufsz,
 430                 coord sx, coord sy, coord dx, coord dy)
 431 {
 432     coord x, y;
 433     size_t i, len;
 434     int suid, uid, d;
 435
 436     suid = XYOFFSET(sx, sy);
 437     assert(bufsz > 0 && !pf_is_unvisited(suid));
 438
 439     i = bufsz;
 440     buf[--i] = 0;
 441     len = 0;
 442
 443     x = dx;
 444     y = dy;
 445     for (;;) {
 446         DPRINTF("pf: %d,%d %.*s%.*s\n",
 447                 x, y,
 448                 (int)(bufsz - i), buf + i,
 449                 len >= bufsz ? (int)i : 0, buf);
 450         uid = XYOFFSET(x, y);
 451         assert(!pf_is_unvisited(uid));
 452         d = pf_map[uid].dir;
 453         if (d == DIR_STOP || uid == suid)
 454             break;
 455         if (!i)
 456             i = bufsz;
 457         buf[--i] = dirch[d];
 458         len++;
 459         x = x_in_dir(x, rev_dir(d));
 460         y = y_in_dir(y, rev_dir(d));
 461     }
 462
 463     assert(x == sx && y == sy);
 464     if (len >= bufsz)
 465         bufrotate(buf, bufsz, i);
 466     else {
 467         assert(i + len < bufsz);
 468         memmove(buf, buf + i, len + 1);
 469     }
 470     return len;
 471 }
 472
 473 /*
 474  * Rotate BUF[BUFSZ] to put BUF[I] into BUF[0], and zero-terminate.
 475  */
 476 static char *
 477 bufrotate(char *buf, size_t bufsz, size_t i)
 478 {
 479     char tmp[64];
 480     size_t n;
 481
 482     while (i) {
 483         n = MIN(i, sizeof(tmp));
 484         memcpy(tmp, buf, n);
 485         memcpy(buf, buf + n, bufsz - n);
 486         memcpy(buf + bufsz - n, tmp, n);
 487         i -= n;
 488     }
 489     buf[bufsz - 1] = 0;
 490     return buf;
 491 }
 492
 493 #ifdef PATH_FIND_DEBUG
 494 void
 495 path_find_visualize(coord sx, coord sy, coord dx, coord dy)
 496 {
 497     int uid;
 498     int xmin, xmax, ymin, ymax, x, y, odd, ch;
 499     double c, u, cost;
 500     char buf[1024];
 501
 502     assert(pf_cost(XYOFFSET(sx, sy)) == 0.0);
 503     c = pf_cost(XYOFFSET(dx, dy));
 504     u = c / 10.0;
 505
 506     /* find bounding box */
 507     xmin = xmax = 0;
 508     ymin = ymax = 0;
 509     for (y = -WORLD_Y / 2; y < WORLD_Y / 2; y++) {
 510         odd = ((sx + -WORLD_X / 2) ^ (sy + y)) & 1;
 511         for (x = -WORLD_X / 2 + odd; x < WORLD_X / 2; x += 2) {
 512             uid = XYOFFSET(XNORM(sx + x), YNORM(sy + y));
 513             if (pf_is_unvisited(uid))
 514                 continue;
 515             if (xmin > x)
 516                 xmin = x;
 517             if (xmax < x)
 518                 xmax = x;
 519             if (ymin > y)
 520                 ymin = y;
 521             if (ymax < y)
 522                 ymax = y;
 523         }
 524     }
 525     printf("bbox %d:%d,%d:%d origin %d,%d\n",
 526            xmin, xmax, ymin, ymax, sx, sy);
 527
 528     for (y = ymin; y <= ymax; y++) {
 529         odd = ((sx + xmin) ^ (sy + y)) & 1;
 530         if (odd)
 531             printf(" ");
 532         for (x = xmin + odd; x <= xmax; x += 2) {
 533             uid = XYOFFSET(XNORM(sx + x), YNORM(sy + y));
 534             if (pf_is_unvisited(uid))
 535                 ch = ' ';
 536             else if (uid == XYOFFSET(dx, dy))
 537                 ch = 'D';
 538             else if (uid == XYOFFSET(sx, sy))
 539                 ch = 'S';
 540             else {
 541                 cost = pf_cost(uid);
 542                 ch = cost > c ? '+' : '0' + (int)(10 * (cost / c));
 543             }
 544             printf(" %c", ch);
 545         }
 546         printf("\n");
 547     }
 548     path_find_route(buf, sizeof(buf), sx, sy, dx, dy);
 549     printf("%s %g\n", buf, pf_cost(XYOFFSET(dx, dy)));
 550 }
 551 #endif
 552
 553 #ifdef PATH_FIND_STATS
 554 void
 555 path_find_print_stats(void)
 556 {
 557     printf("pathfind %u searches, %u sources, %u opened, %u closed,"
 558            " %u heap max, %zu bytes, %u noway, %g avg cost\n",
 559            pf_nsearch, pf_nsource, pf_nopen, pf_nclose,
 560            pf_nheap_max,
 561            (WORLD_SZ() * (sizeof(*pf_map) + sizeof(*pf_heap))),
 562            pf_noway, pf_nsearch ? pf_sumcost / pf_nsearch : 0.0);
 563 }
 564 #endif
 565
 566 /*
 567  * Empire interface glue
 568  */
 569
 570 static double
 571 cost_land(natid actor, int uid, int mobtype)
 572 {
 573     /*
 574      * Non-negative cost must not depend on ACTOR, see BestLandPath().
 575      */
 576     struct sctstr *sp = (void *)empfile[EF_SECTOR].cache;
 577
 578     if (sp[uid].sct_own != actor)
 579         return -1.0;
 580     return sector_mcost(&sp[uid], mobtype);
 581 }
 582
 583 static double
 584 cost_move(natid actor, int uid)
 585 {
 586     return cost_land(actor, uid, MOB_MOVE);
 587 }
 588
 589 static double
 590 cost_march(natid actor, int uid)
 591 {
 592     return cost_land(actor, uid, MOB_MARCH);
 593 }
 594
 595 static double
 596 cost_rail(natid actor, int uid)
 597 {
 598     return cost_land(actor, uid, MOB_RAIL);
 599 }
 600
 601 static double (*cost_tab[])(natid, int) = {
 602     cost_move, cost_march, cost_rail
 603 };
 604
 605 /*
 606  * Start finding paths from SX,SY.
 607  * Use mobility costs for ACTOR and MOBTYPE.
 608  */
 609 void
 610 path_find_from(coord sx, coord sy, natid actor, int mobtype)
 611 {
 612     pf_set_source(sx, sy, actor, cost_tab[mobtype]);
 613 }
 614
 615 /*
 616  * Find cheapest path from SX,SY to DX,DY, return its mobility cost.
 617  * Use mobility costs for ACTOR and MOBTYPE.
 618  */
 619 double
 620 path_find(coord sx, coord sy, coord dx, coord dy, natid actor, int mobtype)
 621 {
 622     pf_set_source(sx, sy, actor, cost_tab[mobtype]);
 623     return path_find_to(dx, dy);
 624 }