Using std::set to sort nodes.

demo/main.gd

@@ -61,7 +61,7 @@ func find_path() -> void:
 	
 	var arr = astar.find_path(from, to, check_box.button_pressed) # vrne Variant: null ali Array
 	#print(arr)
-	print(arr.size())
+	#print(arr.size())
 	
 	if arr.size() > 1: # if the start and the end are the same there is no line to draw
 		# add new path

src/MultilevelAStarEx.cpp

@@ -4,9 +4,7 @@
 #include <godot_cpp/core/error_macros.hpp>
 #include <godot_cpp/variant/utility_functions.hpp>
 
-#include <cstdlib>
-#include <algorithm>
-#include <cassert>
+#include <set>
 
 #define TERRAIN(x, y) (_terrain[(x) + (y) * _width])
 #define UNITS(x, y) (_units[(x) + (y) * _width])
@@ -91,6 +89,33 @@ int Node::total_cost() const
 	return distanceFromStart + distanceToEnd;
 }
 
+bool Node::compare_nodes(const Node *left, const Node *right) {
+	if (left->total_cost() < right->total_cost())
+	{
+		return true;
+	}
+	else if (left->total_cost() > right->total_cost())
+	{
+		return false;
+	}
+	else // if (left->total_cost() == right->total_cost())
+	{
+		if (left->distanceToEnd < right->distanceToEnd)
+		{
+			return true;
+		}
+		else if (left->distanceToEnd > right->distanceToEnd)
+		{
+			return false;
+		}
+		else // if (left->distanceToEnd == right->distanceToEnd)
+		{
+			// make sure no two nodes are the same
+			return left < right;
+		}
+	}
+}
+
 void MultilevelAStarEx::_bind_methods()
 {
 	ClassDB::bind_method(D_METHOD("init", "region"), &MultilevelAStarEx::init);
@@ -114,10 +139,7 @@ MultilevelAStarEx::MultilevelAStarEx()
 	_pass = 0;
 }
 
-MultilevelAStarEx::~MultilevelAStarEx()
-{
-	//UtilityFunctions::print("Destructor.");
-}
+MultilevelAStarEx::~MultilevelAStarEx() { }
 
 void MultilevelAStarEx::init(const Rect2i &region)
 {
@@ -251,30 +273,34 @@ TypedArray<Vector2i> MultilevelAStarEx::find_path(const Vector2i &from, const Ve
 	Vector2i from2 = from - _trans;
 	Vector2i to2 = to - _trans;
 
-	std::vector<Node *> open;
-
-	open.reserve(_width * _height);
+	std::set<Node *, decltype(Node::compare_nodes)*> open(&Node::compare_nodes);
 
 	Node *closest = &NODES(from2.x, from2.y);
 	closest->open(_pass, nullptr, 0, to2);
-	open.push_back(closest);
+	open.insert(closest);
 
 	auto process = [this, &open, &to2, &closest](Node *current, int x, int y, int distance) {
 		Node *node = &NODES(x, y);
 		
-		if (node->state(_pass) == Node::UNUSED)
-		{
-			node->open(_pass, current, current->distanceFromStart + distance, to2);
-			open.push_back(node);
-		}
-		else if (node->state(_pass) == Node::CLOSED)
+		if (node->state(_pass) == Node::CLOSED)
 		{
 			return;
 		}
+		else if (node->state(_pass) == Node::UNUSED)
+		{
+			node->open(_pass, current, current->distanceFromStart + distance, to2);
+			open.insert(node);
+		}
 		else if (current->distanceFromStart + distance < node->distanceFromStart)
 		{
+			auto nodeIter = open.find(node);
+			DEV_ASSERT(nodeIter != open.end());
+			open.erase(nodeIter);
+
 			node->parent = current;
 			node->distanceFromStart = current->distanceFromStart + distance;
+
+			open.insert(node);
 		}
 
 		// find the closest cell
@@ -293,19 +319,7 @@ TypedArray<Vector2i> MultilevelAStarEx::find_path(const Vector2i &from, const Ve
 
 	while (!open.empty())
 	{
-		// find closest to destination
-		Node *current = open[0];
-		for (Node *n : open)
-		{
-			if (n->total_cost() < current->total_cost())
-			{
-				current = n;
-			}
-			else if ((n->total_cost() == current->total_cost()) && (n->distanceToEnd < current->distanceToEnd))
-			{
-				current = n;
-			}
-		}
+		Node *current = *open.begin();
 
 		if (current->distanceToEnd == 0)
 		{
@@ -314,8 +328,8 @@ TypedArray<Vector2i> MultilevelAStarEx::find_path(const Vector2i &from, const Ve
 		}
 
 		// close it
+		open.erase(open.begin());
 		current->close(_pass);
-		open.erase(std::remove(open.begin(), open.end(), current), open.end());
 
 		// expand it
 		if (current->x - 1 >= 0) // left

src/MultilevelAStarEx.h

@@ -29,6 +29,8 @@ private:
 
 	int distanceToEndForClosest;
 
+	static bool compare_nodes(const Node *left, const Node *right);
+
 	Node(int x, int y);
 	void open(int pass, Node *parent, int distanceFromStart, const Vector2i &end);
 	void close(int pass);