364 lines
10 KiB
C
364 lines
10 KiB
C
#include "pathfinding.h"
|
|
#include "game.h"
|
|
#include "util.h"
|
|
#include "comp/physical.h"
|
|
#include "comp/fighter.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;
|
|
int16_t cost;
|
|
uint8_t dir;
|
|
};
|
|
|
|
static int compare_point(void const *_p1, void const *_p2)
|
|
{
|
|
struct point const *p1=_p1, *p2=_p2;
|
|
return p1->cost - p2->cost;
|
|
}
|
|
|
|
pfg_all2one_t pfg_dijkstra(map_t const *map, ivec2 center, uint8_t *occupation)
|
|
{
|
|
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))
|
|
|
|
/* 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 */
|
|
pqueue_t queue = pqueue_alloc(size, sizeof(struct point), compare_point);
|
|
|
|
if(!paths.direction || !paths.distance || !queue.array) {
|
|
pfg_all2one_free(&paths);
|
|
paths.direction = NULL;
|
|
paths.distance = NULL;
|
|
pqueue_destroy(&queue);
|
|
return paths;
|
|
}
|
|
|
|
/* Initialize directions and distance */
|
|
memset(paths.direction, 0xff, size * sizeof *paths.direction);
|
|
memset(paths.distance, 0xff, size * sizeof *paths.distance);
|
|
/* Initialize queue */
|
|
struct point point = { center.x, center.y, 0, UP };
|
|
pqueue_add(&queue, &point);
|
|
|
|
/* Explore nodes listed in the queue */
|
|
while(!pqueue_empty(&queue)) {
|
|
/* Pop a node from the queue; ignore it if we've visited it before */
|
|
pqueue_pop(&queue, &point);
|
|
int point_i = idx(point.x, point.y);
|
|
if(paths.direction[point_i] != -1)
|
|
continue;
|
|
|
|
paths.direction[point_i] = point.dir;
|
|
|
|
for(int dir = 0; dir < 4; dir++) {
|
|
int dx = dx_array[dir];
|
|
int dy = dy_array[dir];
|
|
|
|
map_cell_t *cell = map_cell(map, point.x+dx, point.y+dy);
|
|
if(!cell) continue;
|
|
|
|
if(map->tileset->tiles[cell->base].solid)
|
|
continue;
|
|
if(cell->decor && map->tileset->tiles[cell->decor].solid)
|
|
continue;
|
|
|
|
int next_i = idx(point.x+dx, point.y+dy);
|
|
int occ = occupation ? occupation[next_i] : 0;
|
|
|
|
/* 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 */
|
|
struct point next = { .x=point.x+dx, .y=point.y+dy };
|
|
next.cost = paths.distance[point_i] + 1 + 2*occ;
|
|
|
|
if(paths.distance[next_i]>=0 && paths.distance[next_i]<=next.cost)
|
|
continue;
|
|
paths.distance[next_i] = next.cost;
|
|
|
|
next.dir = dir;
|
|
pqueue_add(&queue, &next);
|
|
}
|
|
}
|
|
|
|
pqueue_destroy(&queue);
|
|
return paths;
|
|
#undef idx
|
|
}
|
|
|
|
//---
|
|
// Path generation from the BFS
|
|
//---
|
|
|
|
pfg_path_t pfg_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_outwards(pfg_all2one_t const *field, ivec2 p)
|
|
{
|
|
pfg_path_t path = pfg_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
|
|
//---
|
|
|
|
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've reached [end] */
|
|
fixed_t t = fix(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 step (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));
|
|
|
|
map_cell_t *cell = map_cell(map, current_x, current_y);
|
|
if(!cell)
|
|
return false;
|
|
if(map->tileset->tiles[cell->base].solid)
|
|
return false;
|
|
if(cell->decor && map->tileset->tiles[cell->decor].solid)
|
|
return false;
|
|
|
|
/* 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);
|
|
|
|
int next_x = current_x;
|
|
int next_y = current_y;
|
|
|
|
/* 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);
|
|
next_y += (u.y >= 0 ? 1 : -1);
|
|
}
|
|
else {
|
|
t += dtx + (dtx == 0);
|
|
next_x += (u.x >= 0 ? 1 : -1);
|
|
}
|
|
|
|
if(t > fix(1)) break;
|
|
cell = map_cell(map, next_x, next_y);
|
|
if(!cell)
|
|
return false;
|
|
if(map->tileset->tiles[cell->base].solid)
|
|
return false;
|
|
if(cell->decor && map->tileset->tiles[cell->decor].solid)
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
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 };
|
|
|
|
return raycast_clear(map, s1, e1) && raycast_clear(map, s2, e2);
|
|
}
|
|
|
|
//---
|
|
// 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 a point that can be reached directly by bisecting the path */
|
|
for(int next = grid->length + 1; next >= 0; next /= 2) {
|
|
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;
|
|
}
|
|
if(next == 0)
|
|
break;
|
|
}
|
|
|
|
return target;
|
|
}
|
|
|
|
//---
|
|
// Auto-aiming
|
|
//---
|
|
|
|
vec2 pathfinding_autoaim(game_t const *game, entity_t *src, map_t const *map)
|
|
{
|
|
physical_t *src_p = getcomp(src, physical);
|
|
if(!src_p)
|
|
return (vec2){ 0, 0 };
|
|
|
|
vec2 src_pos = { src_p->x, src_p->y };
|
|
vec2 src_facing = fdir(src_p->facing);
|
|
|
|
vec2 current_best_dir = { 0, 0 };
|
|
fixed_t best_dist2 = -1;
|
|
|
|
for(int i = 0; i < game->entity_count; i++) {
|
|
entity_t *e = game->entities[i];
|
|
physical_t *p = getcomp(e, physical);
|
|
fighter_t *f = getcomp(e, fighter);
|
|
if(!p || !f || !f->enemy)
|
|
continue;
|
|
|
|
vec2 relpos = { p->x - src_pos.x, p->y - src_pos.y };
|
|
if(vec_dot(relpos, vec_rotate_60(src_facing)) < 0)
|
|
continue;
|
|
if(vec_dot(relpos, vec_rotate_m60(src_facing)) < 0)
|
|
continue;
|
|
|
|
fixed_t dist2 = fmul(relpos.x, relpos.x) + fmul(relpos.y, relpos.y);
|
|
if(dist2 < best_dist2 || best_dist2 < 0) {
|
|
/* TODO: Prune based on raycasts */
|
|
(void)map;
|
|
|
|
best_dist2 = dist2;
|
|
current_best_dir = fnormalize(relpos);
|
|
}
|
|
}
|
|
|
|
return current_best_dir;
|
|
}
|