Pathfinding A * e ALT nei suggerimenti C.
Ho aggiunto un po 'di pathfinding a un gioco su cui sto lavorando. Ha usato principalmente A * con come suggerito negli articoli di pathfinding nei giochi reb blob .
Funziona, ma non è molto veloce.
È una mappa a griglia quadrata che (al momento) ha un costo di movimento uniforme, ma in futuro aggiungerò pesi per evitare che i percorsi evitino le unità nemiche ecc.
Ecco un codice:
Ecco la mia intestazione FIFO-queue, fortemente influenzata da 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
E la mia coda prioritaria:
#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
E infine, il codice stesso (almeno la maggior parte; ho tagliato le cose specifiche del gioco):
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
}
Gli ostacoli sulla mappa saranno abbastanza dinamici e cambieranno nel corso del gioco, quindi i punti di riferimento che sono stati posizionati nell'editor della mappa potrebbero diventare meno utili o completamente circondati da erbacce.
Suggerimenti / metodi / risorse per posizionare dinamicamente punti di riferimento e rendere il mio codice più veloce / più carino in generale sarebbero apprezzati.
Un'idea che ho avuto è quella di avere un array delle dimensioni della mappa che contiene l'indice della posizione dell'heap delle rispettive tessere, che in modo da poter modificare la priorità di un elemento come questo:
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();
}
E aggiungerei quell'array in pqueue
modo che in qualche modo venga aggiornato durante il push / popping / heapifying.
Potrebbe anche valere la pena notare che le mappe sono probabilmente comprese tra 64x64 livelli (mappa minuscola) e 512x512 tessere (mappa enorme).
Risposte
Quindi una cosa a cui ho pensato è basare la dimensione della coda di priorità sull'euristica piuttosto che sulla dimensione della mappa:
const int pq_size = heuristic(start, goal, map_size) * 3;
Inoltre, quando la coda di priorità va in overflow, riscrivi solo l'ultimo elemento se quello nuovo è migliore:
if (h->count >= h->size) {
printf("Overflowing pqueue of with %d elements! Last element as priority of %d\n", h->size, h->data[h->count-1].p);
if (h->data[h->count-1] <= p) {
return;
}
h->count --;
}