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

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

1 files changed, 259 insertions, 0 deletions

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
 };