Skip to content

Instantly share code, notes, and snippets.

@mmozeiko

mmozeiko/astar.h

Last active December 29, 2023 10:18
Show Gist options
  • Save mmozeiko/68f0a8459ef2f98bcd879158011cc275 to your computer and use it in GitHub Desktop.
Save mmozeiko/68f0a8459ef2f98bcd879158011cc275 to your computer and use it in GitHub Desktop.
Download ZIP
generic A* in C
Raw
astar.h
// generic A* pathfinding
//
// INTERFACE
//
// mandatory macros
#ifndef ASTAR_POS_TYPE
#error ASTAR_POS_TYPE should specify position type
#endif
#ifndef ASTAR_POS_START
#error ASTAR_POS_START should specify start position
#endif
#ifndef ASTAR_POS_FINISH
#error ASTAR_POS_FINISH should specify finish position
#endif
#ifndef ASTAR_POS_INDEX
#error ASTAR_POS_INDEX(p) should specify macro to map position to index
#endif
#ifndef ASTAR_MAX_INDEX
#error ASTAR_MAX_INDEX should specify max count of indices the position can map to
#endif
#ifndef ASTAR_INDEX_POS
#error ASTAR_INDEX_POS(i) should specify macro to map index to position
#endif
#ifndef ASTAR_POS_EQUAL
#error ASTAR_POS_EQUAL(a, b) should specify macro to compare if two positions are the same
#endif
#ifndef ASTAR_MAP_IS_FREE
#error ASTAR_MAP_IS_FREE(p) should specify macro to check if map at position p is free
#endif
#ifndef ASTAR_NEXT_POS
#error ASTAR_NEXT_POS(p, i) should specify macro to get next position in specific direction
#endif
#ifndef ASTAR_PREV_POS
#error ASTAR_PREV_POS(p, i) should specify macro to get previous position from specific direction
#endif
#ifndef ASTAR_DIR_COUNT
#error ASTAR_DIR_COUNT should specify possible direction count
#endif
#ifndef ASTAR_GET_COST
#error ASTAR_GET_COST(a, b) should specify macro to get get cost between two positions
#endif
#ifndef ASTAR_PATH
#error ASTAR_PATH(p) should specify macro that will be invoked on each position for path (in reverse order), including start/finish positions
#endif
#if !defined(ASTAR_TEMP) || !defined(ASTAR_TEMP_SIZE)
#error ASTAR_TEMP and ASTAR_TEMP_SIZE should specify variable & size for temporary memory (should be at least ASTAR_MAX_INDEX * 4 + extra)
#endif
// optional macros
// adds extra cost for specific direction (useful for increasing cost for diagonal movements)
#ifndef ASTAR_EXTRA_COST
#define ASTAR_EXTRA_COST(i) 1
#endif
// multiplier for adding cost values (current_cost + mul * new_cost) - useful when using extra cost for diagonal movements
#ifndef ASTAR_COST_MUL
#define ASTAR_COST_MUL 1
#endif
//
// IMPLEMENTATION
//
#if ASTAR_DIR_COUNT <= 4
#define ASTAR_DIR_BITS 2
#elif ASTAR_DIR_COUNT <= 8
#define ASTAR_DIR_BITS 3
#elif ASTAR_DIR_COUNT <= 16
#define ASTAR_DIR_BITS 4
#elif ASTAR_DIR_COUNT <= 32
#define ASTAR_DIR_BITS 5
#elif ASTAR_DIR_COUNT <= 64
#define ASTAR_DIR_BITS 6
#else
#error Too many elements for ASTAR_DIR_COUNT, 64 is max
#endif
#define ASTAR_COST_BITS (32 - 1 - ASTAR_DIR_BITS)
#define ASTAR_HEAP_SWAP(a, b) \
do { \
heapnode t = heap[a]; \
heap[a] = heap[b]; \
heap[b] = t; \
} while (0) \
#define ASTAR_HEAP_PUSH(idx, c) \
do { \
heap[heap_count].index = idx; \
heap[heap_count].cost = c; \
int i = heap_count++; \
int p = (i - 1) / 2; \
while (i != 0 && heap[p].cost > heap[i].cost) \
{ \
ASTAR_HEAP_SWAP(i, p); \
i = p; \
p = (i - 1) / 2; \
} \
} while (0)
#define ASTAR_HEAP_POP() \
do { \
heap[0] = heap[--heap_count]; \
int i = 0; \
for (;;) \
{ \
int l = 2 * i + 1; \
int r = 2 * i + 2; \
int s = i; \
if (l < heap_count && heap[l].cost < heap[i].cost) s = l; \
if (r < heap_count && heap[r].cost < heap[s].cost) s = r; \
if (s == i) break; \
ASTAR_HEAP_SWAP(i, s); \
i = s; \
} \
} while (0)
typedef union {
struct {
unsigned int cost : ASTAR_COST_BITS;
unsigned int dir : ASTAR_DIR_BITS;
unsigned int visited : 1;
};
unsigned int all;
} node;
typedef struct {
unsigned int index;
unsigned int cost;
} heapnode;
if (ASTAR_TEMP_SIZE >= sizeof(node) * ASTAR_MAX_INDEX + sizeof(heapnode))
{
node* nodes = (node*)ASTAR_TEMP;
for (unsigned int i = 0; i < ASTAR_MAX_INDEX; i++)
{
nodes[i].all = 0;
}
heapnode* heap = (heapnode*)((char*)ASTAR_TEMP + sizeof(node) * ASTAR_MAX_INDEX);
unsigned int heap_max = (ASTAR_TEMP_SIZE - sizeof(node) * ASTAR_MAX_INDEX) / sizeof(heapnode);
int heap_count = 0;
ASTAR_POS_TYPE p = ASTAR_POS_START;
unsigned int nindex = ASTAR_POS_INDEX(p);
node* n = nodes + nindex;
n->cost = 0;
n->visited = 1;
ASTAR_HEAP_PUSH(nindex, ASTAR_GET_COST(p, ASTAR_POS_FINISH));
int found = 0;
while (heap_count != 0)
{
nindex = heap[0].index;
n = nodes + nindex;
ASTAR_HEAP_POP();
ASTAR_INDEX_POS(p, nindex);
if (ASTAR_POS_EQUAL(p, ASTAR_POS_FINISH))
{
found = 1;
break;
}
n->visited = 1;
for (unsigned int i = 0; i < ASTAR_DIR_COUNT; i++)
{
ASTAR_POS_TYPE next = p;
ASTAR_NEXT_POS(next, i);
if (ASTAR_MAP_IS_FREE(next))
{
unsigned int nnext_index = ASTAR_POS_INDEX(next);
node* nnext = nodes + nnext_index;
unsigned int cost = n->cost + ASTAR_EXTRA_COST(i);
if (nnext->visited == 0 || cost < nnext->cost)
{
nnext->cost = cost;
nnext->dir = i;
nnext->visited = 1;
if (heap_count == heap_max)
{
// out of memory
goto bail;
}
unsigned int new_cost = cost + ASTAR_COST_MUL * ASTAR_GET_COST(next, ASTAR_POS_FINISH);
ASTAR_HEAP_PUSH(nnext_index, new_cost);
}
}
}
}
bail:
if (found)
{
ASTAR_PATH(p);
while (!ASTAR_POS_EQUAL(p, ASTAR_POS_START))
{
ASTAR_PREV_POS(p, n->dir);
n = nodes + ASTAR_POS_INDEX(p);
ASTAR_PATH(p);
}
}
}
else
{
// not enough temp memory
}
#undef ASTAR_POS_TYPE
#undef ASTAR_POS_START
#undef ASTAR_POS_FINISH
#undef ASTAR_POS_INDEX
#undef ASTAR_MAX_INDEX
#undef ASTAR_INDEX_POS
#undef ASTAR_POS_EQUAL
#undef ASTAR_MAP_IS_FREE
#undef ASTAR_NEXT_POS
#undef ASTAR_PREV_POS
#undef ASTAR_DIR_COUNT
#undef ASTAR_GET_COST
#undef ASTAR_EXTRA_COST
#undef ASTAR_COST_MUL
#undef ASTAR_PATH
#undef ASTAR_TEMP
#undef ASTAR_TEMP_SIZE
#undef ASTAR_COST_BITS
#undef ASTAR_DIR_BITS
#undef ASTAR_HEAP_SWAP
#undef ASTAR_HEAP_PUSH
#undef ASTAR_HEAP_POP
Raw
astar_jps2d.h
// generic A* pathfinding on 2D grid
// only non-diagonal movement is implemented
//
// INTERFACE
//
// mandatory macros
#if !defined(ASTAR_POS_START_X) || !defined(ASTAR_POS_START_Y)
#error ASTAR_POS_START_X/Y should specify start position
#endif
#if !defined(ASTAR_POS_FINISH_X) || !defined(ASTAR_POS_FINISH_Y)
#error ASTAR_POS_FINISH_X/Y should specify finish position
#endif
#if !defined(ASTAR_WIDTH) || !defined(ASTAR_HEIGHT)
#error ASTAR_WIDTH and ASTAR_HEIGHT should specify width & height of map
#endif
#ifndef ASTAR_MAP_IS_FREE
#error ASTAR_MAP_IS_FREE(x, y) should specify macro to check if map at position (x,y) is free
#endif
#ifndef ASTAR_COST
#error ASTAR_COST(ax, ay, bx, by) should specify macro to get get cost between two positions
#endif
#ifndef ASTAR_PATH
#error ASTAR_PATH(x,y) should specify macro that will be invoked on each position for path (in reverse order), including start/finish positions
#endif
#if !defined(ASTAR_TEMP) || !defined(ASTAR_TEMP_SIZE)
#error ASTAR_TEMP and ASTAR_TEMP_SIZE should specify variable & size for temporary memory (should be at least ASTAR_MAX_INDEX * 4 + extra)
#endif
// optional macros
#ifndef ASTAR_PATH_REDUCED
#define ASTAR_PATH_REDUCED 0 // define to 1 to only produce path on turning positions
#endif
//
// IMPLEMENTATION
//
#define ASTAR_INDEX(x, y) ((y) * ASTAR_WIDTH + (x))
#define ASTAR_HEAP_SWAP(a, b) \
do { \
heapnode t = heap[a]; \
heap[a] = heap[b]; \
heap[b] = t; \
} while (0) \
#define ASTAR_HEAP_PUSH(idx, c) \
do { \
heap[heap_count].index = idx; \
heap[heap_count].cost = c; \
int i = heap_count++; \
int p = (i - 1) / 2; \
while (i != 0 && heap[p].cost > heap[i].cost) \
{ \
ASTAR_HEAP_SWAP(i, p); \
i = p; \
p = (i - 1) / 2; \
} \
} while (0)
#define ASTAR_HEAP_POP() \
do { \
heap[0] = heap[--heap_count]; \
int i = 0; \
for (;;) \
{ \
int l = 2 * i + 1; \
int r = 2 * i + 2; \
int s = i; \
if (l < heap_count && heap[l].cost < heap[i].cost) s = l; \
if (r < heap_count && heap[r].cost < heap[s].cost) s = r; \
if (s == i) break; \
ASTAR_HEAP_SWAP(i, s); \
i = s; \
} \
} while (0)
#define ASTAR_VISIT(ox, oy, x, y) \
do { \
unsigned int nnext_index = ASTAR_INDEX(x, y); \
node* nnext = nodes + nnext_index; \
unsigned int cost = n->cost + ASTAR_COST(ox, oy, x, y); \
if (nnext->visited == 0 || cost < nnext->cost) \
{ \
nnext->cost = cost; \
nnext->visited = 1; \
nnext->parent = ASTAR_INDEX(ox, oy); \
if (heap_count == heap_max) \
{ \
/* out of memory */ \
goto bail; \
} \
unsigned int new_cost = cost + ASTAR_COST(x, y, ASTAR_POS_FINISH_X, ASTAR_POS_FINISH_Y); \
ASTAR_HEAP_PUSH(nnext_index, new_cost); \
} \
} while (0)
#define ASTAR_WALK_H(ox, oy, dx) \
do { \
int px = ox + (dx); \
int py = oy; \
int visit = 0; \
while (ASTAR_MAP_IS_FREE(px, py)) \
{ \
if ((px == ASTAR_POS_FINISH_X && py == ASTAR_POS_FINISH_Y) || \
(ASTAR_MAP_IS_FREE(px, py-1) && !ASTAR_MAP_IS_FREE(px-(dx), py-1)) || \
(ASTAR_MAP_IS_FREE(px, py+1) && !ASTAR_MAP_IS_FREE(px-(dx), py+1))) \
{ \
visit = 1; \
break; \
} \
px += (dx); \
} \
if (visit) \
{ \
ASTAR_VISIT(ox, oy, px, py); \
} \
} while (0)
#define ASTAR_WALK_V(ox, oy, dy) \
do { \
int px = ox; \
int py = oy + (dy); \
int visit = 0; \
while (ASTAR_MAP_IS_FREE(px, py)) \
{ \
if ((px == ASTAR_POS_FINISH_X && py == ASTAR_POS_FINISH_Y) || \
(ASTAR_MAP_IS_FREE(px-1, py) && !ASTAR_MAP_IS_FREE(px-1, py-(dy))) || \
(ASTAR_MAP_IS_FREE(px+1, py) && !ASTAR_MAP_IS_FREE(px+1, py-(dy)))) \
{ \
visit = 1; \
break; \
} \
for (int dx=-1; dx<=+1; dx+=2) \
{ \
int hx = px + dx; \
int hy = py; \
while (ASTAR_MAP_IS_FREE(hx, hy)) \
{ \
if ((hx == ASTAR_POS_FINISH_X && hy == ASTAR_POS_FINISH_Y) || \
(ASTAR_MAP_IS_FREE(hx, hy-1) && !ASTAR_MAP_IS_FREE(hx-dx, hy-1)) || \
(ASTAR_MAP_IS_FREE(hx, hy+1) && !ASTAR_MAP_IS_FREE(hx-dx, hy+1))) \
{ \
visit = 1; \
break; \
} \
hx += dx; \
} \
if (visit) break; \
} \
if (visit) break; \
py += (dy); \
} \
if (visit) \
{ \
ASTAR_VISIT(ox, oy, px, py); \
} \
} while (0)
typedef union {
struct {
unsigned int cost : 31;
unsigned int visited : 1;
unsigned int parent : 32;
};
unsigned long long all;
} node;
typedef struct {
unsigned int index;
unsigned int cost;
} heapnode;
if (ASTAR_TEMP_SIZE >= sizeof(node) * (ASTAR_WIDTH * ASTAR_HEIGHT) + sizeof(heapnode))
{
node* nodes = (node*)ASTAR_TEMP;
for (unsigned int i = 0; i < (ASTAR_WIDTH * ASTAR_HEIGHT); i++)
{
nodes[i].all = 0;
}
heapnode* heap = (heapnode*)((char*)ASTAR_TEMP + sizeof(node) * (ASTAR_WIDTH * ASTAR_HEIGHT));
unsigned int heap_max = (ASTAR_TEMP_SIZE - sizeof(node) * (ASTAR_WIDTH * ASTAR_HEIGHT)) / sizeof(heapnode);
int heap_count = 0;
unsigned int nindex = ASTAR_INDEX(ASTAR_POS_START_X, ASTAR_POS_START_Y);
node* n = nodes + nindex;
n->cost = 0;
n->visited = 1;
ASTAR_HEAP_PUSH(nindex, ASTAR_COST(ASTAR_POS_START_X, ASTAR_POS_START_Y, ASTAR_POS_FINISH_X, ASTAR_POS_FINISH_Y));
int found = 0;
while (heap_count != 0)
{
nindex = heap[0].index;
n = nodes + nindex;
ASTAR_HEAP_POP();
int x = nindex % ASTAR_WIDTH;
int y = nindex / ASTAR_WIDTH;
if (x == ASTAR_POS_FINISH_X && y == ASTAR_POS_FINISH_Y)
{
found = 1;
break;
}
n->visited = 1;
if (x == ASTAR_POS_START_X && y == ASTAR_POS_START_Y)
{
ASTAR_WALK_V(x, y, -1);
ASTAR_WALK_V(x, y, +1);
ASTAR_WALK_H(x, y, -1);
ASTAR_WALK_H(x, y, +1);
}
else
{
int px = n->parent % ASTAR_WIDTH;
int py = n->parent / ASTAR_WIDTH;
int dx = x < px ? -1 : x > px ? +1 : 0;
int dy = y < py ? -1 : y > py ? +1 : 0;
if (dx != 0)
{
ASTAR_WALK_V(x, y, -1);
ASTAR_WALK_V(x, y, +1);
ASTAR_WALK_H(x, y, dx);
}
else if (dy != 0)
{
ASTAR_WALK_H(x, y, -1);
ASTAR_WALK_H(x, y, +1);
ASTAR_WALK_V(x, y, dy);
}
}
}
bail:
if (found)
{
#if ASTAR_PATH_REDUCED
int x = ASTAR_POS_FINISH_X;
int y = ASTAR_POS_FINISH_Y;
ASTAR_PATH(x, y);
while (x != ASTAR_POS_START_X || y != ASTAR_POS_START_Y)
{
x = n->parent % ASTAR_WIDTH;
y = n->parent / ASTAR_WIDTH;
n = nodes + n->parent;
ASTAR_PATH(x, y);
}
#else
int x = ASTAR_POS_FINISH_X;
int y = ASTAR_POS_FINISH_Y;
for (;;)
{
int px = n->parent % ASTAR_WIDTH;
int py = n->parent / ASTAR_WIDTH;
int dx = (x > px) ? -1 : (x < px) ? +1 : 0;
int dy = (y > py) ? -1 : (y < py) ? +1 : 0;
while (x != px)
{
x += dx;
ASTAR_PATH(x, y);
}
while (y != py)
{
y += dy;
ASTAR_PATH(x, y);
}
if (x == ASTAR_POS_START_X && y == ASTAR_POS_START_Y)
{
break;
}
n = nodes + n->parent;
ASTAR_PATH(x, y);
}
#endif
}
}
else
{
// not enough temp memory
}
#undef ASTAR_POS_START_X
#undef ASTAR_POS_START_Y
#undef ASTAR_POS_FINISH_X
#undef ASTAR_POS_FINISH_Y
#undef ASTAR_WIDTH
#undef ASTAR_HEIGHT
#undef ASTAR_MAP_IS_FREE
#undef ASTAR_COST
#undef ASTAR_PATH
#undef ASTAR_TEMP
#undef ASTAR_TEMP_SIZE
#undef ASTAR_PATH_REDUCED
#undef ASTAR_INDEX
#undef ASTAR_HEAP_SWAP
#undef ASTAR_HEAP_PUSH
#undef ASTAR_HEAP_POP
#undef ASTAR_VISIT
#undef ASTAR_WALK_H
#undef ASTAR_WALK_V
Raw
astar_jps2d_test.c
//
// first download stb_image_write.h to same folder as astar_test.c
// from https://raw.githubusercontent.com/nothings/stb/master/stb_image_write.h
//
// compile: cl.exe /O2 astar_jps2d_test.c
// run: astar_jps2d_test.exe
//
// outputs astar_jps2d.bmp file
#include <stdint.h>
#include <stddef.h>
#include <stdbool.h>
#include <stdlib.h>
typedef struct {
int x;
int y;
} pos;
char* global_for_debug_visualization;
static int astar_path(int width, int height, const unsigned* map, pos src, pos dst, pos* path, size_t maxpath)
{
#define ASTAR_POS_START_X src.x
#define ASTAR_POS_START_Y src.y
#define ASTAR_POS_FINISH_X dst.x
#define ASTAR_POS_FINISH_Y dst.y
#define ASTAR_WIDTH width
#define ASTAR_HEIGHT height
#define ASTAR_MAP_IS_FREE(x, y) ((y) >= 0 && (y) < height && (x) >= 0 && (x) < width && (char)map[(y) * width + (x)] == 0)
#define ASTAR_COST(ax, ay, bx, by) (abs((ax) - (bx)) + abs((ay) - (by)))
size_t path_count = 0;
#define ASTAR_PATH(x,y) \
if (path_count < maxpath) \
{ \
path[path_count].x = x; \
path[path_count].y = y; \
path_count++; \
}
// 16MB of temp memory, not thread-safe for this example
#define ASTAR_TEMP_SIZE (16 * 1024 * 1024)
#define ASTAR_TEMP temp
static char ASTAR_TEMP[ASTAR_TEMP_SIZE];
global_for_debug_visualization = ASTAR_TEMP; // remove this line if you don't need visualize A* visited nodes
#include "astar_jps2d.h"
return path_count;
}
#define STB_IMAGE_WRITE_STATIC
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"
#include <stdlib.h>
#include <stdio.h>
#include <windows.h>
#define W 1000
#define H 1000
int main()
{
static unsigned map[W * H];
// generate random map
{
srand(0);
for (int i = 0; i < W * H; i++)
{
map[i] = 0xff000000;
}
for (int i = 0; i < (W + H) / 2; i++)
{
int x = rand() % W;
int y = rand() % H;
int w = 1 + (rand() % (W / 25));
int h = 1 + (rand() % (H / 25));
for (int xi = 0; xi < w; xi++)
{
for (int yi = 0; yi < h; yi++)
{
if (y + yi < H && x + xi < W)
{
map[(y + yi) * W + (x + xi)] = 0xffffffff;
}
}
}
}
#if 0
for (int i = 0; i < W * H / 20; i++)
{
int x = rand() % W;
int y = rand() % H;
map[y * W + x] = 0xffffffff;
}
#endif
map[0] = map[(H - 1) * W + W - 1] = 0xff000000;
}
pos src = { 0, 0 };
pos dst = { W - 1, H - 1 };
LARGE_INTEGER f, c1, c2;
QueryPerformanceFrequency(&f);
QueryPerformanceCounter(&c1);
static pos path[10000];
int path_count = astar_path(W, H, map, src, dst, path, sizeof(path) / sizeof(path[0]));
QueryPerformanceCounter(&c2);
printf("Time = %.2f msec\n", 1000.0 * (c2.QuadPart - c1.QuadPart) / f.QuadPart);
if (path_count == 0)
{
printf("Path not found!\n");
}
else
{
// debug visualization for A* visited nodes
typedef struct node {
unsigned int cost : 31;
unsigned int visited : 1;
unsigned int parent : 32;
} node;
node* n = (node*)global_for_debug_visualization;
unsigned int visited = 0;
for (int y = 0; y < H; y++)
{
for (int x = 0; x < W; x++)
{
if (n[y * W + x].visited)
{
map[y * W + x] = 0xff008000;
visited++;
}
}
}
printf("Visited %u cells (%.2f%%)\n", visited, visited * 100.0 / (W * H));
//
// path visualization
pos* p = &path[path_count - 1];
for (int i = 0; i < path_count; i++)
{
map[p->y * W + p->x] = 0xff0000ff;
--p;
}
//
}
stbi_write_bmp("astar_jps2d.bmp", W, H, 4, map);
}
Raw
astar_test.c
//
// first download stb_image_write.h to same folder as astar_test.c
// from https://raw.githubusercontent.com/nothings/stb/master/stb_image_write.h
//
// compile: cl.exe /O2 astar_test.c
// run: astar_test.exe
//
// outputs astar.bmp file: https://i.imgur.com/HKsYvtN.png
// with diagonal movement: https://i.imgur.com/FVN8BkT.png
#include <stdint.h>
#include <stddef.h>
#include <stdbool.h>
#include <stdlib.h>
typedef struct {
int x;
int y;
} pos;
char* global_for_debug_visualization;
static int astar_path(int width, int height, const unsigned* map, pos src, pos dst, pos* path, size_t maxpath)
{
#define ALLOW_DIAGONAL_MOVEMENT 0
#if ALLOW_DIAGONAL_MOVEMENT
#define ASTAR_DIR_COUNT 8
#else
#define ASTAR_DIR_COUNT 4
#endif
static const pos dir[ASTAR_DIR_COUNT] =
{
{ 1, 0 },
{ 0, 1 },
{ -1, 0 },
{ 0, -1 },
#if ALLOW_DIAGONAL_MOVEMENT
{ 1, 1 },
{ 1, -1 },
{ -1, 1 },
{ -1, -1 },
#endif
};
#define ASTAR_POS_TYPE pos
#define ASTAR_POS_START src
#define ASTAR_POS_FINISH dst
#define ASTAR_POS_INDEX(p) ((p).y * width + (p).x)
#define ASTAR_MAX_INDEX (width * height)
#define ASTAR_INDEX_POS(p, i) \
do { \
(p).x = (i) % width; \
(p).y = (i) / width; \
} while (0)
#define ASTAR_POS_EQUAL(a, b) ((a).x == (b).x && (a).y == (b).y)
#define ASTAR_MAP_IS_FREE(p) ((p).y >= 0 && (p).y < height && (p).x >= 0 && (p).x < width && (char)map[(p).y * width + (p).x] == 0)
#define ASTAR_NEXT_POS(p, i) \
do { \
(p).x += dir[i].x; \
(p).y += dir[i].y; \
} while (0)
#define ASTAR_PREV_POS(p, i) \
do { \
(p).x -= dir[i].x; \
(p).y -= dir[i].y; \
} while (0)
#define ASTAR_GET_COST(a, b) (abs((a).x - (b).x) + abs((a).y - (b).y))
#if ALLOW_DIAGONAL_MOVEMENT
#define ASTAR_EXTRA_COST(i) (i < 4 ? 5 : 7) // 7/5 is approx sqrt(2)
#define ASTAR_COST_MUL 5
#endif
size_t path_count = 0;
#define ASTAR_PATH(p) if (path_count < maxpath) path[path_count++] = p
// 16MB of temp memory, not thread-safe for this example
#define ASTAR_TEMP_SIZE (16 * 1024 * 1024)
#define ASTAR_TEMP temp
static char ASTAR_TEMP[ASTAR_TEMP_SIZE];
global_for_debug_visualization = ASTAR_TEMP; // remove this line if you don't need visualize A* visited nodes
#include "astar.h"
return path_count;
}
#define STB_IMAGE_WRITE_STATIC
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "stb_image_write.h"
#include <stdlib.h>
#include <stdio.h>
#include <windows.h>
#define W 1000
#define H 1000
int main()
{
static unsigned map[W * H];
// generate random map
{
srand(0);
for (int i = 0; i < W * H; i++)
{
map[i] = 0xff000000;
}
for (int i = 0; i < (W + H) / 2; i++)
{
int x = rand() % W;
int y = rand() % H;
int w = 1 + (rand() % (W / 25));
int h = 1 + (rand() % (H / 25));
for (int xi = 0; xi < w; xi++)
{
for (int yi = 0; yi < h; yi++)
{
if (y + yi < H && x + xi < W)
{
map[(y + yi) * W + (x + xi)] = 0xffffffff;
}
}
}
}
#if 0
for (int i = 0; i < W * H / 20; i++)
{
int x = rand() % W;
int y = rand() % H;
map[y * W + x] = 0xffffffff;
}
#endif
map[0] = map[(H - 1) * W + W - 1] = 0xff000000;
}
pos src = { 0, 0 };
pos dst = { W - 1, H - 1 };
LARGE_INTEGER f, c1, c2;
QueryPerformanceFrequency(&f);
QueryPerformanceCounter(&c1);
static pos path[10000];
int path_count = astar_path(W, H, map, src, dst, path, sizeof(path) / sizeof(path[0]));
QueryPerformanceCounter(&c2);
printf("Time = %.2f msec\n", 1000.0 * (c2.QuadPart - c1.QuadPart) / f.QuadPart);
if (path_count == 0)
{
printf("Path not found!\n");
}
else
{
// debug visualization for A* visited nodes
typedef struct node {
unsigned cost : 29;
unsigned dir : 2;
unsigned visited : 1;
} node;
node* n = (node*)global_for_debug_visualization;
unsigned int visited = 0;
for (int y = 0; y < H; y++)
{
for (int x = 0; x < W; x++)
{
if (n[y * W + x].visited)
{
map[y * W + x] = 0xff008000;
visited++;
}
}
}
printf("Visited %u cells (%.2f%%)\n", visited, visited * 100.0 / (W * H));
//
// path visualization
pos* p = &path[path_count - 1];
for (int i = 0; i < path_count; i++)
{
map[p->y * W + p->x] = 0xff0000ff;
--p;
}
//
}
stbi_write_bmp("astar.bmp", W, H, 4, map);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment