Initial commit.

src/MultilevelAStarEx.cpp

@@ -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 &region)
+{
+	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;
+}

src/MultilevelAStarEx.h

@@ -0,0 +1,85 @@
+#ifndef MultilevelAStarEx_H
+#define MultilevelAStarEx_H
+
+#include <gdextension_interface.h>
+#include <godot_cpp/classes/ref.hpp>
+#include <godot_cpp/variant/array.hpp>
+#include <godot_cpp/variant/vector2i.hpp>
+
+#include <vector>
+
+namespace godot {
+
+class Node {
+	friend class MultilevelAStarEx;
+
+public:
+	enum NodeState
+	{
+		UNUSED, OPEN, CLOSED
+	};
+
+private:
+	NodeState state;
+	int x, y;
+
+	Node* parent;
+	int distanceFromStart;
+	int distanceToEnd;
+
+	int distanceToEndForClosest;
+
+	Node(int x, int y);
+	void init(Node* parent, int distanceFromStart, const Vector2i &end);
+	int total_cost() const;
+
+public:
+	Node();
+};
+
+class MultilevelAStarEx : public RefCounted
+{
+	GDCLASS(MultilevelAStarEx, RefCounted)
+
+public:
+	enum TerrainType
+	{
+		STAIRS = -1,
+		BLOCKED = 0,
+		GROUND = 1,
+	};
+
+private:
+	bool _init;
+	std::vector<TerrainType> _terrain;
+	std::vector<bool> _units;
+	std::vector<Node> _nodes;
+
+	Rect2i _region;
+	Vector2i _trans;
+	int _width, _height;
+
+	bool can_move(const Node* current, int x, int y) const;
+	TypedArray<Vector2i> generate_path(const Node* current) const;
+
+protected:
+	static void _bind_methods();
+
+public:
+	MultilevelAStarEx();
+	~MultilevelAStarEx();
+
+	void init(const Rect2i &region);
+	Rect2i get_region() const;
+	void set_terrain(const Vector2i &cell, TerrainType type);
+	TerrainType get_terrain(const Vector2i &cell) const;
+	void set_unit(const Vector2i &cell, bool blocked);
+	bool get_unit(const Vector2i &cell) const;
+	Variant find_path(const Vector2i &from, const Vector2i &to, const bool return_closest);
+};
+
+}
+
+VARIANT_ENUM_CAST(MultilevelAStarEx::TerrainType);
+
+#endif

src/main.cpp

@@ -0,0 +1,36 @@
+#include "MultilevelAStarEx.h"
+
+#include <gdextension_interface.h>
+#include <godot_cpp/core/defs.hpp>
+#include <godot_cpp/godot.hpp>
+
+using namespace godot;
+
+void initialize_module(ModuleInitializationLevel p_level)
+{
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) {
+		return;
+	}
+
+	ClassDB::register_class<MultilevelAStarEx>();
+}
+
+void uninitialize_module(ModuleInitializationLevel p_level)
+{
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SCENE) {
+		return;
+	}
+}
+
+extern "C" {
+GDExtensionBool GDE_EXPORT my_astar_ext_init(GDExtensionInterfaceGetProcAddress p_get_proc_address, const GDExtensionClassLibraryPtr p_library, GDExtensionInitialization *r_initialization)
+{
+	godot::GDExtensionBinding::InitObject init_obj(p_get_proc_address, p_library, r_initialization);
+
+	init_obj.register_initializer(initialize_module);
+	init_obj.register_terminator(uninitialize_module);
+	init_obj.set_minimum_library_initialization_level(MODULE_INITIALIZATION_LEVEL_SCENE);
+
+	return init_obj.init();
+}
+}