Merge pull request #30556 from kawa-yoiko/astar-directed

Improve support for directed graphs in A*; docs update included

4 files changed, 358 insertions, 40 deletions

diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index ae2b56e7b7..810e290922 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -164,23 +164,23 @@ void AStar::connect_points(int p_id, int p_with_id, bool bidirectional) {
 	}
 
 	Segment s(p_id, p_with_id);
-	if (s.from == p_id) {
-		s.from_point = a;
-		s.to_point = b;
-	} else {
-		s.from_point = b;
-		s.to_point = a;
+	if (bidirectional) s.direction = Segment::BIDIRECTIONAL;
+
+	Set<Segment>::Element *element = segments.find(s);
+	if (element != NULL) {
+		s.direction |= element->get().direction;
+		if (s.direction == Segment::BIDIRECTIONAL) {
+			// Both are neighbours of each other now
+			a->unlinked_neighbours.remove(b->id);
+			b->unlinked_neighbours.remove(a->id);
+		}
+		segments.erase(element);
 	}
 
 	segments.insert(s);
 }
 
-void AStar::disconnect_points(int p_id, int p_with_id) {
-
-	Segment s(p_id, p_with_id);
-	ERR_FAIL_COND(!segments.has(s));
-
-	segments.erase(s);
+void AStar::disconnect_points(int p_id, int p_with_id, bool bidirectional) {
 
 	Point *a;
 	bool a_exists = points.lookup(p_id, a);
@@ -190,10 +190,33 @@ void AStar::disconnect_points(int p_id, int p_with_id) {
 	bool b_exists = points.lookup(p_with_id, b);
 	CRASH_COND(!b_exists);
 
-	a->neighbours.remove(b->id);
-	a->unlinked_neighbours.remove(b->id);
-	b->neighbours.remove(a->id);
-	b->unlinked_neighbours.remove(a->id);
+	Segment s(p_id, p_with_id);
+	int remove_direction = bidirectional ? (int)Segment::BIDIRECTIONAL : s.direction;
+
+	Set<Segment>::Element *element = segments.find(s);
+	if (element != NULL) {
+		// s is the new segment
+		// Erase the directions to be removed
+		s.direction = (element->get().direction & ~remove_direction);
+
+		a->neighbours.remove(b->id);
+		if (bidirectional) {
+			b->neighbours.remove(a->id);
+			if (element->get().direction != Segment::BIDIRECTIONAL) {
+				a->unlinked_neighbours.remove(b->id);
+				b->unlinked_neighbours.remove(a->id);
+			}
+		} else {
+			if (s.direction == Segment::NONE)
+				b->unlinked_neighbours.remove(a->id);
+			else
+				a->unlinked_neighbours.set(b->id, b);
+		}
+
+		segments.erase(element);
+		if (s.direction != Segment::NONE)
+			segments.insert(s);
+	}
 }
 
 bool AStar::has_point(int p_id) const {
@@ -227,10 +250,13 @@ PoolVector<int> AStar::get_point_connections(int p_id) {
 	return point_list;
 }
 
-bool AStar::are_points_connected(int p_id, int p_with_id) const {
+bool AStar::are_points_connected(int p_id, int p_with_id, bool bidirectional) const {
 
 	Segment s(p_id, p_with_id);
-	return segments.has(s);
+	const Set<Segment>::Element *element = segments.find(s);
+
+	return element != NULL &&
+		   (bidirectional || (element->get().direction & s.direction) == s.direction);
 }
 
 void AStar::clear() {
@@ -284,13 +310,17 @@ Vector3 AStar::get_closest_position_in_segment(const Vector3 &p_point) const {
 
 	for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) {
 
-		if (!(E->get().from_point->enabled && E->get().to_point->enabled)) {
+		Point *from_point = nullptr, *to_point = nullptr;
+		points.lookup(E->get().u, from_point);
+		points.lookup(E->get().v, to_point);
+
+		if (!(from_point->enabled && to_point->enabled)) {
 			continue;
 		}
 
 		Vector3 segment[2] = {
-			E->get().from_point->pos,
-			E->get().to_point->pos,
+			from_point->pos,
+			to_point->pos,
 		};
 
 		Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment);
@@ -532,8 +562,8 @@ void AStar::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("is_point_disabled", "id"), &AStar::is_point_disabled);
 
 	ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id", "bidirectional"), &AStar::connect_points, DEFVAL(true));
-	ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar::disconnect_points);
-	ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar::are_points_connected);
+	ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id", "bidirectional"), &AStar::disconnect_points, DEFVAL(true));
+	ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id", "bidirectional"), &AStar::are_points_connected, DEFVAL(true));
 
 	ClassDB::bind_method(D_METHOD("get_point_count"), &AStar::get_point_count);
 	ClassDB::bind_method(D_METHOD("get_point_capacity"), &AStar::get_point_capacity);
diff --git a/core/math/a_star.h b/core/math/a_star.h
index 0a5d3e992c..8ff62e646b 100644
--- a/core/math/a_star.h
+++ b/core/math/a_star.h
@@ -81,24 +81,35 @@ class AStar : public Reference {
 	struct Segment {
 		union {
 			struct {
-				int32_t from;
-				int32_t to;
+				int32_t u;
+				int32_t v;
 			};
 			uint64_t key;
 		};
 
-		Point *from_point;
-		Point *to_point;
+		enum {
+			NONE = 0,
+			FORWARD = 1,
+			BACKWARD = 2,
+			BIDIRECTIONAL = FORWARD | BACKWARD
+		};
+		unsigned char direction;
 
 		bool operator<(const Segment &p_s) const { return key < p_s.key; }
-		Segment() { key = 0; }
+		Segment() {
+			key = 0;
+			direction = NONE;
+		}
 		Segment(int p_from, int p_to) {
-			if (p_from > p_to) {
-				SWAP(p_from, p_to);
+			if (p_from < p_to) {
+				u = p_from;
+				v = p_to;
+				direction = FORWARD;
+			} else {
+				u = p_to;
+				v = p_from;
+				direction = BACKWARD;
 			}
-
-			from = p_from;
-			to = p_to;
 		}
 	};
 
@@ -133,8 +144,8 @@ public:
 	bool is_point_disabled(int p_id) const;
 
 	void connect_points(int p_id, int p_with_id, bool bidirectional = true);
-	void disconnect_points(int p_id, int p_with_id);
-	bool are_points_connected(int p_id, int p_with_id) const;
+	void disconnect_points(int p_id, int p_with_id, bool bidirectional = true);
+	bool are_points_connected(int p_id, int p_with_id, bool bidirectional = true) const;
 
 	int get_point_count() const;
 	int get_point_capacity() const;
diff --git a/doc/classes/AStar.xml b/doc/classes/AStar.xml
index e835af01e8..6304bd34f6 100644
--- a/doc/classes/AStar.xml
+++ b/doc/classes/AStar.xml
@@ -1,11 +1,23 @@
 <?xml version="1.0" encoding="UTF-8" ?>
 <class name="AStar" inherits="Reference" category="Core" version="3.2">
 	<brief_description>
-		AStar class representation that uses 3d-vectors as edges.
+		An implementation of A* to find shortest paths among connected points in space.
 	</brief_description>
 	<description>
-		A* (A star) is a computer algorithm that is widely used in pathfinding and graph traversal, the process of plotting an efficiently directed path between multiple points. It enjoys widespread use due to its performance and accuracy. Godot's A* implementation make use of vectors as points.
-		You must add points manually with [method add_point] and create segments manually with [method connect_points]. So you can test if there is a path between two points with the [method are_points_connected] function, get the list of existing ids in the found path with [method get_id_path], or the points list with [method get_point_path].
+		A* (A star) is a computer algorithm that is widely used in pathfinding and graph traversal, the process of plotting short paths among vertices (points), passing through a given set of edges (segments). It enjoys widespread use due to its performance and accuracy. Godot's A* implementation uses points in three-dimensional space and Euclidean distances by default.
+		You must add points manually with [method add_point] and create segments manually with [method connect_points]. Then you can test if there is a path between two points with the [method are_points_connected] function, get a path containing indices by [method get_id_path], or one containing actual coordinates with [method get_point_path].
+		It is also possible to use non-Euclidean distances. To do so, create a class that extends [code]AStar[/code] and override methods [method _compute_cost] and [method _estimate_cost]. Both take two indices and return a length, as is shown in the following example.
+		[codeblock]
+		class MyAStar:
+		    extends AStar
+
+		    func _compute_cost(u, v):
+		        return abs(u - v)
+
+		    func _estimate_cost(u, v):
+		        return min(0, abs(u - v) - 1)
+		[/codeblock]
+		[method _estimate_cost] should return a lower bound of the distance, i.e. [code]_estimate_cost(u, v) &lt;= _compute_cost(u, v)[/code]. This serves as a hint to the algorithm because the custom [code]_compute_cost[/code] might be computation-heavy. If this is not the case, make [method _estimate_cost] return the same value as [method _compute_cost] to provide the algorithm with the most accurate information.
 	</description>
 	<tutorials>
 	</tutorials>
@@ -19,6 +31,7 @@
 			</argument>
 			<description>
 				Called when computing the cost between two connected points.
+				Note that this function is hidden in the default [code]AStar[/code] class.
 			</description>
 		</method>
 		<method name="_estimate_cost" qualifiers="virtual">
@@ -30,6 +43,7 @@
 			</argument>
 			<description>
 				Called when estimating the cost between a point and the path's ending point.
+				Note that this function is hidden in the default [code]AStar[/code] class.
 			</description>
 		</method>
 		<method name="add_point">
@@ -57,8 +71,10 @@
 			</argument>
 			<argument index="1" name="to_id" type="int">
 			</argument>
+			<argument index="2" name="bidirectional" type="bool" default="true">
+			</argument>
 			<description>
-				Returns whether there is a connection/segment between the given points.
+				Returns whether the two given points are directly connected by a segment. If [code]bidirectional[/code] is [code]false[/code], returns whether movement from [code]id[/code] to [code]to_id[/code] is possible through this segment.
 			</description>
 		</method>
 		<method name="clear">
@@ -94,8 +110,10 @@
 			</argument>
 			<argument index="1" name="to_id" type="int">
 			</argument>
+			<argument index="2" name="bidirectional" type="bool" default="true">
+			</argument>
 			<description>
-				Deletes the segment between the given points.
+				Deletes the segment between the given points. If [code]bidirectional[/code] is [code]false[/code], only movement from [code]id[/code] to [code]to_id[/code] is prevented, and a unidirectional segment possibly remains.
 			</description>
 		</method>
 		<method name="get_available_point_id" qualifiers="const">
diff --git a/main/tests/test_astar.cpp b/main/tests/test_astar.cpp
index d34ff0d95e..4b60a3e94a 100644
--- a/main/tests/test_astar.cpp
+++ b/main/tests/test_astar.cpp
@@ -31,8 +31,10 @@
 #include "test_astar.h"
 
 #include "core/math/a_star.h"
+#include "core/math/math_funcs.h"
 #include "core/os/os.h"
 
+#include <math.h>
 #include <stdio.h>
 
 namespace TestAStar {
@@ -87,11 +89,268 @@ bool test_abcx() {
 	return ok;
 }
 
+bool test_add_remove() {
+	AStar a;
+	bool ok = true;
+
+	// Manual tests
+	a.add_point(1, Vector3(0, 0, 0));
+	a.add_point(2, Vector3(0, 1, 0));
+	a.add_point(3, Vector3(1, 1, 0));
+	a.add_point(4, Vector3(2, 0, 0));
+	a.connect_points(1, 2, true);
+	a.connect_points(1, 3, true);
+	a.connect_points(1, 4, false);
+
+	ok = ok && (a.are_points_connected(2, 1) == true);
+	ok = ok && (a.are_points_connected(4, 1) == true);
+	ok = ok && (a.are_points_connected(2, 1, false) == true);
+	ok = ok && (a.are_points_connected(4, 1, false) == false);
+
+	a.disconnect_points(1, 2, true);
+	ok = ok && (a.get_point_connections(1).size() == 2); // 3, 4
+	ok = ok && (a.get_point_connections(2).size() == 0);
+
+	a.disconnect_points(4, 1, false);
+	ok = ok && (a.get_point_connections(1).size() == 2); // 3, 4
+	ok = ok && (a.get_point_connections(4).size() == 0);
+
+	a.disconnect_points(4, 1, true);
+	ok = ok && (a.get_point_connections(1).size() == 1); // 3
+	ok = ok && (a.get_point_connections(4).size() == 0);
+
+	a.connect_points(2, 3, false);
+	ok = ok && (a.get_point_connections(2).size() == 1); // 3
+	ok = ok && (a.get_point_connections(3).size() == 1); // 1
+
+	a.connect_points(2, 3, true);
+	ok = ok && (a.get_point_connections(2).size() == 1); // 3
+	ok = ok && (a.get_point_connections(3).size() == 2); // 1, 2
+
+	a.disconnect_points(2, 3, false);
+	ok = ok && (a.get_point_connections(2).size() == 0);
+	ok = ok && (a.get_point_connections(3).size() == 2); // 1, 2
+
+	a.connect_points(4, 3, true);
+	ok = ok && (a.get_point_connections(3).size() == 3); // 1, 2, 4
+	ok = ok && (a.get_point_connections(4).size() == 1); // 3
+
+	a.disconnect_points(3, 4, false);
+	ok = ok && (a.get_point_connections(3).size() == 2); // 1, 2
+	ok = ok && (a.get_point_connections(4).size() == 1); // 3
+
+	a.remove_point(3);
+	ok = ok && (a.get_point_connections(1).size() == 0);
+	ok = ok && (a.get_point_connections(2).size() == 0);
+	ok = ok && (a.get_point_connections(4).size() == 0);
+
+	a.add_point(0, Vector3(0, -1, 0));
+	a.add_point(3, Vector3(2, 1, 0));
+	// 0: (0, -1)
+	// 1: (0, 0)
+	// 2: (0, 1)
+	// 3: (2, 1)
+	// 4: (2, 0)
+
+	// Tests for get_closest_position_in_segment
+	a.connect_points(2, 3);
+	ok = ok && (a.get_closest_position_in_segment(Vector3(0.5, 0.5, 0)) == Vector3(0.5, 1, 0));
+
+	a.connect_points(3, 4);
+	a.connect_points(0, 3);
+	a.connect_points(1, 4);
+	a.disconnect_points(1, 4, false);
+	a.disconnect_points(4, 3, false);
+	a.disconnect_points(3, 4, false);
+	// Remaining edges: <2, 3>, <0, 3>, <1, 4> (directed)
+	ok = ok && (a.get_closest_position_in_segment(Vector3(2, 0.5, 0)) == Vector3(1.75, 0.75, 0));
+	ok = ok && (a.get_closest_position_in_segment(Vector3(-1, 0.2, 0)) == Vector3(0, 0, 0));
+	ok = ok && (a.get_closest_position_in_segment(Vector3(3, 2, 0)) == Vector3(2, 1, 0));
+
+	Math::seed(0);
+
+	// Random tests for connectivity checks
+	for (int i = 0; i < 20000; i++) {
+		int u = Math::rand() % 5;
+		int v = Math::rand() % 4;
+		if (u == v) v = 4;
+		if (Math::rand() % 2 == 1) {
+			// Add a (possibly existing) directed edge and confirm connectivity
+			a.connect_points(u, v, false);
+			ok = ok && (a.are_points_connected(u, v, false) == true);
+		} else {
+			// Remove a (possibly nonexistent) directed edge and confirm disconnectivity
+			a.disconnect_points(u, v, false);
+			ok = ok && (a.are_points_connected(u, v, false) == false);
+		}
+	}
+
+	// Random tests for point removal
+	for (int i = 0; i < 20000; i++) {
+		a.clear();
+		for (int j = 0; j < 5; j++)
+			a.add_point(j, Vector3(0, 0, 0));
+
+		// Add or remove random edges
+		for (int j = 0; j < 10; j++) {
+			int u = Math::rand() % 5;
+			int v = Math::rand() % 4;
+			if (u == v) v = 4;
+			if (Math::rand() % 2 == 1)
+				a.connect_points(u, v, false);
+			else
+				a.disconnect_points(u, v, false);
+		}
+
+		// Remove point 0
+		a.remove_point(0);
+		// White box: this will check all edges remaining in the segments set
+		for (int j = 1; j < 5; j++) {
+			ok = ok && (a.are_points_connected(0, j, true) == false);
+		}
+	}
+
+	// It's been great work, cheers \(^ ^)/
+	return ok;
+}
+
+bool test_solutions() {
+	// Random stress tests with Floyd-Warshall
+
+	const int N = 30;
+	Math::seed(0);
+
+	for (int test = 0; test < 1000; test++) {
+		AStar a;
+		Vector3 p[N];
+		bool adj[N][N] = { { false } };
+
+		// Assign initial coordinates
+		for (int u = 0; u < N; u++) {
+			p[u].x = Math::rand() % 100;
+			p[u].y = Math::rand() % 100;
+			p[u].z = Math::rand() % 100;
+			a.add_point(u, p[u]);
+		}
+
+		// Generate a random sequence of operations
+		for (int i = 0; i < 1000; i++) {
+			// Pick two different vertices
+			int u, v;
+			u = Math::rand() % N;
+			v = Math::rand() % (N - 1);
+			if (u == v) v = N - 1;
+
+			// Pick a random operation
+			int op = Math::rand();
+			switch (op % 9) {
+				case 0:
+				case 1:
+				case 2:
+				case 3:
+				case 4:
+				case 5:
+					// Add edge (u, v); possibly bidirectional
+					a.connect_points(u, v, op % 2);
+					adj[u][v] = true;
+					if (op % 2) adj[v][u] = true;
+					break;
+				case 6:
+				case 7:
+					// Remove edge (u, v); possibly bidirectional
+					a.disconnect_points(u, v, op % 2);
+					adj[u][v] = false;
+					if (op % 2) adj[v][u] = false;
+					break;
+				case 8:
+					// Remove point u and add it back; clears adjacent edges and changes coordinates
+					a.remove_point(u);
+					p[u].x = Math::rand() % 100;
+					p[u].y = Math::rand() % 100;
+					p[u].z = Math::rand() % 100;
+					a.add_point(u, p[u]);
+					for (v = 0; v < N; v++)
+						adj[u][v] = adj[v][u] = false;
+					break;
+			}
+		}
+
+		// Floyd-Warshall
+		float d[N][N];
+		for (int u = 0; u < N; u++)
+			for (int v = 0; v < N; v++)
+				d[u][v] = (u == v || adj[u][v]) ? p[u].distance_to(p[v]) : INFINITY;
+
+		for (int w = 0; w < N; w++)
+			for (int u = 0; u < N; u++)
+				for (int v = 0; v < N; v++)
+					if (d[u][v] > d[u][w] + d[w][v])
+						d[u][v] = d[u][w] + d[w][v];
+
+		// Display statistics
+		int count = 0;
+		for (int u = 0; u < N; u++)
+			for (int v = 0; v < N; v++)
+				if (adj[u][v]) count++;
+		printf("Test #%4d: %3d edges, ", test + 1, count);
+		count = 0;
+		for (int u = 0; u < N; u++)
+			for (int v = 0; v < N; v++)
+				if (!Math::is_inf(d[u][v])) count++;
+		printf("%3d/%d pairs of reachable points\n", count - N, N * (N - 1));
+
+		// Check A*'s output
+		bool match = true;
+		for (int u = 0; u < N; u++)
+			for (int v = 0; v < N; v++)
+				if (u != v) {
+					PoolVector<int> route = a.get_id_path(u, v);
+					if (!Math::is_inf(d[u][v])) {
+						// Reachable
+						if (route.size() == 0) {
+							printf("From %d to %d: A* did not find a path\n", u, v);
+							match = false;
+							goto exit;
+						}
+						float astar_dist = 0;
+						for (int i = 1; i < route.size(); i++) {
+							if (!adj[route[i - 1]][route[i]]) {
+								printf("From %d to %d: edge (%d, %d) does not exist\n",
+										u, v, route[i - 1], route[i]);
+								match = false;
+								goto exit;
+							}
+							astar_dist += p[route[i - 1]].distance_to(p[route[i]]);
+						}
+						if (!Math::is_equal_approx(astar_dist, d[u][v])) {
+							printf("From %d to %d: Floyd-Warshall gives %.6f, A* gives %.6f\n",
+									u, v, d[u][v], astar_dist);
+							match = false;
+							goto exit;
+						}
+					} else {
+						// Unreachable
+						if (route.size() > 0) {
+							printf("From %d to %d: A* somehow found a nonexistent path\n", u, v);
+							match = false;
+							goto exit;
+						}
+					}
+				}
+
+	exit:
+		if (!match) return false;
+	}
+	return true;
+}
+
 typedef bool (*TestFunc)(void);
 
 TestFunc test_funcs[] = {
 	test_abc,
 	test_abcx,
+	test_add_remove,
+	test_solutions,
 	NULL
 };