I adding some pathfinding to a game I'm working on. It primarily used A* with as suggested in the pathfinding articles at reb blob games.
It works, but isn't very fast.
It is a square grid map that (at the moment) has uniform movement cost, but in the future I will to add weights that make paths avoid enemy units etc.
Here is some code:
Here is my FIFO-queue header, heavily influenced by the stb stretchy_buffer.h:
#ifndef QUEUE_H
#define QUEUE_H
#include <stdlib.h>
#include <string.h>
#include <assert.h>
// Entire data block
#define queue_raw(a) ((int*) (a)-3)
// Number of elements queue can hold
#define queue__s(a) (queue_raw(a)[0])
// Index of the first element
#define queue__f(a) (queue_raw(a)[1])
// Number of queued elements
#define queue__c(a) (queue_raw(a)[2])
#define queue_count(a) ((a) ? queue__c(a) : 0)
#define queue_empty(a) (queue_count(a)==0)
#define queue_push(a,v) (queue__maybegrow(a,1), (a)[queue__norm(a, (queue__f(a)+(queue__c(a)++)))]=v)
#define queue_append(a,n) (queue__maybegrow(a,n), queue__c(a)+=(n), &(a)[queue__c(a)-n])
#define queue_peek(a) ((a) ? (a)[queue__f(a)] : 0)
#define queue_pop(a) (queue_empty(a) ? 0 : (queue__c(a)--, queue__f(a)=queue__norm(a,queue__f(a)+1), ((a)[queue__f(a) ? queue__f(a)-1 : queue__s(a)-1])))
#define queue_last(a) (queue_empty(a) ? 0 : (a)[queue__norm(queue__f(a)+queue__c(a))])
#define queue_poplast(a) (queue_empty(a) ? 0 : (queue__c(a)--, (a)[queue__norm(queue__f(a)+queue__c(a))]))
#define queue_free(a) ((a) ? free(queue_raw(a)),0 : 0)
#define queue__norm(a,i) (((i)%queue__s(a)+queue__s(a))%queue__s(a))
#define queue__grow(a,n) queue__growf((void*) &(a), (n), sizeof(*(a)))
#define queue__needgrow(a,n) ((a)==0 || queue_count(a)+n > queue__s(a))
#define queue_resize(a,n) (queue__maybegrow((a),(n)))
#define queue__maybegrow(a,n) (queue__needgrow((a),(n)) ? queue__grow((a),(n)) : (void)0)
static void queue__growf(void** arr, int increment, size_t itemsize) {
// Grow the size of *arr by increments*itemsize bytes.
// Does not change queue__c(*arr)
int c = queue_count(*arr);
if (*arr && !c) queue__f(*arr) = 0;
int s = *arr ? queue__s(*arr) : 0;
int f = *arr ? queue__f(*arr) : 0;
int m = c + increment;
assert(m > s);
if (f) {
// Reallocate the queue with the first element at index 0
void* buf = malloc(itemsize*m + sizeof(int)*3);
assert(buf);
if (buf) {
void* arr_buf = (void*) ((int*) buf + 3);
if (f + c <= s) {
memcpy(arr_buf, (unsigned char*)(*arr) + f*itemsize, itemsize * c);
} else {
memcpy(arr_buf, (unsigned char*)(*arr) + f*itemsize, itemsize * (s-f));
memcpy((unsigned char*) arr_buf + itemsize*(s-f), *arr, itemsize * (f+c-s));
}
queue__s(arr_buf) = m;
queue__f(arr_buf) = 0;
queue__c(arr_buf) = c;
queue_free(*arr);
*arr = arr_buf;
}
} else {
void* buf = realloc(*arr ? queue_raw(*arr) : 0, itemsize*m + sizeof(int)*3);
assert(buf);
if (buf) {
*arr = (void*) ((int*) buf + 3);
queue__s(*arr) = m;
queue__f(*arr) = 0;
queue__c(*arr) = c;
}
}
}
#endif
And my priority queue:
#ifndef PRIORITY_QUEUE_H
#define PRIORITY_QUEUE_H
typedef struct {
int v;
int p;
} pqueue_pair;
struct pqueue {
int size;
int count;
pqueue_pair* data;
};
void pqueue_push(struct pqueue* h, int v, int p);
int pqueue_pop(struct pqueue* h);
#endif
#ifdef PRIORITY_QUEUE_IMPLEMENTATION
static inline void swap(pqueue_pair* a, pqueue_pair* b) {
pqueue_pair tmp;
memcpy(&tmp, a, sizeof(pqueue_pair));
memcpy(a, b, sizeof(pqueue_pair));
memcpy(b, &tmp, sizeof(pqueue_pair));
}
static void heapify(struct pqueue* h, int i) {
int largest = i;
while (true) {
int l = 2*i + 1;
int r = l + 1;
if (l < h->count && h->data[l].p < h->data[largest].p) largest = l;
if (r < h->count && h->data[r].p < h->data[largest].p) largest = r;
if (largest != i) {
swap(h->data+largest, h->data+i);
i = largest;
} else {
break;
}
}
}
void pqueue_push(struct pqueue* h, int v, int p) {
if (h->count >= h->size) {
h->count --;
printf("Overflowing pqueue of with %d elements! Last element as priority of %d\n", h->size, h->data[h->count].p);
}
h->data[h->count].v = v;
h->data[h->count].p = p;
h->count ++;
if (h->count > 1) {
for (int i=h->count/2-1; i>=0; i--) {
heapify(h, i);
}
}
}
int pqueue_pop(struct pqueue* h) {
assert(h->count);
int v = h->data[0].v;
h->count --;
memcpy(h->data, h->data+h->count, sizeof(pqueue_pair));
if (h->count > 1) {
heapify(h, 0);
}
return v;
}
#endif
#endif
And finally, the code itself (at least most of it; I cut the game-specific stuff):
uint8_t* obstacles = 0;
unsigned int obstacles_size = 0;
#define MAX_LANDMARK_DISTANCE 0xff
uint8_t* landmarks = 0;
int* landmark_positions = 0;
int num_landmarks = 0;
int landmark_size = 0;
// Functions for but shifting into an array of single-bit bools.
// I don't know if the speed difference compared to normal
// indexing, but I assume the size difference is worth it?
static inline uint8_t get_obstacle(int i) {
assert(i/8 < obstacles_size);
return obstacles[i/8] & (1 << i%8);
}
static inline void set_obstacle(int i) {
assert(i/8 < obstacles_size);
obstacles[i/8] |= 1 << i % 8;
}
static inline void unset_obstacle(int i) {
assert(i/8 < obstacles_size);
obstacles[i/8] = ~((~obstacles[i/8]) | 1 << i%8);
}
static int get_neighbors(int* neighbors, int i, int s) {
// Fill neighbors with flattened coords of tiles adjacent to i and return the count
assert(i >= 0 && i < s*s && s >= 0);
int x = i % s;
int y = i / s;
int count = 0;
if (x > 0) neighbors[count++] = i-1; // East
if (x < s-1) neighbors[count++] = i+1; // West
if (y > 0) neighbors[count++] = i-s; // North
if (y < s-1) neighbors[count++] = i+s; // South
return count;
}
void update_map(/* Game-specific arguments */) {
// This function is called every time the map
// changes, (i.e., wall is remove, building added/destroyed)
// It happens fairly often.
// Update obstacles here, and allocates them if need be
// Update the landmarks
#define L(i) (landmarks + (i)*landmark_size)
// This part here is rather slow
memset(landmarks, 0xff, num_landmarks*landmark_size*sizeof(*landmarks));
for (int l=0; l<num_landmarks; l++) {
assert(landmark_positions[l] >= 0 && landmark_positions[l] < size);
L(l)[landmark_positions[l]] = 0;
int* queue = 0;
queue_resize(queue, map->size * 3);
queue_push(queue, landmark_positions[l]);
while (queue_count(queue)) {
int current = queue_pop(queue);
assert(L(l)[current] < MAX_LANDMARK_DISTANCE);
int neighbors[4];
int neighbors_count = get_neighbors(neighbors, current, map->size);
for (int n=0; n<neighbors_count; n++) {
int next = neighbors[n];
if (get_obstacle(next)) continue;
int new_cost = L(l)[current] + 1;
if (new_cost < L(l)[next]) {
L(l)[next] = new_cost;
if (new_cost < MAX_LANDMARK_DISTANCE) queue_push(queue, next);
}
}
}
queue_free(queue);
}
#undef L
}
static inline int distance_heuristic(int a, int b, int w) {
return abs(a%w - b%w) + abs(a/w - b/w);
}
static inline int heuristic(int a, int b, int w) {
int d = distance_heuristic(a, b, w);
for (int i=0; i<num_landmarks; i++) {
int da = landmarks[i*landmark_size + a];
int db = landmarks[i*landmark_size + b];
int dd = abs(da - db);
if (dd > d) {
d = dd;
}
}
return d;
}
void nav_path_find(int map_size, int sx, int sy, int gx, int gy, uint16_t* path_out, uint8_t* path_length, uint8_t max_path) {
int start = sy*map->size + sx;
int goal = gy*map->size + gx;
// The maps are always square
int size = map_size * map_size;
const int pq_size = map->size*3;
pqueue_pair pq_data[pq_size];
for (int i=0; i<pq_size; i++) pq_data[i].p = -1;
struct pqueue pq = {.size=pq_size, .count=0, .data=pq_data};
pqueue_push(&pq, start, 1);
// Create the closed list the size of the entire map which stores
// the flattened Cartesian coordinates of the previous tile such that
// y * map_width + x = i
// and
// x == i % map_size && y == (int) i / map_size
int came_from[size];
for (int i=0; i<size; i++) came_from[i] = -1;
came_from[start] = 0;
uint16_t cost[size];
memset(cost, 0xff, sizeof(*cost) * size);
bool found_path = false;
while (pq.count > 0 && !found_path) {
int current = pqueue_pop(&pq);
assert(came_from[current] >= 0);
if (current == goal) {
found_path = true;
}
int neighbors[4];
int neighbors_count = get_neighbors(neighbors, current, map->size);
for (int n=0; n<neighbors_count; n++) {
int next = neighbors[n];
if (get_obstacle(next)) continue;
int new_cost = cost[current] + 1;
if (came_from[next] < 0 || new_cost < cost[next]) {
cost[next] = new_cost;
pqueue_push(&pq, next, new_cost + heuristic(next, goal, map_width));
came_from[next] = current;
}
}
}
// Here we trace the path back and return the first `max_path` steps
}
The map obstacles will be fairly dynamic and change over the course of the game, thus landmarks that were placed in the map editor will possibly become less useful or entirely surrounded with weeds.
Suggestions/methods/resources for dynamically placing landmarks and making my code faster/prettier in general would be appreciated.
One idea I had is to had an array the size of the map that holds the index to the respective tiles' heap location, which so you could change the priority of an item kinda like this:
int pq_indices[size];
for (int i=0; i<size; i++) pq_indices[i] = -1;
// Then later when looping through neighbors
if (pq_indices[next] != -1) {
// Push it
} else {
pq_data[next].priority = new_priority;
pqueue_update();
}
And I would add that array the pqueue so it would somehow get updated when pushing/popping/heapifying.
It also might be worth noting that maps are probably between 64x64 tiels (tiny map) to 512x512 tiles (enormous map).
