312 lines
8.8 KiB
C
312 lines
8.8 KiB
C
#include "pathfinding.h"
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
void pfg_all2one_free(pfg_all2one_t *paths)
|
|
{
|
|
free(paths->direction);
|
|
free(paths->distance);
|
|
}
|
|
|
|
void pfg_path_free(pfg_path_t *grid_path)
|
|
{
|
|
free(grid_path->points);
|
|
}
|
|
|
|
//---
|
|
// BFS algorithm
|
|
//---
|
|
|
|
struct point {
|
|
uint8_t x, y;
|
|
};
|
|
|
|
pfg_all2one_t pfg_bfs(map_t const *map, ivec2 center)
|
|
{
|
|
pfg_all2one_t paths = { .map = map, .x = center.x, .y = center.y };
|
|
int size = map->width * map->height;
|
|
|
|
#define idx(x, y) ((y) * map->width + (x))
|
|
int center_i = idx(center.x, center.y);
|
|
|
|
/* These correspond to the opposites of the directions of <geometry.h> */
|
|
static int dx_array[4] = { 0, -1, 0, +1 };
|
|
static int dy_array[4] = { +1, 0, -1, 0 };
|
|
|
|
/* Distance and shortest path information for each node */
|
|
paths.direction = malloc(size * sizeof *paths.direction);
|
|
paths.distance = malloc(size * sizeof *paths.distance);
|
|
/* Queue of nodes to check */
|
|
struct point *queue = malloc(size * sizeof *queue);
|
|
int queue_start = 0, queue_end = 0;
|
|
|
|
if(!paths.direction || !paths.distance || !queue) {
|
|
pfg_all2one_free(&paths);
|
|
paths.direction = NULL;
|
|
paths.distance = NULL;
|
|
free(queue);
|
|
return paths;
|
|
}
|
|
|
|
/* Initialize directions and distance */
|
|
memset(paths.direction, 0xff, size * sizeof *paths.direction);
|
|
memset(paths.distance, 0xff, size * sizeof *paths.distance);
|
|
paths.distance[center_i] = 0;
|
|
paths.direction[center_i] = UP;
|
|
/* Initialize queue */
|
|
queue[queue_end++] = (struct point){ center.x, center.y };
|
|
|
|
/* Explore nodes listed in the queue */
|
|
while(queue_start < queue_end) {
|
|
struct point point = queue[queue_start++];
|
|
int point_i = idx(point.x, point.y);
|
|
|
|
for(int dir = 0; dir < 4; dir++) {
|
|
int dx = dx_array[dir];
|
|
int dy = dy_array[dir];
|
|
|
|
tile_t *tile = map_tile(map, point.x+dx, point.y+dy);
|
|
if(!tile || tile->solid) continue;
|
|
|
|
int next_i = idx(point.x+dx, point.y+dy);
|
|
if(paths.distance[next_i] != -1) continue;
|
|
|
|
/* The direction to the center is the opposite of [dx;dy], which
|
|
is [dir] due to how [dx_array] and [dy_array] are laid out */
|
|
paths.distance[next_i] = paths.distance[point_i] + 1;
|
|
paths.direction[next_i] = dir;
|
|
|
|
queue[queue_end++] = (struct point){ point.x+dx, point.y+dy };
|
|
}
|
|
}
|
|
|
|
free(queue);
|
|
return paths;
|
|
#undef idx
|
|
}
|
|
|
|
//---
|
|
// Path generation from the BFS
|
|
//---
|
|
|
|
pfg_path_t pfg_bfs_inwards(pfg_all2one_t const *field, ivec2 p)
|
|
{
|
|
#define idx(x, y) ((y) * field->map->width + (x))
|
|
|
|
pfg_path_t path = { field->map, field->distance[idx(p.x, p.y)], NULL };
|
|
if(path.length < 0 || !field->distance) return path;
|
|
|
|
path.points = malloc((path.length + 1) * sizeof *path.points);
|
|
if(!path.points) return path;
|
|
|
|
for(int i = 0; i <= path.length; i++) {
|
|
path.points[i] = p;
|
|
|
|
vec2 dir = fdir(field->direction[idx(p.x,p.y)]);
|
|
p.x += ffloor(dir.x);
|
|
p.y += ffloor(dir.y);
|
|
}
|
|
|
|
return path;
|
|
#undef idx
|
|
}
|
|
|
|
pfg_path_t pfg_bfs_outwards(pfg_all2one_t const *field, ivec2 p)
|
|
{
|
|
pfg_path_t path = pfg_bfs_inwards(field, p);
|
|
if(!path.points) return path;
|
|
|
|
/* Invert all steps along the path */
|
|
for(int i = 0; i <= path.length / 2; i++) {
|
|
int j=i, k=path.length-i;
|
|
|
|
ivec2 tmp = path.points[j];
|
|
path.points[j] = path.points[k];
|
|
path.points[k] = tmp;
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
//---
|
|
// Raycasting tools
|
|
//---
|
|
|
|
int raycast_clear_points = 0;
|
|
vec2 *raycast_clear_p;
|
|
|
|
bool raycast_clear(map_t const *map, vec2 start, vec2 end)
|
|
{
|
|
vec2 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));
|
|
tile_t *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);
|
|
|
|
tile_t *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));
|
|
|
|
tile_t *tile = map_tile(map, next_x, next_y);
|
|
if(tile && tile->solid) return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
int rch_c1=0, rch_c2=0;
|
|
vec2 rch_p1[64], rch_p2[64];
|
|
|
|
bool raycast_clear_hitbox(map_t const *map, vec2 start, vec2 end,
|
|
rect hitbox)
|
|
{
|
|
vec2 dir = { end.x - start.x, end.y - start.y };
|
|
|
|
/* Opposite corners */
|
|
vec2 p1, p2;
|
|
|
|
/* Select the corners of the hitbox */
|
|
if(((dir.x >= 0) ^ (dir.y >= 0)) == 0) {
|
|
p1 = (vec2){ hitbox.r, hitbox.t };
|
|
p2 = (vec2){ hitbox.l, hitbox.b };
|
|
}
|
|
else {
|
|
p1 = (vec2){ hitbox.l, hitbox.t };
|
|
p2 = (vec2){ hitbox.r, hitbox.b };
|
|
}
|
|
|
|
vec2 s1 = { start.x + p1.x, start.y + p1.y };
|
|
vec2 s2 = { start.x + p2.x, start.y + p2.y };
|
|
vec2 e1 = { end.x + p1.x, end.y + p1.y };
|
|
vec2 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
|
|
//---
|
|
|
|
void pfc_path_free(pfc_path_t *path)
|
|
{
|
|
free(path->points);
|
|
}
|
|
|
|
pfc_path_t pfc_shortcut_full(pfg_path_t const *grid, vec2 start,
|
|
vec2 end, rect hitbox)
|
|
{
|
|
pfc_path_t path = { grid->map, 0, NULL };
|
|
if(!grid->points) return path;
|
|
|
|
/* Allocate enough points to avoid doing the calculation twice */
|
|
path.points = malloc((grid->length + 3) * sizeof *path.points);
|
|
if(!path.points) return path;
|
|
|
|
int current = -1;
|
|
path.points[path.length] = start;
|
|
|
|
/* 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) {
|
|
vec2 p1 = (current == -1) ? start :
|
|
vec_i2f_center(grid->points[current]);
|
|
bool made_progress = false;
|
|
|
|
for(int next = grid->length + 1; next > current; next--) {
|
|
vec2 p2 = (next == grid->length + 1) ? end :
|
|
vec_i2f_center(grid->points[next]);
|
|
|
|
if(raycast_clear_hitbox(path.map, p1, p2, hitbox)) {
|
|
path.points[++path.length] = 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) return path;
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
vec2 pfc_shortcut_one(pfg_path_t const *grid, vec2 start, vec2 end,
|
|
rect hitbox)
|
|
{
|
|
vec2 target = start;
|
|
if(!grid->points) return target;
|
|
|
|
/* Find the furthest point that can be reached */
|
|
for(int next = grid->length + 1; next >= 0; next--) {
|
|
vec2 p2 = (next == grid->length + 1) ? end :
|
|
vec_i2f_center(grid->points[next]);
|
|
|
|
if(raycast_clear_hitbox(grid->map, start, p2, hitbox)) {
|
|
target = p2;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return target;
|
|
}
|