197 lines
5.1 KiB
C++
197 lines
5.1 KiB
C++
#include <math.h>
|
|
#include "Pathfinder.h"
|
|
#include "Tilemap.h"
|
|
#include <queue>
|
|
|
|
namespace Pathfinder
|
|
{
|
|
float distance(vec2i a, vec2i b)
|
|
{
|
|
float dx = a.x - b.x;
|
|
float dy = a.y - b.y;
|
|
return sqrtf(dx*dx + dy*dy);
|
|
}
|
|
|
|
std::vector<vec2i> a_star(Tilemap * map, vec2i start, vec2i goal)
|
|
{
|
|
std::vector<AStarNode*> open;
|
|
std::vector<AStarNode*> closed;
|
|
|
|
std::vector<vec2i> path;
|
|
|
|
AStarNode* st = new AStarNode();
|
|
st->pos = start;
|
|
open.emplace_back(st);
|
|
|
|
while (open.size() != 0) {
|
|
AStarNode* current = nullptr;
|
|
int currentindex = -1;
|
|
for (int i = 0; i < open.size(); i++) {
|
|
if (current == nullptr || current->f > open[i]->f) {
|
|
current = open[i];
|
|
currentindex = i;
|
|
}
|
|
}
|
|
open.erase(open.begin()+currentindex);
|
|
closed.emplace_back(current);
|
|
|
|
auto neighbours = map->get_neighbours(current->pos.x, current->pos.y);
|
|
for (auto pos : neighbours) {
|
|
if (!map->get_tile(pos.x, pos.y).passable) {
|
|
continue;
|
|
}
|
|
AStarNode* neighbour = new AStarNode();
|
|
neighbour->pos = current->pos;
|
|
neighbour->pos.x = pos.x;
|
|
neighbour->pos.y = pos.y;
|
|
|
|
neighbour->from = current;
|
|
neighbour->g = current->g + 1;
|
|
neighbour->h = distance(neighbour->pos, goal);
|
|
neighbour->f = neighbour->g + neighbour->h;
|
|
|
|
// Is this the goal?
|
|
if (neighbour->pos == goal)
|
|
{
|
|
delete neighbour;
|
|
path.emplace_back(goal);
|
|
while (current->from != nullptr)
|
|
{
|
|
path.emplace_back(current->pos);
|
|
current = current->from;
|
|
}
|
|
for (AStarNode* var : open)
|
|
{
|
|
delete var;
|
|
}
|
|
for (AStarNode* var : closed)
|
|
{
|
|
delete var;
|
|
}
|
|
return path;
|
|
}
|
|
|
|
// Check if the new node is in the closed list
|
|
bool isClosed = false;
|
|
for (auto it = closed.begin(); it != closed.end(); it++)
|
|
{
|
|
AStarNode* n = *it;
|
|
if (n->pos == neighbour->pos)
|
|
{
|
|
isClosed = true;
|
|
if (n->h > neighbour->h)
|
|
{
|
|
closed.erase(it);
|
|
delete (*it);
|
|
open.emplace_back(neighbour);
|
|
isClosed = false;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!isClosed)
|
|
{
|
|
// Check if the new node is in the open list
|
|
bool isOpened = false;
|
|
for (auto it = open.begin(); it != open.end(); it++)
|
|
{
|
|
AStarNode* n = *it;
|
|
if (n->pos == neighbour->pos)
|
|
{
|
|
isOpened = true;
|
|
if (n->h > neighbour->h)
|
|
{
|
|
open.erase(it);
|
|
delete (*it);
|
|
open.emplace_back(neighbour);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!isOpened)
|
|
{
|
|
open.emplace_back(neighbour);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
delete neighbour;
|
|
}
|
|
}
|
|
for (auto it = open.begin(); it != open.end(); it++)
|
|
{
|
|
if (current == (*it))
|
|
{
|
|
open.erase(it);
|
|
delete (*it);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
for (AStarNode* var : open)
|
|
{
|
|
delete var;
|
|
}
|
|
for (AStarNode* var : closed)
|
|
{
|
|
delete var;
|
|
}
|
|
return path;
|
|
}
|
|
|
|
void calc_dijkstra_map(Tilemap& map, std::vector<vec2i>& goals, DijkstraMap& out, float maxValue) {
|
|
if (out.tilemap != nullptr) {
|
|
delete out.tilemap;
|
|
}
|
|
out.tilemap = new float[map.get_width() * map.get_height()];
|
|
for (int i = 0; i < map.get_width() * map.get_height(); i++) {
|
|
out.tilemap[i] = maxValue;
|
|
}
|
|
out.height = map.get_height();
|
|
out.width = map.get_width();
|
|
for (vec2i& pos : goals) {
|
|
out.setValue(pos.x, pos.y, 0);
|
|
}
|
|
|
|
std::queue<vec2i> queue;
|
|
|
|
for (vec2i& pos : goals) {
|
|
auto neigh = map.get_neighbours(pos.x, pos.y);
|
|
for (vec2i& npos : neigh) {
|
|
int val = out.get_value(npos.x, npos.y);
|
|
if (map.get_tile(npos.x, npos.y).passable && val > 1.4f) {
|
|
if (npos.x != 0 && npos.y != 0) {
|
|
out.setValue(npos.x, npos.y, 1.4f);
|
|
}
|
|
else {
|
|
out.setValue(npos.x, npos.y, 1);
|
|
}
|
|
queue.push(npos);
|
|
}
|
|
}
|
|
}
|
|
|
|
while (queue.size() > 0) {
|
|
vec2i current = queue.front();
|
|
float val = out.get_value(current.x, current.y);
|
|
queue.pop();
|
|
|
|
std::vector<vec2i> neigh = map.get_neighbours(current.x, current.y);
|
|
for (vec2i& npos : neigh) {
|
|
vec2i dp = npos - current;
|
|
float nval = val + 1;
|
|
if (dp.x != 0 && dp.y != 0) {
|
|
nval += .4f;
|
|
}
|
|
if (map.get_tile(npos.x, npos.y).passable && out.get_value(npos.x, npos.y) > nval) {
|
|
out.setValue(npos.x, npos.y, nval);
|
|
queue.push(npos);
|
|
}
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
}
|