Initial commit.
This commit is contained in:
346
src/MultilevelAStarEx.cpp
Normal file
346
src/MultilevelAStarEx.cpp
Normal file
@@ -0,0 +1,346 @@
|
||||
#include "MultilevelAStarEx.h"
|
||||
|
||||
#include <godot_cpp/core/class_db.hpp>
|
||||
#include <godot_cpp/core/error_macros.hpp>
|
||||
#include <godot_cpp/variant/utility_functions.hpp>
|
||||
|
||||
#include <cstdlib>
|
||||
#include <algorithm>
|
||||
#include <cassert>
|
||||
|
||||
#define TERRAIN(x, y) (_terrain[(x) + (y) * _width])
|
||||
#define UNITS(x, y) (_units[(x) + (y) * _width])
|
||||
#define NODES(x, y) (_nodes[(x) + (y) * _width])
|
||||
|
||||
using namespace godot;
|
||||
|
||||
Node::Node() { }
|
||||
|
||||
Node::Node(int x, int y)
|
||||
{
|
||||
this->x = x;
|
||||
this->y = y;
|
||||
this->state = UNUSED;
|
||||
}
|
||||
|
||||
void Node::init(Node* parent, int distanceFromStart, const Vector2i &end)
|
||||
{
|
||||
this->state = OPEN;
|
||||
this->parent = parent;
|
||||
this->distanceFromStart = distanceFromStart;
|
||||
|
||||
int dx = abs(x - end.x);
|
||||
int dy = abs(y - end.y);
|
||||
this->distanceToEnd = (dx + dy) * 10;
|
||||
|
||||
if ((dx == 0 || dx == 1) && (dy == 0 || dy == 1))
|
||||
{
|
||||
this->distanceToEndForClosest = 10;
|
||||
}
|
||||
else
|
||||
{
|
||||
this->distanceToEndForClosest = this->distanceToEnd;
|
||||
}
|
||||
}
|
||||
|
||||
int Node::total_cost() const
|
||||
{
|
||||
return distanceFromStart + distanceToEnd;
|
||||
}
|
||||
|
||||
void MultilevelAStarEx::_bind_methods()
|
||||
{
|
||||
ClassDB::bind_method(D_METHOD("init", "region"), &MultilevelAStarEx::init);
|
||||
ClassDB::bind_method(D_METHOD("get_region"), &MultilevelAStarEx::get_region);
|
||||
ClassDB::bind_method(D_METHOD("set_terrain", "cell", "type"), &MultilevelAStarEx::set_terrain);
|
||||
ClassDB::bind_method(D_METHOD("get_terrain", "cell"), &MultilevelAStarEx::get_terrain);
|
||||
ClassDB::bind_method(D_METHOD("set_unit", "cell", "blocked"), &MultilevelAStarEx::set_unit);
|
||||
ClassDB::bind_method(D_METHOD("get_unit", "cell"), &MultilevelAStarEx::get_unit);
|
||||
ClassDB::bind_method(D_METHOD("find_path", "from", "to", "return_closest"), &MultilevelAStarEx::find_path);
|
||||
|
||||
BIND_ENUM_CONSTANT(STAIRS);
|
||||
BIND_ENUM_CONSTANT(BLOCKED);
|
||||
BIND_ENUM_CONSTANT(GROUND);
|
||||
}
|
||||
|
||||
MultilevelAStarEx::MultilevelAStarEx()
|
||||
{
|
||||
//UtilityFunctions::print("Constructor.");
|
||||
|
||||
_init = false;
|
||||
}
|
||||
|
||||
MultilevelAStarEx::~MultilevelAStarEx()
|
||||
{
|
||||
//UtilityFunctions::print("Destructor.");
|
||||
}
|
||||
|
||||
void MultilevelAStarEx::init(const Rect2i ®ion)
|
||||
{
|
||||
DEV_ASSERT(!_init);
|
||||
|
||||
DEV_ASSERT(region.get_area() > 0);
|
||||
|
||||
_region = region;
|
||||
_width = region.get_size().width;
|
||||
_height = region.get_size().height;
|
||||
_trans = region.get_position();
|
||||
|
||||
_terrain.resize(_width * _height, BLOCKED);
|
||||
_units.resize(_width * _height, false);
|
||||
_nodes.resize(_width * _height);
|
||||
|
||||
std::vector<Node>::iterator iter = _nodes.begin();
|
||||
for (int y = 0; y < _height; y++)
|
||||
{
|
||||
for (int x = 0; x < _width; x++)
|
||||
{
|
||||
*iter++ = Node(x, y);
|
||||
}
|
||||
}
|
||||
|
||||
_init = true;
|
||||
}
|
||||
|
||||
Rect2i MultilevelAStarEx::get_region() const
|
||||
{
|
||||
DEV_ASSERT(_init);
|
||||
|
||||
return _region;
|
||||
}
|
||||
|
||||
void MultilevelAStarEx::set_terrain(const Vector2i &cell, TerrainType type)
|
||||
{
|
||||
DEV_ASSERT(_init);
|
||||
DEV_ASSERT(_region.has_point(cell));
|
||||
|
||||
Vector2i cell2 = cell - _trans;
|
||||
TERRAIN(cell2.x, cell2.y) = type;
|
||||
}
|
||||
|
||||
MultilevelAStarEx::TerrainType MultilevelAStarEx::get_terrain(const Vector2i &cell) const
|
||||
{
|
||||
DEV_ASSERT(_init);
|
||||
DEV_ASSERT(_region.has_point(cell));
|
||||
|
||||
Vector2i cell2 = cell - _trans;
|
||||
return TERRAIN(cell2.x, cell2.y);
|
||||
}
|
||||
|
||||
void MultilevelAStarEx::set_unit(const Vector2i &cell, bool blocked)
|
||||
{
|
||||
DEV_ASSERT(_init);
|
||||
DEV_ASSERT(_region.has_point(cell));
|
||||
|
||||
Vector2i cell2 = cell - _trans;
|
||||
UNITS(cell2.x, cell2.y) = blocked;
|
||||
}
|
||||
|
||||
bool MultilevelAStarEx::get_unit(const Vector2i &cell) const
|
||||
{
|
||||
DEV_ASSERT(_init);
|
||||
DEV_ASSERT(_region.has_point(cell));
|
||||
|
||||
Vector2i cell2 = cell - _trans;
|
||||
return UNITS(cell2.x, cell2.y);
|
||||
}
|
||||
|
||||
bool MultilevelAStarEx::can_move(const Node* current, int x, int y) const
|
||||
{
|
||||
if (UNITS(x, y)) return false;
|
||||
|
||||
TerrainType tc = TERRAIN(current->x, current->y);
|
||||
TerrainType td = TERRAIN(x, y);
|
||||
|
||||
if (td == BLOCKED)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
else if (td == tc)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else if (td < 0 && tc > 0) // td is stairs
|
||||
{
|
||||
if (-td == tc || -td == tc - 1) return true;
|
||||
}
|
||||
else if (td > 0 && tc < 0) // tc is stairs
|
||||
{
|
||||
if (-tc == td || -tc == td - 1) return true;
|
||||
}
|
||||
else if (td < 0 && tc < 0) // both are stairs
|
||||
{
|
||||
return abs(td - tc) < 2;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
TypedArray<Vector2i> MultilevelAStarEx::generate_path(const Node* current) const
|
||||
{
|
||||
TypedArray<Vector2i> arr;
|
||||
|
||||
while (current->parent != nullptr)
|
||||
{
|
||||
arr.insert(0, Vector2i(current->x, current->y) + _trans);
|
||||
current = current->parent;
|
||||
}
|
||||
|
||||
return arr;
|
||||
}
|
||||
|
||||
Variant MultilevelAStarEx::find_path(const Vector2i &from, const Vector2i &to, const bool return_closest)
|
||||
{
|
||||
DEV_ASSERT(_init);
|
||||
|
||||
DEV_ASSERT(_region.has_point(from));
|
||||
DEV_ASSERT(_region.has_point(to));
|
||||
|
||||
Vector2i from2 = from - _trans;
|
||||
Vector2i to2 = to - _trans;
|
||||
|
||||
Variant result;
|
||||
|
||||
std::vector<Node*> open;
|
||||
std::vector<Node*> closed;
|
||||
|
||||
open.reserve(_width * _height);
|
||||
closed.reserve(_width * _height);
|
||||
|
||||
Node* closest = &NODES(from2.x, from2.y);
|
||||
closest->init(nullptr, 0, to2);
|
||||
open.push_back(closest);
|
||||
|
||||
auto process = [this, &open, &closed, &to2, &closest](Node* current, int x, int y, int distance) {
|
||||
Node* node = &NODES(x, y);
|
||||
if (node->state == Node::UNUSED)
|
||||
{
|
||||
node->init(current, current->distanceFromStart + distance, to2);
|
||||
open.push_back(node);
|
||||
}
|
||||
else if (node->state == Node::OPEN)
|
||||
{
|
||||
if (current->distanceFromStart < node->parent->distanceFromStart)
|
||||
{
|
||||
node->parent = current;
|
||||
node->distanceFromStart = current->distanceFromStart + distance;
|
||||
}
|
||||
}
|
||||
|
||||
// find the closest cell
|
||||
if (node->distanceToEndForClosest < closest->distanceToEndForClosest)
|
||||
{
|
||||
closest = node;
|
||||
}
|
||||
else if (node->distanceToEndForClosest == closest->distanceToEndForClosest)
|
||||
{
|
||||
if (node->distanceFromStart < closest->distanceFromStart)
|
||||
{
|
||||
closest = node;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
while (open.size() > 0)
|
||||
{
|
||||
// find closest to destination
|
||||
Node* current = open[0];
|
||||
for (Node* n : open)
|
||||
{
|
||||
if (n->total_cost() < current->total_cost())
|
||||
{
|
||||
current = n;
|
||||
}
|
||||
}
|
||||
|
||||
if (current->distanceToEnd == 0)
|
||||
{
|
||||
// found the path
|
||||
result = Variant(generate_path(current));
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// close it
|
||||
current->state = Node::CLOSED;
|
||||
open.erase(std::remove(open.begin(), open.end(), current), open.end());
|
||||
closed.push_back(current);
|
||||
|
||||
// expand it
|
||||
if (current->x - 1 >= 0) // left
|
||||
{
|
||||
if (can_move(current, current->x - 1, current->y))
|
||||
{
|
||||
process(current, current->x - 1, current->y, 10);
|
||||
}
|
||||
}
|
||||
if (current->x + 1 < _width) // right
|
||||
{
|
||||
if (can_move(current, current->x + 1, current->y))
|
||||
{
|
||||
process(current, current->x + 1, current->y, 10);
|
||||
}
|
||||
}
|
||||
if (current->y - 1 >= 0) // up
|
||||
{
|
||||
if (can_move(current, current->x, current->y - 1))
|
||||
{
|
||||
process(current, current->x, current->y - 1, 10);
|
||||
}
|
||||
}
|
||||
if (current->y + 1 < _height) // down
|
||||
{
|
||||
if (can_move(current, current->x, current->y + 1))
|
||||
{
|
||||
process(current, current->x, current->y + 1, 10);
|
||||
}
|
||||
}
|
||||
if ((current->x - 1 >= 0) && (current->y - 1 >= 0)) // top left
|
||||
{
|
||||
if (can_move(current, current->x - 1, current->y - 1))
|
||||
{
|
||||
process(current, current->x - 1, current->y - 1, 14);
|
||||
}
|
||||
}
|
||||
if ((current->x + 1 < _width) && (current->y - 1 >= 0)) // top right
|
||||
{
|
||||
if (can_move(current, current->x + 1, current->y - 1))
|
||||
{
|
||||
process(current, current->x + 1, current->y - 1, 14);
|
||||
}
|
||||
}
|
||||
if ((current->x - 1 >= 0) && (current->y + 1 < _height)) // bottom left
|
||||
{
|
||||
if (can_move(current, current->x - 1, current->y + 1))
|
||||
{
|
||||
process(current, current->x - 1, current->y + 1, 14);
|
||||
}
|
||||
}
|
||||
if ((current->x + 1 < _width) && (current->y + 1 < _height)) // bottom right
|
||||
{
|
||||
if (can_move(current, current->x + 1, current->y + 1))
|
||||
{
|
||||
process(current, current->x + 1, current->y + 1, 14);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// this is skipped if a path was found
|
||||
if (return_closest)
|
||||
{
|
||||
// return path to closest
|
||||
result = Variant(generate_path(closest));
|
||||
}
|
||||
|
||||
cleanup:
|
||||
// release the nodes
|
||||
for (Node* node : open)
|
||||
{
|
||||
node->state = Node::UNUSED;
|
||||
}
|
||||
for (Node* node : closed)
|
||||
{
|
||||
node->state = Node::UNUSED;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
Reference in New Issue
Block a user