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

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

2 files changed, 76 insertions, 35 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;