RogueLife/src/pathfinding.c

#include "pathfinding.h"

#include <stdlib.h>
#include <string.h>

//---
// BFS algorithm
//---

struct node {
    int16_t dist;
    int16_t prev;
};

ipoint_t *bfs(map_t const *map, ipoint_t start, ipoint_t end, int *count)
{
    int h = map->height;
    int w_shift = 0;
    int w = 1;
    while(w < map->width) w <<= 1, w_shift++;

    #define idx(x, y) (((y) << w_shift) + (x))

    static int dx_array[4] = { -1, +1,  0,  0 };
    static int dy_array[4] = {  0,  0, -1, +1 };

    /* Array keeping track of each node's distance and previous-link */
    struct node *array = malloc(w * h * sizeof *array);
    if(!array) return NULL;

    memset(array, 0xff, (w * h * sizeof *array));
    int i = idx(start.x, start.y);
    array[i].dist = 0;
    array[i].prev = i;

    /* Queue of nodes to check */
    uint16_t *queue = malloc(w * h * sizeof *queue);
    if(!queue) { free(array); return NULL; }
    int queue_start=0, queue_end=0;

    queue[queue_end++] = idx(start.x, start.y);

    /* Explore nodes listed in the queue */
    while(queue_start < queue_end) {
        int current = queue[queue_start++];
        int x = current & (w-1);
        int y = current >> w_shift;
        if(x == end.x && y == end.y) break;

        for(int i = 0; i < 4; i++) {
            int dx = dx_array[i];
            int dy = dy_array[i];

            struct tile *tile = map_tile(map, x+dx, y+dy);
            if(!tile || tile->solid) continue;

            int next = idx(x+dx, y+dy);
            if(array[next].prev != -1) continue;

            array[next].dist = array[current].dist + 1;
            array[next].prev = current;

            queue[queue_end++] = next;
        }
    }

    free(queue);

    /* Generate the path */
    int n = idx(end.x, end.y);
    if(array[n].prev == -1) { free(array); return NULL; }

    *count = array[n].dist;
    ipoint_t *points = malloc((*count + 1) * sizeof *points);

    for(int i = *count; i >= 0; i--) {
        points[i].x = n & (w-1);
        points[i].y = n >> w_shift;
        n = array[n].prev;
    }

    free(array);
    return points;
}

//---
// Ray casting
//---

int raycast_clear_points = 0;
fpoint_t *raycast_clear_p;

bool raycast_clear(map_t const *map, fpoint_t start, fpoint_t end)
{
    fpoint_t u = { end.x - start.x, end.y - start.y };
    if(u.x == 0 && u.y == 0) return true;

    fixed_t inv_ux = u.x ? fdiv(fix(1), u.x) : 0;
    fixed_t inv_uy = u.y ? fdiv(fix(1), u.y) : 0;

    /* Current point is [start + t*u]; when t = 1, we're reached [end] */
    fixed_t t = fix(0);

    raycast_clear_points = 0;

    while(t < fix(1)) {
        fixed_t x = start.x + fmul(t, u.x);
        fixed_t y = start.y + fmul(t, u.y);

        /* Re-check current cell to avoid diagonal clips, where we change tiles
           diagonally in a single stpe (which happens quite often when snapping
           to points with integer or half-integer coordinates) as things align
           perfectly */
        int current_x = ffloor(x-(u.x < 0));
        int current_y = ffloor(y-(u.y < 0));
        struct tile *tile = map_tile(map, current_x, current_y);
        if(tile && tile->solid) return false;

        if(raycast_clear_points < 64) {
            raycast_clear_p[raycast_clear_points].x = x;
            raycast_clear_p[raycast_clear_points].y = y;
        }
        raycast_clear_points++;

        /* Distance to the next horizontal, and vertical line */
        fixed_t dist_y = (u.y >= 0) ? fix(1) - fdec(y) : -(fdec(y-1) + 1);
        fixed_t dist_x = (u.x >= 0) ? fix(1) - fdec(x) : -(fdec(x-1) + 1);

        /* Increase in t that would make us hit a horizontal line */
        fixed_t dty = fmul(dist_y, inv_uy);
        fixed_t dtx = fmul(dist_x, inv_ux);

        /* Move to the next point */
        if(!u.x || (u.y && dty <= dtx)) {
            /* Make sure we don't get stuck, at all costs */
            t += dty + (dty == 0);
            if(t > fix(1)) break;

            int next_x = ffloor(x-(u.x < 0));
            int next_y = ffloor(y-(u.y < 0)) + (u.y >= 0 ? 1 : -1);

            struct tile *tile = map_tile(map, next_x, next_y);
            if(tile && tile->solid) return false;
        }
        else {
            t += dtx + (dtx == 0);
            if(t > fix(1)) break;

            int next_x = ffloor(x-(u.x < 0)) + (u.x >= 0 ? 1 : -1);
            int next_y = ffloor(y-(u.y < 0));

            struct tile *tile = map_tile(map, next_x, next_y);
            if(tile && tile->solid) return false;
        }
    }

    return true;
}

int rch_c1=0, rch_c2=0;
fpoint_t rch_p1[64], rch_p2[64];

bool raycast_clear_hitbox(map_t const *map, fpoint_t start, fpoint_t end,
    frect_t hitbox)
{
    fpoint_t dir = { end.x - start.x, end.y - start.y };

    /* Opposite corners */
    fpoint_t p1, p2;

    /* Select the corners of the hitbox */
    if(((dir.x >= 0) ^ (dir.y >= 0)) == 0) {
        p1 = (fpoint_t){ hitbox.r, hitbox.t };
        p2 = (fpoint_t){ hitbox.l, hitbox.b };
    }
    else {
        p1 = (fpoint_t){ hitbox.l, hitbox.t };
        p2 = (fpoint_t){ hitbox.r, hitbox.b };
    }

    fpoint_t s1 = { start.x + p1.x, start.y + p1.y };
    fpoint_t s2 = { start.x + p2.x, start.y + p2.y };
    fpoint_t e1 = { end.x + p1.x, end.y + p1.y };
    fpoint_t e2 = { end.x + p2.x, end.y + p2.y };

    raycast_clear_p = rch_p1;
    bool b1 = raycast_clear(map, s1, e1);
    rch_c1 = raycast_clear_points;

    raycast_clear_p = rch_p2;
    bool b2 = raycast_clear(map, s2, e2);
    rch_c2 = raycast_clear_points;

    return b1 && b2;
}

//---
// General pathfinding
//---

fpoint_t *pathfind(map_t const *map, fpoint_t start, fpoint_t end, int *count,
    ipoint_t **p_grid, int *p_grid_length, frect_t hitbox)
{
    int grid_length;
    ipoint_t *grid = bfs(map, (ipoint_t){ ffloor(start.x), ffloor(start.y) },
        (ipoint_t){ ffloor(end.x), ffloor(end.y) }, &grid_length);

    if(p_grid) *p_grid = grid;
    if(p_grid_length) *p_grid_length = grid_length;

    if(!grid) return NULL;

    /* Allocate enough points to avoid doing the calculation twice */
    fpoint_t *path = malloc((grid_length+3) * sizeof *path);
    if(!path) goto pathfind_end;

    *count = 0;
    path[*count] = start;

    int current = -1;

    /* Find the furthest point on the integer path which can be reached in a
       straight line, then start again from there */
    while(current <= grid_length) {
        fpoint_t p1 = (current == -1) ? start : (fpoint_t){
            fix(grid[current].x) + fix(1)/2,
            fix(grid[current].y) + fix(1)/2 };
        bool made_progress = false;

        for(int next = grid_length + 1; next > current; next--) {
            fpoint_t p2 = (next == grid_length + 1) ? end : (fpoint_t){
                fix(grid[next].x) + fix(1)/2,
                fix(grid[next].y) + fix(1)/2 };

            if(raycast_clear_hitbox(map, p1, p2, hitbox)) {
                path[++*count] = p2;
                current = next;
                made_progress = true;
                break;
            }
        }

        /* This should never happen because you can always move from start to
           the first grid point, and from the last grid point to the end; plus,
           grid points are trivially adjacent. But for robustness, check */
        if(!made_progress) { free(path); path = NULL; goto pathfind_end; }
    }

pathfind_end:
    if(!p_grid) free(grid);
    return path;
}

fpoint_t pathfind_one(map_t const *map, fpoint_t start, fpoint_t end,
    frect_t hitbox)
{
    fpoint_t target = start;

    int grid_length;
    ipoint_t *grid = bfs(map, (ipoint_t){ ffloor(start.x), ffloor(start.y) },
        (ipoint_t){ ffloor(end.x), ffloor(end.y) }, &grid_length);

    if(!grid) return target;

    /* Find the furthest point that can be reached */
    for(int next = grid_length + 1; next >= 0; next--) {
        fpoint_t p2 = (next == grid_length + 1) ? end : (fpoint_t){
            fix(grid[next].x) + fix(1)/2,
            fix(grid[next].y) + fix(1)/2 };

        if(raycast_clear_hitbox(map, start, p2, hitbox)) {
            target = p2;
            break;
        }
    }

    free(grid);
    return target;
}