Allow Navigation to be more flexible

doc/classes/NavigationServer2D.xml

@@ -267,6 +267,37 @@
 				Creates a new region.
 			</description>
 		</method>
+		<method name="region_get_connection_pathway_end" qualifiers="const">
+			<return type="Vector2">
+			</return>
+			<argument index="0" name="region" type="RID">
+			</argument>
+			<argument index="1" name="connection" type="int">
+			</argument>
+			<description>
+				Returns the ending point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count].
+			</description>
+		</method>
+		<method name="region_get_connection_pathway_start" qualifiers="const">
+			<return type="Vector2">
+			</return>
+			<argument index="0" name="region" type="RID">
+			</argument>
+			<argument index="1" name="connection" type="int">
+			</argument>
+			<description>
+				Returns the starting point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count].
+			</description>
+		</method>
+		<method name="region_get_connections_count" qualifiers="const">
+			<return type="int">
+			</return>
+			<argument index="0" name="region" type="RID">
+			</argument>
+			<description>
+				Returns how many connections this [code]region[/code] has with other regions in the map.
+			</description>
+		</method>
 		<method name="region_get_layers" qualifiers="const">
 			<return type="int">
 			</return>
@@ -321,6 +352,15 @@
 			</description>
 		</method>
 	</methods>
+	<signals>
+		<signal name="map_changed">
+			<argument index="0" name="map" type="RID">
+			</argument>
+			<description>
+				Emitted when a navigation map is updated, when a region moves or is modified.
+			</description>
+		</signal>
+	</signals>
 	<constants>
 	</constants>
 </class>

doc/classes/NavigationServer3D.xml

@@ -335,6 +335,37 @@
 				Creates a new region.
 			</description>
 		</method>
+		<method name="region_get_connection_pathway_end" qualifiers="const">
+			<return type="Vector3">
+			</return>
+			<argument index="0" name="region" type="RID">
+			</argument>
+			<argument index="1" name="connection" type="int">
+			</argument>
+			<description>
+				Returns the ending point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count].
+			</description>
+		</method>
+		<method name="region_get_connection_pathway_start" qualifiers="const">
+			<return type="Vector3">
+			</return>
+			<argument index="0" name="region" type="RID">
+			</argument>
+			<argument index="1" name="connection" type="int">
+			</argument>
+			<description>
+				Returns the starting point of a connection door. [code]connection[/code] is an index between 0 and the return value of [method region_get_connections_count].
+			</description>
+		</method>
+		<method name="region_get_connections_count" qualifiers="const">
+			<return type="int">
+			</return>
+			<argument index="0" name="region" type="RID">
+			</argument>
+			<description>
+				Returns how many connections this [code]region[/code] has with other regions in the map.
+			</description>
+		</method>
 		<method name="region_get_layers" qualifiers="const">
 			<return type="int">
 			</return>
@@ -398,6 +429,15 @@
 			</description>
 		</method>
 	</methods>
+	<signals>
+		<signal name="map_changed">
+			<argument index="0" name="map" type="RID">
+			</argument>
+			<description>
+				Emitted when a navigation map is updated, when a region moves or is modified.
+			</description>
+		</signal>
+	</signals>
 	<constants>
 	</constants>
 </class>

modules/gdnavigation/gd_navigation_server.cpp

@@ -145,9 +145,13 @@ COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
 	if (p_active) {
 		if (!map_is_active(p_map)) {
 			active_maps.push_back(map);
+			active_maps_update_id.push_back(map->get_map_update_id());
 		}
 	} else {
-		active_maps.erase(map);
+		int map_index = active_maps.find(map);
+		ERR_FAIL_COND(map_index < 0);
+		active_maps.remove(map_index);
+		active_maps_update_id.remove(map_index);
 	}
 }
 
@@ -304,6 +308,27 @@ void GdNavigationServer::region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p
 #endif
 }
 
+int GdNavigationServer::region_get_connections_count(RID p_region) const {
+	NavRegion *region = region_owner.getornull(p_region);
+	ERR_FAIL_COND_V(!region, 0);
+
+	return region->get_connections_count();
+}
+
+Vector3 GdNavigationServer::region_get_connection_pathway_start(RID p_region, int p_connection_id) const {
+	NavRegion *region = region_owner.getornull(p_region);
+	ERR_FAIL_COND_V(!region, Vector3());
+
+	return region->get_connection_pathway_start(p_connection_id);
+}
+
+Vector3 GdNavigationServer::region_get_connection_pathway_end(RID p_region, int p_connection_id) const {
+	NavRegion *region = region_owner.getornull(p_region);
+	ERR_FAIL_COND_V(!region, Vector3());
+
+	return region->get_connection_pathway_end(p_connection_id);
+}
+
 RID GdNavigationServer::agent_create() const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
 	MutexLock lock(mut_this->operations_mutex);
@@ -443,7 +468,9 @@ COMMAND_1(free, RID, p_object) {
 			agents[i]->set_map(nullptr);
 		}
 
-		active_maps.erase(map);
+		int map_index = active_maps.find(map);
+		active_maps.remove(map_index);
+		active_maps_update_id.remove(map_index);
 		map_owner.free(p_object);
 		memdelete(map);
 
@@ -504,10 +531,17 @@ void GdNavigationServer::process(real_t p_delta_time) {
 	// In c++ we can't be sure that this is performed in the main thread
 	// even with mutable functions.
 	MutexLock lock(operations_mutex);
-	for (int i(0); i < active_maps.size(); i++) {
+	for (uint32_t i(0); i < active_maps.size(); i++) {
 		active_maps[i]->sync();
 		active_maps[i]->step(p_delta_time);
 		active_maps[i]->dispatch_callbacks();
+
+		// Emit a signal if a map changed.
+		const uint32_t new_map_update_id = active_maps[i]->get_map_update_id();
+		if (new_map_update_id != active_maps_update_id[i]) {
+			emit_signal("map_changed", active_maps[i]->get_self());
+			active_maps_update_id[i] = new_map_update_id;
+		}
 	}
 }
 

modules/gdnavigation/gd_navigation_server.h

@@ -31,6 +31,7 @@
 #ifndef GD_NAVIGATION_SERVER_H
 #define GD_NAVIGATION_SERVER_H
 
+#include "core/templates/local_vector.h"
 #include "core/templates/rid.h"
 #include "core/templates/rid_owner.h"
 #include "servers/navigation_server_3d.h"
@@ -79,7 +80,8 @@ class GdNavigationServer : public NavigationServer3D {
 	mutable RID_PtrOwner<RvoAgent> agent_owner;
 
 	bool active = true;
-	Vector<NavMap *> active_maps;
+	LocalVector<NavMap *> active_maps;
+	LocalVector<uint32_t> active_maps_update_id;
 
 public:
 	GdNavigationServer();
@@ -114,6 +116,9 @@ public:
 	COMMAND_2(region_set_transform, RID, p_region, Transform, p_transform);
 	COMMAND_2(region_set_navmesh, RID, p_region, Ref<NavigationMesh>, p_nav_mesh);
 	virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const;
+	virtual int region_get_connections_count(RID p_region) const;
+	virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const;
+	virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const;
 
 	virtual RID agent_create() const;
 	COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);

modules/gdnavigation/nav_map.cpp

@@ -40,7 +40,7 @@
 	@author AndreaCatania
 */
 
-#define USE_ENTRY_POINT
+#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
 
 void NavMap::set_up(Vector3 p_up) {
 	up = p_up;
@@ -71,13 +71,13 @@ gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const {
 }
 
 Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_layers) const {
+	// Find the start poly and the end poly on this map.
 	const gd::Polygon *begin_poly = nullptr;
 	const gd::Polygon *end_poly = nullptr;
 	Vector3 begin_point;
 	Vector3 end_point;
 	float begin_d = 1e20;
 	float end_d = 1e20;
-
 	// Find the initial poly and the end poly on this map.
 	for (size_t i(0); i < polygons.size(); i++) {
 		const gd::Polygon &p = polygons[i];
@@ -88,31 +88,34 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 		}
 
 		// For each point cast a face and check the distance between the origin/destination
-		for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
-			Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
-			Vector3 spoint = f.get_closest_point_to(p_origin);
-			float dpoint = spoint.distance_to(p_origin);
-			if (dpoint < begin_d) {
-				begin_d = dpoint;
+		for (size_t point_id = 0; point_id < p.points.size(); point_id++) {
+			const Vector3 p1 = p.points[point_id].pos;
+			const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos;
+			const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos;
+			const Face3 face(p1, p2, p3);
+
+			Vector3 point = face.get_closest_point_to(p_origin);
+			float distance_to_point = point.distance_to(p_origin);
+			if (distance_to_point < begin_d) {
+				begin_d = distance_to_point;
 				begin_poly = &p;
-				begin_point = spoint;
+				begin_point = point;
 			}
 
-			spoint = f.get_closest_point_to(p_destination);
-			dpoint = spoint.distance_to(p_destination);
-			if (dpoint < end_d) {
-				end_d = dpoint;
+			point = face.get_closest_point_to(p_destination);
+			distance_to_point = point.distance_to(p_destination);
+			if (distance_to_point < end_d) {
+				end_d = distance_to_point;
 				end_poly = &p;
-				end_point = spoint;
+				end_point = point;
 			}
 		}
 	}
 
+	// Check for trival cases
 	if (!begin_poly || !end_poly) {
-		// No path
 		return Vector<Vector3>();
 	}
-
 	if (begin_poly == end_poly) {
 		Vector<Vector3> path;
 		path.resize(2);
@@ -121,95 +124,89 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 		return path;
 	}
 
+	// List of all reachable navigation polys.
 	std::vector<gd::NavigationPoly> navigation_polys;
 	navigation_polys.reserve(polygons.size() * 0.75);
 
-	// The elements indices in the `navigation_polys`.
-	int least_cost_id(-1);
-	List<uint32_t> open_list;
-	bool found_route = false;
+	// Add the start polygon to the reachable navigation polygons.
+	gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly);
+	begin_navigation_poly.self_id = 0;
+	begin_navigation_poly.entry = begin_point;
+	begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
+	begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
+	navigation_polys.push_back(begin_navigation_poly);
 
-	navigation_polys.push_back(gd::NavigationPoly(begin_poly));
-	{
-		least_cost_id = 0;
-		gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
-		least_cost_poly->self_id = least_cost_id;
-		least_cost_poly->entry = begin_point;
-	}
+	// List of polygon IDs to visit.
+	List<uint32_t> to_visit;
+	to_visit.push_back(0);
 
-	open_list.push_back(0);
+	// This is an implementation of the A* algorithm.
+	int least_cost_id = 0;
+	bool found_route = false;
 
 	const gd::Polygon *reachable_end = nullptr;
 	float reachable_d = 1e30;
 	bool is_reachable = true;
 
-	while (found_route == false) {
-		{
-			// Takes the current least_cost_poly neighbors and compute the traveled_distance of each
-			for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) {
-				gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
+	while (true) {
+		gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id];
 
-				// Only consider the polygon if it in a region with compatible layers.
-				if ((p_layers & least_cost_poly->poly->owner->get_layers()) == 0) {
-					continue;
-				}
+		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
+		for (size_t i = 0; i < least_cost_poly->poly->edges.size(); i++) {
+			const gd::Edge &edge = least_cost_poly->poly->edges[i];
+
+			// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
+			for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
+				const gd::Edge::Connection &connection = edge.connections[connection_index];
 
-				const gd::Edge &edge = least_cost_poly->poly->edges[i];
-				if (!edge.other_polygon) {
+				// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
+				if ((p_layers & connection.polygon->owner->get_layers()) == 0) {
 					continue;
 				}
 
-#ifdef USE_ENTRY_POINT
-				Vector3 edge_line[2] = {
-					least_cost_poly->poly->points[i].pos,
-					least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos
-				};
-
-				const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, edge_line);
+				Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end };
+				const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly->entry, pathway);
 				const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance;
-#else
-				const float new_distance = least_cost_poly->poly->center.distance_to(edge.other_polygon->center) + least_cost_poly->traveled_distance;
-#endif
 
 				auto it = std::find(
 						navigation_polys.begin(),
 						navigation_polys.end(),
-						gd::NavigationPoly(edge.other_polygon));
+						gd::NavigationPoly(connection.polygon));
 
 				if (it != navigation_polys.end()) {
-					// Oh this was visited already, can we win the cost?
-					if (it->traveled_distance > new_distance) {
-						it->prev_navigation_poly_id = least_cost_id;
-						it->back_navigation_edge = edge.other_edge;
+					// Polygon already visited, check if we can reduce the travel cost.
+					if (new_distance < it->traveled_distance) {
+						it->back_navigation_poly_id = least_cost_id;
+						it->back_navigation_edge = connection.edge;
+						it->back_navigation_edge_pathway_start = connection.pathway_start;
+						it->back_navigation_edge_pathway_end = connection.pathway_end;
 						it->traveled_distance = new_distance;
-#ifdef USE_ENTRY_POINT
 						it->entry = new_entry;
-#endif
 					}
 				} else {
-					// Add to open neighbours
-
-					navigation_polys.push_back(gd::NavigationPoly(edge.other_polygon));
-					gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1];
-
-					np->self_id = navigation_polys.size() - 1;
-					np->prev_navigation_poly_id = least_cost_id;
-					np->back_navigation_edge = edge.other_edge;
-					np->traveled_distance = new_distance;
-#ifdef USE_ENTRY_POINT
-					np->entry = new_entry;
-#endif
-					open_list.push_back(navigation_polys.size() - 1);
+					// Add the neighbour polygon to the reachable ones.
+					gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon);
+					new_navigation_poly.self_id = navigation_polys.size();
+					new_navigation_poly.back_navigation_poly_id = least_cost_id;
+					new_navigation_poly.back_navigation_edge = connection.edge;
+					new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start;
+					new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end;
+					new_navigation_poly.traveled_distance = new_distance;
+					new_navigation_poly.entry = new_entry;
+					navigation_polys.push_back(new_navigation_poly);
+
+					// Add the neighbour polygon to the polygons to visit.
+					to_visit.push_back(navigation_polys.size() - 1);
 				}
 			}
 		}
 
-		// Removes the least cost polygon from the open list so we can advance.
-		open_list.erase(least_cost_id);
+		// Removes the least cost polygon from the list of polygons to visit so we can advance.
+		to_visit.erase(least_cost_id);
 
-		if (open_list.size() == 0) {
-			// When the open list is empty at this point the End Polygon is not reachable
-			// so use the further reachable polygon
+		// When the list of polygons to visit is empty at this point it means the End Polygon is not reachable
+		if (to_visit.size() == 0) {
+			// Thus use the further reachable polygon
 			ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
 			is_reachable = false;
 			if (reachable_end == nullptr) {
@@ -234,26 +231,21 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 			gd::NavigationPoly np = navigation_polys[0];
 			navigation_polys.clear();
 			navigation_polys.push_back(np);
-			open_list.clear();
-			open_list.push_back(0);
+			to_visit.clear();
+			to_visit.push_back(0);
 
 			reachable_end = nullptr;
 
 			continue;
 		}
 
-		// Now take the new least_cost_poly from the open list.
+		// Find the polygon with the minimum cost from the list of polygons to visit.
 		least_cost_id = -1;
 		float least_cost = 1e30;
-
-		for (auto element = open_list.front(); element != nullptr; element = element->next()) {
+		for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) {
 			gd::NavigationPoly *np = &navigation_polys[element->get()];
 			float cost = np->traveled_distance;
-#ifdef USE_ENTRY_POINT
 			cost += np->entry.distance_to(end_point);
-#else
-			cost += np->poly->center.distance_to(end_point);
-#endif
 			if (cost < least_cost) {
 				least_cost_id = np->self_id;
 				least_cost = cost;
@@ -273,124 +265,108 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p
 
 		// Check if we reached the end
 		if (navigation_polys[least_cost_id].poly == end_poly) {
-			// Yep, done!!
 			found_route = true;
 			break;
 		}
 	}
 
-	if (found_route) {
-		Vector<Vector3> path;
-		if (p_optimize) {
-			// String pulling
-
-			gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
-			Vector3 apex_point = end_point;
-			Vector3 portal_left = apex_point;
-			Vector3 portal_right = apex_point;
-			gd::NavigationPoly *left_poly = apex_poly;
-			gd::NavigationPoly *right_poly = apex_poly;
-			gd::NavigationPoly *p = apex_poly;
-
-			path.push_back(end_point);
+	// If we did not find a route, return an empty path.
+	if (!found_route) {
+		return Vector<Vector3>();
+	}
 
-			while (p) {
-				Vector3 left;
-				Vector3 right;
+	Vector<Vector3> path;
+	// Optimize the path.
+	if (p_optimize) {
+		// Set the apex poly/point to the end point
+		gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+		Vector3 apex_point = end_point;
 
-#define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b)))
+		gd::NavigationPoly *left_poly = apex_poly;
+		Vector3 left_portal = apex_point;
+		gd::NavigationPoly *right_poly = apex_poly;
+		Vector3 right_portal = apex_point;
 
-				if (p->poly == begin_poly) {
-					left = begin_point;
-					right = begin_point;
-				} else {
-					int prev = p->back_navigation_edge;
-					int prev_n = (p->back_navigation_edge + 1) % p->poly->points.size();
-					left = p->poly->points[prev].pos;
-					right = p->poly->points[prev_n].pos;
+		gd::NavigationPoly *p = apex_poly;
 
-					if (p->poly->clockwise) {
-						SWAP(left, right);
-					}
-				}
+		path.push_back(end_point);
 
-				bool skip = false;
-
-				if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) {
-					//process
-					if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) {
-						left_poly = p;
-						portal_left = left;
-					} else {
-						clip_path(navigation_polys, path, apex_poly, portal_right, right_poly);
-
-						apex_point = portal_right;
-						p = right_poly;
-						left_poly = p;
-						apex_poly = p;
-						portal_left = apex_point;
-						portal_right = apex_point;
-						path.push_back(apex_point);
-						skip = true;
-					}
-				}
+		while (p) {
+			// Set left and right points of the pathway between polygons.
+			Vector3 left = p->back_navigation_edge_pathway_start;
+			Vector3 right = p->back_navigation_edge_pathway_end;
+			if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) {
+				SWAP(left, right);
+			}
 
-				if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) {
-					//process
-					if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) {
-						right_poly = p;
-						portal_right = right;
-					} else {
-						clip_path(navigation_polys, path, apex_poly, portal_left, left_poly);
-
-						apex_point = portal_left;
-						p = left_poly;
-						right_poly = p;
-						apex_poly = p;
-						portal_right = apex_point;
-						portal_left = apex_point;
-						path.push_back(apex_point);
-					}
+			bool skip = false;
+			if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) {
+				//process
+				if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) {
+					left_poly = p;
+					left_portal = left;
+				} else {
+					clip_path(navigation_polys, path, apex_poly, right_portal, right_poly);
+
+					apex_point = right_portal;
+					p = right_poly;
+					left_poly = p;
+					apex_poly = p;
+					left_portal = apex_point;
+					right_portal = apex_point;
+					path.push_back(apex_point);
+					skip = true;
 				}
+			}
 
-				if (p->prev_navigation_poly_id != -1) {
-					p = &navigation_polys[p->prev_navigation_poly_id];
+			if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) {
+				//process
+				if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) {
+					right_poly = p;
+					right_portal = right;
 				} else {
-					// The end
-					p = nullptr;
+					clip_path(navigation_polys, path, apex_poly, left_portal, left_poly);
+
+					apex_point = left_portal;
+					p = left_poly;
+					right_poly = p;
+					apex_poly = p;
+					right_portal = apex_point;
+					left_portal = apex_point;
+					path.push_back(apex_point);
 				}
 			}
 
-			if (path[path.size() - 1] != begin_point) {
-				path.push_back(begin_point);
+			// Go to the previous polygon.
+			if (p->back_navigation_poly_id != -1) {
+				p = &navigation_polys[p->back_navigation_poly_id];
+			} else {
+				// The end
+				p = nullptr;
 			}
+		}
 
-			path.invert();
-
-		} else {
-			path.push_back(end_point);
+		// If the last point is not the begin point, add it to the list.
+		if (path[path.size() - 1] != begin_point) {
+			path.push_back(begin_point);
+		}
 
-			// Add mid points
-			int np_id = least_cost_id;
-			while (np_id != -1) {
-#ifdef USE_ENTRY_POINT
-				Vector3 point = navigation_polys[np_id].entry;
-#else
-				int prev = navigation_polys[np_id].back_navigation_edge;
-				int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
-				Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
-#endif
+		path.invert();
 
-				path.push_back(point);
-				np_id = navigation_polys[np_id].prev_navigation_poly_id;
-			}
+	} else {
+		path.push_back(end_point);
 
-			path.invert();
+		// Add mid points
+		int np_id = least_cost_id;
+		while (np_id != -1) {
+			path.push_back(navigation_polys[np_id].entry);
+			np_id = navigation_polys[np_id].back_navigation_poly_id;
 		}
 
-		return path;
+		path.invert();
 	}
-	return Vector<Vector3>();
+
+	return path;
 }
 
 Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
@@ -571,6 +547,7 @@ void NavMap::remove_agent_as_controlled(RvoAgent *agent) {
 }
 
 void NavMap::sync() {
+	// Check if we need to update the links.
 	if (regenerate_polygons) {
 		for (size_t r(0); r < regions.size(); r++) {
 			regions[r]->scratch_polygons();
@@ -585,27 +562,30 @@ void NavMap::sync() {
 	}
 
 	if (regenerate_links) {
-		// Copy all region polygons in the map.
+		// Remove regions connections.
+		for (size_t r(0); r < regions.size(); r++) {
+			regions[r]->get_connections().clear();
+		}
+
+		// Resize the polygon count.
 		int count = 0;
 		for (size_t r(0); r < regions.size(); r++) {
 			count += regions[r]->get_polygons().size();
 		}
-
 		polygons.resize(count);
-		count = 0;
 
+		// Copy all region polygons in the map.
+		count = 0;
 		for (size_t r(0); r < regions.size(); r++) {
 			std::copy(
 					regions[r]->get_polygons().data(),
 					regions[r]->get_polygons().data() + regions[r]->get_polygons().size(),
 					polygons.begin() + count);
-
 			count += regions[r]->get_polygons().size();
 		}
 
-		// Connects the `Edges` of all the `Polygons` of all `Regions` each other.
-		Map<gd::EdgeKey, gd::Connection> connections;
-
+		// Group all edges per key.
+		Map<gd::EdgeKey, Vector<gd::Edge::Connection>> connections;
 		for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) {
 			gd::Polygon &poly(polygons[poly_id]);
 
@@ -613,30 +593,18 @@ void NavMap::sync() {
 				int next_point = (p + 1) % poly.points.size();
 				gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
 
-				Map<gd::EdgeKey, gd::Connection>::Element *connection = connections.find(ek);
+				Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *connection = connections.find(ek);
 				if (!connection) {
-					// Nothing yet
-					gd::Connection c;
-					c.A = &poly;
-					c.A_edge = p;
-					c.B = nullptr;
-					c.B_edge = -1;
-					connections[ek] = c;
-
-				} else if (connection->get().B == nullptr) {
-					CRASH_COND(connection->get().A == nullptr); // Unreachable
-
-					// Connect the two Polygons by this edge
-					connection->get().B = &poly;
-					connection->get().B_edge = p;
-
-					connection->get().A->edges[connection->get().A_edge].this_edge = connection->get().A_edge;
-					connection->get().A->edges[connection->get().A_edge].other_polygon = connection->get().B;
-					connection->get().A->edges[connection->get().A_edge].other_edge = connection->get().B_edge;
-
-					connection->get().B->edges[connection->get().B_edge].this_edge = connection->get().B_edge;
-					connection->get().B->edges[connection->get().B_edge].other_polygon = connection->get().A;
-					connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge;
+					connections[ek] = Vector<gd::Edge::Connection>();
+				}
+				if (connections[ek].size() <= 1) {
+					// Add the polygon/edge tuple to this key.
+					gd::Edge::Connection new_connection;
+					new_connection.polygon = &poly;
+					new_connection.edge = p;
+					new_connection.pathway_start = poly.points[p].pos;
+					new_connection.pathway_end = poly.points[next_point].pos;
+					connections[ek].push_back(new_connection);
 				} else {
 					// The edge is already connected with another edge, skip.
 					ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem.");
@@ -644,38 +612,21 @@ void NavMap::sync() {
 			}
 		}
 
-		// Takes all the free edges.
-		std::vector<gd::FreeEdge> free_edges;
-		free_edges.reserve(connections.size());
-
-		for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) {
-			if (connection_element->get().B == nullptr) {
-				CRASH_COND(connection_element->get().A == nullptr); // Unreachable
-				CRASH_COND(connection_element->get().A_edge < 0); // Unreachable
-
-				// This is a free edge
-				uint32_t id(free_edges.size());
-				free_edges.push_back(gd::FreeEdge());
-				free_edges[id].is_free = true;
-				free_edges[id].poly = connection_element->get().A;
-				free_edges[id].edge_id = connection_element->get().A_edge;
-				uint32_t point_0(free_edges[id].edge_id);
-				uint32_t point_1((free_edges[id].edge_id + 1) % free_edges[id].poly->points.size());
-				Vector3 pos_0 = free_edges[id].poly->points[point_0].pos;
-				Vector3 pos_1 = free_edges[id].poly->points[point_1].pos;
-				Vector3 relative = pos_1 - pos_0;
-				free_edges[id].edge_center = (pos_0 + pos_1) / 2.0;
-				free_edges[id].edge_dir = relative.normalized();
-				free_edges[id].edge_len_squared = relative.length_squared();
+		Vector<gd::Edge::Connection> free_edges;
+		for (Map<gd::EdgeKey, Vector<gd::Edge::Connection>>::Element *E = connections.front(); E; E = E->next()) {
+			if (E->get().size() == 2) {
+				// Connect edge that are shared in different polygons.
+				gd::Edge::Connection &c1 = E->get().write[0];
+				gd::Edge::Connection &c2 = E->get().write[1];
+				c1.polygon->edges[c1.edge].connections.push_back(c2);
+				c2.polygon->edges[c2.edge].connections.push_back(c1);
+				// Note: The pathway_start/end are full for those connection and do not need to be modified.
+			} else {
+				CRASH_COND_MSG(E->get().size() != 1, vformat("Number of connection != 1. Found: %d", E->get().size()));
+				free_edges.push_back(E->get()[0]);
 			}
 		}
 
-		const float ecm_squared(edge_connection_margin * edge_connection_margin);
-#define LEN_TOLLERANCE 0.1
-#define DIR_TOLLERANCE 0.9
-		// In front of tolerance
-#define IFO_TOLLERANCE 0.5
-
 		// Find the compatible near edges.
 		//
 		// Note:
@@ -683,43 +634,67 @@ void NavMap::sync() {
 		// to be connected, create new polygons to remove that small gap is
 		// not really useful and would result in wasteful computation during
 		// connection, integration and path finding.
-		for (size_t i(0); i < free_edges.size(); i++) {
-			if (!free_edges[i].is_free) {
-				continue;
-			}
-			gd::FreeEdge &edge = free_edges[i];
-			for (size_t y(0); y < free_edges.size(); y++) {
-				gd::FreeEdge &other_edge = free_edges[y];
-				if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) {
+		for (int i = 0; i < free_edges.size(); i++) {
+			const gd::Edge::Connection &free_edge = free_edges[i];
+			Vector3 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
+			Vector3 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
+
+			for (int j = 0; j < free_edges.size(); j++) {
+				const gd::Edge::Connection &other_edge = free_edges[j];
+				if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
+					continue;
+				}
+
+				Vector3 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
+				Vector3 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
+
+				// Compute the projection of the opposite edge on the current one
+				Vector3 edge_vector = edge_p2 - edge_p1;
+				float projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / (edge_vector.length_squared());
+				float projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / (edge_vector.length_squared());
+				if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
 					continue;
 				}
 
-				Vector3 rel_centers = other_edge.edge_center - edge.edge_center;
-				if (ecm_squared > rel_centers.length_squared() // Are enough closer?
-						&& ABS(edge.edge_len_squared - other_edge.edge_len_squared) < LEN_TOLLERANCE // Are the same length?
-						&& ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLLERANCE // Are aligned?
-						&& ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLLERANCE // Are one in front the other?
-				) {
-					// The edges can be connected
-					edge.is_free = false;
-					other_edge.is_free = false;
-
-					edge.poly->edges[edge.edge_id].this_edge = edge.edge_id;
-					edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id;
-					edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly;
-
-					other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id;
-					other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id;
-					other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly;
+				// Check if the two edges are close to each other enough and compute a pathway between the two regions.
+				Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+				Vector3 other1;
+				if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
+					other1 = other_edge_p1;
+				} else {
+					other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
 				}
+				if ((self1 - other1).length() > edge_connection_margin) {
+					continue;
+				}
+
+				Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+				Vector3 other2;
+				if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
+					other2 = other_edge_p2;
+				} else {
+					other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+				}
+				if ((self2 - other2).length() > edge_connection_margin) {
+					continue;
+				}
+
+				// The edges can now be connected.
+				gd::Edge::Connection new_connection = other_edge;
+				new_connection.pathway_start = (self1 + other1) / 2.0;
+				new_connection.pathway_end = (self2 + other2) / 2.0;
+				free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
+
+				// Add the connection to the region_connection map.
+				free_edge.polygon->owner->get_connections().push_back(new_connection);
 			}
 		}
-	}
 
-	if (regenerate_links) {
+		// Update the update ID.
 		map_update_id = (map_update_id + 1) % 9999999;
 	}
 
+	// Update agents tree.
 	if (agents_dirty) {
 		std::vector<RVO::Agent *> raw_agents;
 		raw_agents.reserve(agents.size());
@@ -771,16 +746,15 @@ void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys
 	cut_plane.d = cut_plane.normal.dot(from);
 
 	while (from_poly != p_to_poly) {
-		int back_nav_edge = from_poly->back_navigation_edge;
-		Vector3 a = from_poly->poly->points[back_nav_edge].pos;
-		Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos;
+		Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start;
+		Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end;
 
-		ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1);
-		from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id];
+		ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1);
+		from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id];
 
-		if (a.distance_to(b) > CMP_EPSILON) {
+		if (pathway_start.distance_to(pathway_end) > CMP_EPSILON) {
 			Vector3 inters;
-			if (cut_plane.intersects_segment(a, b, &inters)) {
+			if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) {
 				if (inters.distance_to(p_to_point) > CMP_EPSILON && inters.distance_to(path[path.size() - 1]) > CMP_EPSILON) {
 					path.push_back(inters);
 				}

modules/gdnavigation/nav_map.h

@@ -34,6 +34,7 @@
 #include "nav_rid.h"
 
 #include "core/math/math_defs.h"
+#include "core/templates/map.h"
 #include "nav_utils.h"
 #include <KdTree.h>
 

modules/gdnavigation/nav_region.cpp

@@ -39,6 +39,9 @@
 void NavRegion::set_map(NavMap *p_map) {
 	map = p_map;
 	polygons_dirty = true;
+	if (!map) {
+		connections.clear();
+	}
 }
 
 void NavRegion::set_layers(uint32_t p_layers) {
@@ -59,6 +62,25 @@ void NavRegion::set_mesh(Ref<NavigationMesh> p_mesh) {
 	polygons_dirty = true;
 }
 
+int NavRegion::get_connections_count() const {
+	if (!map) {
+		return 0;
+	}
+	return connections.size();
+}
+
+Vector3 NavRegion::get_connection_pathway_start(int p_connection_id) const {
+	ERR_FAIL_COND_V(!map, Vector3());
+	ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
+	return connections[p_connection_id].pathway_start;
+}
+
+Vector3 NavRegion::get_connection_pathway_end(int p_connection_id) const {
+	ERR_FAIL_COND_V(!map, Vector3());
+	ERR_FAIL_INDEX_V(p_connection_id, connections.size(), Vector3());
+	return connections[p_connection_id].pathway_end;
+}
+
 bool NavRegion::sync() {
 	bool something_changed = polygons_dirty /* || something_dirty? */;
 

modules/gdnavigation/nav_region.h

@@ -49,6 +49,7 @@ class NavRegion : public NavRid {
 	Transform transform;
 	Ref<NavigationMesh> mesh;
 	uint32_t layers = 1;
+	Vector<gd::Edge::Connection> connections;
 
 	bool polygons_dirty = true;
 
@@ -80,6 +81,13 @@ public:
 		return mesh;
 	}
 
+	Vector<gd::Edge::Connection> &get_connections() {
+		return connections;
+	}
+	int get_connections_count() const;
+	Vector3 get_connection_pathway_start(int p_connection_id) const;
+	Vector3 get_connection_pathway_end(int p_connection_id) const;
+
 	std::vector<gd::Polygon> const &get_polygons() const {
 		return polygons;
 	}

modules/gdnavigation/nav_utils.h

@@ -81,11 +81,14 @@ struct Edge {
 	/// This edge ID
 	int this_edge = -1;
 
-	/// Other Polygon
-	Polygon *other_polygon = nullptr;
-
-	/// The other `Polygon` at this edge id has this `Polygon`.
-	int other_edge = -1;
+	/// The gateway in the edge, as, in some case, the whole edge might not be navigable.
+	struct Connection {
+		Polygon *polygon = nullptr;
+		int edge = -1;
+		Vector3 pathway_start;
+		Vector3 pathway_end;
+	};
+	Vector<Connection> connections;
 };
 
 struct Polygon {
@@ -104,21 +107,17 @@ struct Polygon {
 	Vector3 center;
 };
 
-struct Connection {
-	Polygon *A = nullptr;
-	int A_edge = -1;
-	Polygon *B = nullptr;
-	int B_edge = -1;
-};
-
 struct NavigationPoly {
 	uint32_t self_id = 0;
 	/// This poly.
 	const Polygon *poly;
-	/// The previous navigation poly (id in the `navigation_poly` array).
-	int prev_navigation_poly_id = -1;
-	/// The edge id in this `Poly` to reach the `prev_navigation_poly_id`.
-	uint32_t back_navigation_edge = 0;
+
+	/// Those 4 variables are used to travel the path backwards.
+	int back_navigation_poly_id = -1;
+	uint32_t back_navigation_edge = UINT32_MAX;
+	Vector3 back_navigation_edge_pathway_start;
+	Vector3 back_navigation_edge_pathway_end;
+
 	/// The entry location of this poly.
 	Vector3 entry;
 	/// The distance to the destination.
@@ -136,14 +135,6 @@ struct NavigationPoly {
 	}
 };
 
-struct FreeEdge {
-	bool is_free = false;
-	Polygon *poly = nullptr;
-	uint32_t edge_id = 0;
-	Vector3 edge_center;
-	Vector3 edge_dir;
-	float edge_len_squared = 0.0;
-};
 } // namespace gd
 
 #endif // NAV_UTILS_H

modules/gridmap/doc_classes/GridMap.xml

@@ -214,12 +214,12 @@
 		<member name="collision_mask" type="int" setter="set_collision_mask" getter="get_collision_mask" default="1">
 			The physics layers this GridMap detects collisions in. See [url=https://docs.godotengine.org/en/latest/tutorials/physics/physics_introduction.html#collision-layers-and-masks]Collision layers and masks[/url] in the documentation for more information.
 		</member>
-		<member name="navigation_layers" type="int" setter="set_navigation_layers" getter="get_navigation_layers" default="1">
-			The navigation layers the GridMap generates its navigable regions in.
-		</member>
 		<member name="mesh_library" type="MeshLibrary" setter="set_mesh_library" getter="get_mesh_library">
 			The assigned [MeshLibrary].
 		</member>
+		<member name="navigation_layers" type="int" setter="set_navigation_layers" getter="get_navigation_layers" default="1">
+			The navigation layers the GridMap generates its navigable regions in.
+		</member>
 	</members>
 	<signals>
 		<signal name="cell_size_changed">

scene/2d/navigation_region_2d.cpp

@@ -364,8 +364,10 @@ void NavigationRegion2D::set_enabled(bool p_enabled) {
 
 	if (!enabled) {
 		NavigationServer2D::get_singleton()->region_set_map(region, RID());
+		NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
 	} else {
 		NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map());
+		NavigationServer2D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
 	}
 
 	if (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) {
@@ -401,6 +403,7 @@ void NavigationRegion2D::_notification(int p_what) {
 		case NOTIFICATION_ENTER_TREE: {
 			if (enabled) {
 				NavigationServer2D::get_singleton()->region_set_map(region, get_world_2d()->get_navigation_map());
+				NavigationServer2D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
 			}
 		} break;
 		case NOTIFICATION_TRANSFORM_CHANGED: {
@@ -408,12 +411,14 @@ void NavigationRegion2D::_notification(int p_what) {
 		} break;
 		case NOTIFICATION_EXIT_TREE: {
 			NavigationServer2D::get_singleton()->region_set_map(region, RID());
+			if (enabled) {
+				NavigationServer2D::get_singleton()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
+			}
 		} break;
 		case NOTIFICATION_DRAW: {
 			if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) && navpoly.is_valid()) {
 				Vector<Vector2> verts = navpoly->get_vertices();
-				int vsize = verts.size();
-				if (vsize < 3) {
+				if (verts.size() < 3) {
 					return;
 				}
 
@@ -423,33 +428,47 @@ void NavigationRegion2D::_notification(int p_what) {
 				} else {
 					color = get_tree()->get_debug_navigation_disabled_color();
 				}
-				Vector<Color> colors;
-				Vector<Vector2> vertices;
-				vertices.resize(vsize);
-				colors.resize(vsize);
-				{
-					const Vector2 *vr = verts.ptr();
-					for (int i = 0; i < vsize; i++) {
-						vertices.write[i] = vr[i];
-						colors.write[i] = color;
-					}
-				}
+				Color doors_color = color.lightened(0.2);
 
-				Vector<int> indices;
+				RandomPCG rand;
 
 				for (int i = 0; i < navpoly->get_polygon_count(); i++) {
+					// An array of vertices for this polygon.
 					Vector<int> polygon = navpoly->get_polygon(i);
-
-					for (int j = 2; j < polygon.size(); j++) {
-						int kofs[3] = { 0, j - 1, j };
-						for (int k = 0; k < 3; k++) {
-							int idx = polygon[kofs[k]];
-							ERR_FAIL_INDEX(idx, vsize);
-							indices.push_back(idx);
-						}
+					Vector<Vector2> vertices;
+					vertices.resize(polygon.size());
+					for (int j = 0; j < polygon.size(); j++) {
+						ERR_FAIL_INDEX(polygon[j], verts.size());
+						vertices.write[j] = verts[polygon[j]];
 					}
+
+					// Generate the polygon color, slightly randomly modified from the settings one.
+					Color random_variation_color;
+					random_variation_color.set_hsv(color.get_h() + rand.random(-1.0, 1.0) * 0.05, color.get_s(), color.get_v() + rand.random(-1.0, 1.0) * 0.1);
+					random_variation_color.a = color.a;
+					Vector<Color> colors;
+					colors.push_back(random_variation_color);
+
+					RS::get_singleton()->canvas_item_add_polygon(get_canvas_item(), vertices, colors);
+				}
+
+				// Draw the region
+				Transform2D xform = get_global_transform();
+				const NavigationServer2D *ns = NavigationServer2D::get_singleton();
+				float radius = ns->map_get_edge_connection_margin(get_world_2d()->get_navigation_map()) / 2.0;
+				for (int i = 0; i < ns->region_get_connections_count(region); i++) {
+					// Two main points
+					Vector2 a = ns->region_get_connection_pathway_start(region, i);
+					a = xform.affine_inverse().xform(a);
+					Vector2 b = ns->region_get_connection_pathway_end(region, i);
+					b = xform.affine_inverse().xform(b);
+					draw_line(a, b, doors_color);
+
+					// Draw a circle to illustrate the margins.
+					float angle = (b - a).angle();
+					draw_arc(a, radius, angle + Math_PI / 2.0, angle - Math_PI / 2.0 + Math_TAU, 10, doors_color);
+					draw_arc(b, radius, angle - Math_PI / 2.0, angle + Math_PI / 2.0, 10, doors_color);
 				}
-				RS::get_singleton()->canvas_item_add_triangle_array(get_canvas_item(), indices, vertices, colors);
 			}
 		} break;
 	}
@@ -484,6 +503,11 @@ void NavigationRegion2D::_navpoly_changed() {
 		update();
 	}
 }
+void NavigationRegion2D::_map_changed(RID p_map) {
+	if (enabled && get_world_2d()->get_navigation_map() == p_map) {
+		update();
+	}
+}
 
 String NavigationRegion2D::get_configuration_warning() const {
 	if (!is_visible_in_tree() || !is_inside_tree()) {

scene/2d/navigation_region_2d.h

@@ -99,6 +99,7 @@ class NavigationRegion2D : public Node2D {
 	Ref<NavigationPolygon> navpoly;
 
 	void _navpoly_changed();
+	void _map_changed(RID p_RID);
 
 protected:
 	void _notification(int p_what);

scene/main/viewport.cpp

@@ -1163,7 +1163,7 @@ void Viewport::set_world_2d(const Ref<World2D> &p_world_2d) {
 	if (p_world_2d.is_valid()) {
 		world_2d = p_world_2d;
 	} else {
-		WARN_PRINT("Invalid world_3d");
+		WARN_PRINT("Invalid world_2d");
 		world_2d = Ref<World2D>(memnew(World2D));
 	}
 

scene/resources/world_2d.cpp

@@ -368,7 +368,7 @@ World2D::World2D() {
 	navigation_map = NavigationServer2D::get_singleton()->map_create();
 	NavigationServer2D::get_singleton()->map_set_active(navigation_map, true);
 	NavigationServer2D::get_singleton()->map_set_cell_size(navigation_map, GLOBAL_DEF("navigation/2d/default_cell_size", 10));
-	NavigationServer2D::get_singleton()->map_set_edge_connection_margin(navigation_map, GLOBAL_DEF("navigation/2d/default_edge_connection_margin", 100));
+	NavigationServer2D::get_singleton()->map_set_edge_connection_margin(navigation_map, GLOBAL_DEF("navigation/2d/default_edge_connection_margin", 5));
 
 	indexer = memnew(SpatialIndexer2D);
 }

servers/navigation_server_2d.cpp

@@ -156,6 +156,10 @@ static Ref<NavigationMesh> poly_to_mesh(Ref<NavigationPolygon> d) {
 	}
 }
 
+void NavigationServer2D::_emit_map_changed(RID p_map) {
+	emit_signal("map_changed", p_map);
+}
+
 void NavigationServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("map_create"), &NavigationServer2D::map_create);
 	ClassDB::bind_method(D_METHOD("map_set_active", "map", "active"), &NavigationServer2D::map_set_active);
@@ -174,6 +178,9 @@ void NavigationServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("region_get_layers", "region"), &NavigationServer2D::region_get_layers);
 	ClassDB::bind_method(D_METHOD("region_set_transform", "region", "transform"), &NavigationServer2D::region_set_transform);
 	ClassDB::bind_method(D_METHOD("region_set_navpoly", "region", "nav_poly"), &NavigationServer2D::region_set_navpoly);
+	ClassDB::bind_method(D_METHOD("region_get_connections_count", "region"), &NavigationServer2D::region_get_connections_count);
+	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_start", "region", "connection"), &NavigationServer2D::region_get_connection_pathway_start);
+	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_end", "region", "connection"), &NavigationServer2D::region_get_connection_pathway_end);
 
 	ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer2D::agent_create);
 	ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer2D::agent_set_map);
@@ -189,10 +196,14 @@ void NavigationServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("agent_set_callback", "agent", "receiver", "method", "userdata"), &NavigationServer2D::agent_set_callback, DEFVAL(Variant()));
 
 	ClassDB::bind_method(D_METHOD("free", "object"), &NavigationServer2D::free);
+
+	ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map")));
 }
 
 NavigationServer2D::NavigationServer2D() {
 	singleton = this;
+	ERR_FAIL_COND_MSG(!NavigationServer3D::get_singleton(), "The Navigation3D singleton should be initialized before the 2D one.");
+	NavigationServer3D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationServer2D::_emit_map_changed));
 }
 
 NavigationServer2D::~NavigationServer2D() {
@@ -226,6 +237,10 @@ void NavigationServer2D::region_set_navpoly(RID p_region, Ref<NavigationPolygon>
 	NavigationServer3D::get_singleton()->region_set_navmesh(p_region, poly_to_mesh(p_nav_mesh));
 }
 
+int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid);
+Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
+Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
+
 RID NavigationServer2D::agent_create() const {
 	RID agent = NavigationServer3D::get_singleton()->agent_create();
 	NavigationServer3D::get_singleton()->agent_set_ignore_y(agent, true);

servers/navigation_server_2d.h

@@ -45,12 +45,14 @@ class NavigationServer2D : public Object {
 
 	static NavigationServer2D *singleton;
 
+	void _emit_map_changed(RID p_map);
+
 protected:
 	static void _bind_methods();
 
 public:
 	/// Thread safe, can be used across many threads.
-	static const NavigationServer2D *get_singleton() { return singleton; }
+	static NavigationServer2D *get_singleton() { return singleton; }
 
 	/// MUST be used in single thread!
 	static NavigationServer2D *get_singleton_mut() { return singleton; }
@@ -98,6 +100,11 @@ public:
 	/// Set the navigation poly of this region.
 	virtual void region_set_navpoly(RID p_region, Ref<NavigationPolygon> p_nav_mesh) const;
 
+	/// Get a list of a region's connection to other regions.
+	virtual int region_get_connections_count(RID p_region) const;
+	virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const;
+	virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const;
+
 	/// Creates the agent.
 	virtual RID agent_create() const;
 

servers/navigation_server_3d.cpp

@@ -59,6 +59,9 @@ void NavigationServer3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("region_set_transform", "region", "transform"), &NavigationServer3D::region_set_transform);
 	ClassDB::bind_method(D_METHOD("region_set_navmesh", "region", "nav_mesh"), &NavigationServer3D::region_set_navmesh);
 	ClassDB::bind_method(D_METHOD("region_bake_navmesh", "mesh", "node"), &NavigationServer3D::region_bake_navmesh);
+	ClassDB::bind_method(D_METHOD("region_get_connections_count", "region"), &NavigationServer3D::region_get_connections_count);
+	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_start", "region", "connection"), &NavigationServer3D::region_get_connection_pathway_start);
+	ClassDB::bind_method(D_METHOD("region_get_connection_pathway_end", "region", "connection"), &NavigationServer3D::region_get_connection_pathway_end);
 
 	ClassDB::bind_method(D_METHOD("agent_create"), &NavigationServer3D::agent_create);
 	ClassDB::bind_method(D_METHOD("agent_set_map", "agent", "map"), &NavigationServer3D::agent_set_map);
@@ -77,9 +80,11 @@ void NavigationServer3D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("set_active", "active"), &NavigationServer3D::set_active);
 	ClassDB::bind_method(D_METHOD("process", "delta_time"), &NavigationServer3D::process);
+
+	ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map")));
 }
 
-const NavigationServer3D *NavigationServer3D::get_singleton() {
+NavigationServer3D *NavigationServer3D::get_singleton() {
 	return singleton;
 }
 

servers/navigation_server_3d.h

@@ -55,7 +55,7 @@ protected:
 
 public:
 	/// Thread safe, can be used across many threads.
-	static const NavigationServer3D *get_singleton();
+	static NavigationServer3D *get_singleton();
 
 	/// MUST be used in single thread!
 	static NavigationServer3D *get_singleton_mut();
@@ -111,9 +111,14 @@ public:
 	/// Set the navigation mesh of this region.
 	virtual void region_set_navmesh(RID p_region, Ref<NavigationMesh> p_nav_mesh) const = 0;
 
-	/// Bake the navigation mesh
+	/// Bake the navigation mesh.
 	virtual void region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p_node) const = 0;
 
+	/// Get a list of a region's connection to other regions.
+	virtual int region_get_connections_count(RID p_region) const = 0;
+	virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const = 0;
+	virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const = 0;
+
 	/// Creates the agent.
 	virtual RID agent_create() const = 0;