Búsqueda de rutas A * y ALT en consejos C

Question

Agregué algo de búsqueda de caminos a un juego en el que estoy trabajando. Usó principalmente A * con como se sugiere en los artículos de búsqueda de caminos en los juegos reb blob .
Funciona, pero no es muy rápido.
Es un mapa de cuadrícula cuadrada que (por el momento) tiene un costo de movimiento uniforme, pero en el futuro agregaré pesos que hagan que los caminos eviten las unidades enemigas, etc.
Aquí hay un código:

Aquí está mi encabezado de cola FIFO, fuertemente influenciado por stb stretch_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

Y mi cola de prioridad:

#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

Y finalmente, el código en sí (al menos la mayor parte; corté las cosas específicas del juego):

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
}

Los obstáculos del mapa serán bastante dinámicos y cambiarán a lo largo del juego, por lo que los puntos de referencia que se colocaron en el editor de mapas posiblemente se volverán menos útiles o estarán completamente rodeados de malas hierbas.
Se agradecerían sugerencias / métodos / recursos para colocar puntos de referencia dinámicamente y hacer que mi código sea más rápido / más bonito en general.

Una idea que tuve es tener una matriz del tamaño del mapa que contiene el índice de la ubicación del montón de los mosaicos respectivos, por lo que podría cambiar la prioridad de un elemento como este:

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();
}

Y agregaría esa matriz pqueuepara que de alguna manera se actualice al presionar / hacer estallar / acumular.

También puede valer la pena señalar que los mapas probablemente tengan entre 64x64 tiels (mapa pequeño) y 512x512 mosaicos (mapa enorme).

Answer 1

Entonces, una cosa en la que pensé es basar el tamaño de la cola de prioridad en la heurística en lugar del tamaño del mapa:

const int pq_size = heuristic(start, goal, map_size) * 3;

Además, cuando la cola de prioridad se desborde, solo reescribe el último elemento si el nuevo es mejor:

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 --;
}