astar.c

#include <stdio.h>
#include <limits.h>
#include <float.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <stdint.h>
#include <stdarg.h>

/* ---------------------------------------------------------------------------
 *                                  UTIL
 * --------------------------------------------------------------------------- */
#define cntof(a) ((int)(sizeof(a)/sizeof((a)[0])))
#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define iswap(x,y) ((x) ^= (y), (y) ^= (x), (x) ^= (y))
#define cast(T,p) ((T)(p))

#define macro_join_im(arg1, arg2) arg1 ## arg2
#define macro_join_delay(arg1, arg2) macro_join_im(arg1, arg2)
#define macro_join(arg1, arg2) macro_join_delay(arg1, arg2)
#ifdef _MSC_VER
  #define macro_uniq_id(name) macro_join(name,__COUNTER__)
#else
  #define macro_uniq_id(name) macro_join(name,__LINE__)
#endif
#define compile_assert(exp) typedef char macro_uniq_id(compile_assert_)[(exp)?1:-1]

static void
die(const char *fmt, ...) {
  va_list args;
  va_start(args, fmt);
  vfprintf(stderr, fmt, args);
  fprintf(stderr, "\n");
  va_end(args);
  exit(1);
}
static int
estrtol(const char *str) {
  char *ep = NULL;
  long n = strtol(str, &ep, 10);
  if (*ep != '\0' || ep == str) {
    die("Invalid number: %s\n", str);
  }
  if (n < INT_MIN || n > INT_MAX) {
    die("Number: %d is out of range\n", n);
  }
  return cast(int, n);
}

/* ---------------------------------------------------------------------------
 *                                  MAP
 * --------------------------------------------------------------------------- */
#define MAP_LINES   128
#define MAP_COLS    128
#define MAP_SIZE    (MAP_LINES*MAP_COLS)

#define TERRAIN_MAP\
  TERRAIN(PLAIN,    '.',    1.0f)\
  TERRAIN(WATER,    '*',    -1.0f)\
  TERRAIN(SWAMP,    '-',    1.5f)\
  TERRAIN(MOUNTAIN, '^',    2.0f)

enum tile_kind {
#define TERRAIN(a,b,c) TILE_##a,
  TERRAIN_MAP
#undef TERRAIN
  TILE_CNT
};
struct tile {
  enum tile_kind kind;
  const char sym;
  float cost;
};
static const struct tile tiles[] = {
#define TERRAIN(a,b,c) {TILE_##a, b, c},
  TERRAIN_MAP
#undef TERRAIN
};
static void
map_load(char *map, const char *path) {
  int n = 0;
  long siz = 0;
  FILE *fd = 0;
  char *mem = 0;
  size_t ret = 0;
  char *c = 0;

  /* open file */
  assert(map);
  assert(path);
  fd = fopen(path, "r");
  if (!fd) {
    die("Unable to open file: %s\n", path);
  }
  /* calculate file size */
  fseek(fd, 0, SEEK_END);
  siz = ftell(fd);
  if (siz < 0) die("Unable to access file: %s\n", path);
  fseek(fd, 0, SEEK_SET);
  if (siz < MAP_SIZE) {
    die("Map: %s has invalid size: %d\n", path, siz);
  }
  /* read file into memory */
  mem = calloc(1,(size_t)siz);
  assert(mem);
  if (!mem) {
    die("file calloc failed");
  }
  ret = fread(mem, 1, (size_t)siz, fd);
  if (ret < cast(size_t, siz)) {
    die("file fread failed: %d\n", ret);
  }
  /* fill map from file */
  {const char *end = mem + ret;
  for (c = mem; c < end && n < MAP_SIZE; c++) {
    switch (*c) {
    default: break;
    #define TERRAIN(a,b,_) case b: map[n++] = TILE_##a; break;
      TERRAIN_MAP
    #undef TERRAIN
    }
  }}
  assert(n <= MAP_SIZE);
  free(mem);
  fclose(fd);
}
static void
map_path_mask(uint64_t *mask, const short *path, int n) {
  int i = 0;
  for (i = n; i > 0; --i) {
    const int ni = path[i-1];
    const int bx = (ni >> 6);
    const int bi = ni & 63;
    mask[bx] |= (1ull << bi);
  }
}
static void
map_print(FILE *fp, const char *map, uint64_t *mask, int start, int end) {
  int x = 0, y = 0;
  for (y = 0; y < MAP_LINES; ++y) {
    int offx = y * MAP_COLS;
    for (x = 0; x < MAP_COLS; ++x) {
      int i = offx + x;
      
      unsigned bx = cast(unsigned,i) >> 6u;
      unsigned bi = cast(unsigned,i) & 63u;
      if (i == start) {
        fprintf(fp, "O");
      } else if (i == end) {
        fprintf(fp, "X");
      } else if (mask[bx] & (1ull << bi)) {
        fprintf(fp, "+");
      } else {
        fprintf(fp, "%c", tiles[map[i]].sym);
      }
    }
    fprintf(fp, "\n");
  }
}

/* ---------------------------------------------------------------------------
 *                                  A-STAR
 * --------------------------------------------------------------------------- */
enum as_node_state {
  NODE_DEFAULT,
  NODE_OPEN,
  NODE_CLOSED
};
/* 16 Bytes */
struct as_node {
  unsigned state:2;
  unsigned index:15;
  unsigned parent:15;
  float g,h,f;
};
struct as_ctx {
  /* result */
  int path_found;
  float cost;
  /* state */
  struct as_node graph[MAP_SIZE]; /* 256 KB */
  short heap[MAP_SIZE]; /* 32 KB */
  int heap_cnt;
};
static const float as_D = 1.0f;
static const float as_D2 = 1.41421356237f; /* sqrtf(2.0f) */
compile_assert(sizeof(struct as_node) == 16);

#define as_parent(i) (((i)-1) >> 1)
#define as_right(i) (((i)+1) << 1)
#define as_left(i) (as_right(i)-1)

static int
as_get_neighbours(int *list, float *cost, const char *map, int node) {
  static const int off[][2] = { 
    {-1,-1}, {0,-1}, {+1,-1}, {-1,0},
    {1,0}, {-1,+1}, {0,+1}, {+1,+1},
  };
  int i, idx, n = 0;
  int nx = node & (MAP_COLS-1);
  int ny = node >> 7;
  for (i = 0; i < cntof(off); ++i) {
    int offx = off[i][0];
    int offy = off[i][1];
    int x = nx + offx;
    int y = ny + offy;
    if (cast(unsigned,x) >= MAP_COLS ||
        cast(unsigned,y) >= MAP_LINES) {
      continue;
    }
    /* calculate map index and skip unpassable tiles */
    idx = y * MAP_COLS + x;
    if (tiles[map[idx]].cost < 0) {
      continue;
    }
    /* Add neighbour and cost into list.
     * Also has to take distance into account. */
    list[n] = idx;
    cost[n] = tiles[map[idx]].cost;
    if (offx != 0 && offy != 0) {
      cost[n] *= as_D2;
    } else {
      cost[n] *= as_D;
    } 
    n++;
  }
  return n;
}
static float
as_heuristic(int src, int dst) {
  /* diagonal distance */
  int src_x = src & (MAP_COLS-1);
  int src_y = src >> 7;
  int dst_x = dst & (MAP_COLS-1);
  int dst_y = dst >> 7;

  float dx = cast(float, abs(src_x - dst_x));
  float dy = cast(float, abs(src_y - dst_y));
  return as_D * max(dx, dy) + (as_D2-1) * min(dx,dy);
}
static void
as_heapify(struct as_ctx *as, short *ar, int i, int N) {
  while (N) {
    int lo, l = as_left(i), r = as_right(i);
    lo = (l < N && as->graph[ar[l]].f < as->graph[ar[i]].f) ? l: i;
    lo = (r < N && as->graph[ar[r]].f < as->graph[ar[lo]].f) ? r: lo;
    if (lo == i) {
      return;
    }
    iswap(ar[i], ar[lo]);
    iswap(as->graph[ar[i]].index, as->graph[ar[lo]].index);
    i = lo;
  }
}
static void
as_rebalance(struct as_ctx *as, int i) {
  while (i && as->graph[as->heap[as_parent(i)]].f > as->graph[as->heap[i]].f) {
    const int p = as_parent(i);
    iswap(as->heap[p], as->heap[i]);
    iswap(as->graph[as->heap[p]].index, as->graph[as->heap[i]].index);
    i = p;
  }
}
static void
as_process_neighbour(struct as_ctx *as,
    const struct as_node *node, int node_idx,
    int neighbour, float cost, int end) {
  float score = node->g + cost;
  struct as_node *n = as->graph + neighbour;
  if (n->state != NODE_OPEN) {
    /* First time we visit this node, so set it up */
    n->state = NODE_OPEN;
    n->h = as_heuristic(neighbour, end);
    n->index = cast(unsigned short, as->heap_cnt);
    n->parent = cast(unsigned short, node_idx);
    n->f = score + n->h;
    n->g = score;

    /* insert node into priority queue */
    as->heap[as->heap_cnt] = cast(short, neighbour);
    as_rebalance(as, as->heap_cnt++);
  } else if (score < as->graph[neighbour].g) {
    /* Update node with lower cost */
    n->parent = cast(unsigned short, node_idx);
    n->f = score + n->h;
    n->g = score;
    as_rebalance(as, n->index);
  }
}
static void
as_process_neighbours(struct as_ctx *as, const char *map,
    const struct as_node *node, int node_idx, int end) {
  float costs[8];
  int neighbours[8], i = 0;
  int n = as_get_neighbours(neighbours, costs, map, node_idx);
  for (i = 0; i < n; ++i) {
    /* skip nodes already in closed list */
    int nb = neighbours[i];
    if (as->graph[nb].state != NODE_CLOSED) {
      as_process_neighbour(as, node, node_idx, nb, costs[i], end);
    }
  }
}
static int
as_solve(struct as_ctx *as, const char *map, int start, int end) {
  struct as_node *node = 0;
  assert(start < MAP_SIZE);
  assert(end < MAP_SIZE);

  /* setup first node and add to open list */
  node = as->graph + start;
  node->index = 0;
  node->state = NODE_OPEN;
  node->parent = cast(unsigned short, start);
  as->heap[as->heap_cnt++] = cast(short, start);

  while (as->heap_cnt > 0) {
    /* extract node with lowest f(x) */
    int node_idx = as->heap[0];
    node = as->graph + node_idx;
    node->state = NODE_CLOSED;
    if (node_idx == end) {
      /* end found */
      as->cost = node->g;
      return as->path_found = 1;
    }
    /* remove node from priority queue and process */
    as->heap[0] = as->heap[--as->heap_cnt];
    as->graph[as->heap[0]].index = 0;
    as_heapify(as, as->heap, 0, as->heap_cnt);
    as_process_neighbours(as, map, node, node_idx, end);
  }
  return 0;
}
static int
as_pathing(short *path, struct as_ctx *as, int end) {
  int cnt = 0, i = end, p = -1;
  const struct as_node *n = 0;
  assert(end < MAP_SIZE);

  n = &as->graph[i];
  assert(n->parent >= 0);
  while (p != i) {
    path[cnt++] = cast(short, i);
    p = i; i = n->parent;
    n = &as->graph[i];
  }
  return cnt;
}

/* ---------------------------------------------------------------------------
 *                                  PROFILER
 * --------------------------------------------------------------------------- */
#ifdef _WIN32 /* Windows */

#include <Windows.h>

static long long time_begin;
static long long time_end;

static long long
profiler__timestamp(void) {
  LARGE_INTEGER li;
  QueryPerformanceCounter(&li);
  return li.QuadPart;
}
static double
profiler_time(void) {
  double freq;
  LARGE_INTEGER li;
  QueryPerformanceFrequency(&li);
  freq = cast(double, li.QuadPart);
  return (cast(double, time_end - time_begin) * 1000.0) / freq;
}

#else /* Linux */

#include <sys/time.h>
#include <unistd.h>
#include <time.h>

static double time_begin;
static double time_end;

static double
profiler__timestamp(void) {
  struct timeval tv;
  if (gettimeofday(&tv, NULL) < 0) {
   return 0;
  }
  return cast(double, tv.tv_sec) * 1000 + cast(double, tv.tv_usec) / 1000.0;
}
static double
profiler_time(void) {
  return time_end - time_begin;
}
#endif

static void
profiler_begin(void) {
  time_begin = profiler__timestamp();
}
static void
profiler_end(void) {
  time_end = profiler__timestamp();
}

/* ---------------------------------------------------------------------------
 *                                  APP
 * --------------------------------------------------------------------------- */
static void
usage(const char *app) {
  die("\n"
      "usage: %s map startx starty endx endy [showmap]\n"
      "\n"
      "   arguments:\n"
      "\n"
      "   map,        path to map file\n"
      "   startx,     a* map start x-position \n"
      "   starty,     a* map start y-position \n"
      "   endx,       a* map end x-position \n"
      "   endy,       a* map end y-position \n"
      "   showmap,    optionally show map with taken path\n"
      "\n",
      app
  );
  exit(1);
}
static int
eindex(const char *str, int max) {
  int i = estrtol(str);
  if (i < 0) {
    die("Invalid negative %s map position\n", str);
  }
  if (i >= max) {
    die("Map position: %s is to big\n", str);
  }
  return i;
}
static int
epos(const char *x, const char *y) {
  int px = eindex(x, MAP_COLS);
  int py = eindex(y, MAP_LINES);
  return MAP_COLS * py + px;
}
extern int
main(int argc, char **argv) {
  struct as_ctx as = {0}; /* 288Kb */
  char map[MAP_SIZE] = {0}; /* 16Kb */
  unsigned char show_map = 0;
  int start, end;

  /* Arguments */
  if (argc < 6) {
    usage(argv[0]);
  } else if (argc > 6) {
    if (strcmp(argv[6], "showmap") != 0) {
      usage(argv[0]);
    }
    show_map = 1;
  }
  map_load(map, argv[1]);
  start = epos(argv[2], argv[3]);
  end = epos(argv[4], argv[5]);

  /* A* */
  profiler_begin();
  as_solve(&as, map, start, end);
  profiler_end();

  /* Output */
  printf("Start position:\t(%s, %s)\n", argv[2], argv[3]);
  printf("End position:\t(%s, %s)\n", argv[4], argv[5]);
  printf("\n");
  printf("Path found:\t%s\n", as.path_found ? "true": "false");
  printf("Path cost:\t%f\n", as.cost);
  printf("\n");
  printf("Total duration:\t%f ms\n", profiler_time());

  /* Print */
  if (as.path_found && show_map) {
    short path[MAP_SIZE] = {0}; /* 32 KB */
    uint64_t mask[256] = {0}; /* 2KB */
    int n = as_pathing(path, &as, end);

    printf("\n");
    map_path_mask(mask, path, n);
    map_print(stdout, map, mask, start, end);
  }
  return 0;
}