Add Navigation Debug for GridMap edge connections

Adds navigation visual debug for GridMap edge connections that use baked navigationmesh with bake_navigation=true.

2 files changed, 187 insertions, 1 deletions

diff --git a/modules/gridmap/grid_map.cpp b/modules/gridmap/grid_map.cpp
index 7d80cbef7c..5e1d579f35 100644
--- a/modules/gridmap/grid_map.cpp
+++ b/modules/gridmap/grid_map.cpp
@@ -569,7 +569,9 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 				NavigationServer3D::get_singleton()->region_set_navigation_layers(region, navigation_layers);
 				NavigationServer3D::get_singleton()->region_set_navmesh(region, navmesh);
 				NavigationServer3D::get_singleton()->region_set_transform(region, get_global_transform() * nm.xform);
-				NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
+				if (is_inside_tree()) {
+					NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
+				}
 				nm.region = region;
 
 				// add navigation debugmesh visual instances if debug is enabled
@@ -592,6 +594,12 @@ bool GridMap::_octant_update(const OctantKey &p_key) {
 		}
 	}
 
+#ifdef DEBUG_ENABLED
+	if (bake_navigation) {
+		_update_octant_navigation_debug_edge_connections_mesh(p_key);
+	}
+#endif // DEBUG_ENABLED
+
 	//update multimeshes, only if not baked
 	if (baked_meshes.size() == 0) {
 		for (const KeyValue<int, List<Pair<Transform3D, IndexKey>>> &E : multimesh_items) {
@@ -686,6 +694,19 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
 				}
 			}
 		}
+
+#ifdef DEBUG_ENABLED
+		if (bake_navigation) {
+			if (!g.navigation_debug_edge_connections_instance.is_valid()) {
+				g.navigation_debug_edge_connections_instance = RenderingServer::get_singleton()->instance_create();
+			}
+			if (!g.navigation_debug_edge_connections_mesh.is_valid()) {
+				g.navigation_debug_edge_connections_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+			}
+
+			_update_octant_navigation_debug_edge_connections_mesh(p_key);
+		}
+#endif // DEBUG_ENABLED
 	}
 }
 
@@ -713,6 +734,18 @@ void GridMap::_octant_exit_world(const OctantKey &p_key) {
 			F.value.navmesh_debug_instance = RID();
 		}
 	}
+
+#ifdef DEBUG_ENABLED
+	if (bake_navigation) {
+		if (g.navigation_debug_edge_connections_instance.is_valid()) {
+			RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_instance);
+			g.navigation_debug_edge_connections_instance = RID();
+		}
+		if (g.navigation_debug_edge_connections_mesh.is_valid()) {
+			RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_mesh->get_rid());
+		}
+	}
+#endif // DEBUG_ENABLED
 }
 
 void GridMap::_octant_clean_up(const OctantKey &p_key) {
@@ -739,6 +772,18 @@ void GridMap::_octant_clean_up(const OctantKey &p_key) {
 	}
 	g.navmesh_ids.clear();
 
+#ifdef DEBUG_ENABLED
+	if (bake_navigation) {
+		if (g.navigation_debug_edge_connections_instance.is_valid()) {
+			RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_instance);
+			g.navigation_debug_edge_connections_instance = RID();
+		}
+		if (g.navigation_debug_edge_connections_mesh.is_valid()) {
+			RenderingServer::get_singleton()->free(g.navigation_debug_edge_connections_mesh->get_rid());
+		}
+	}
+#endif // DEBUG_ENABLED
+
 	//erase multimeshes
 
 	for (int i = 0; i < g.multimesh_instances.size(); i++) {
@@ -763,6 +808,14 @@ void GridMap::_notification(int p_what) {
 			}
 		} break;
 
+#ifdef DEBUG_ENABLED
+		case NOTIFICATION_ENTER_TREE: {
+			if (bake_navigation && NavigationServer3D::get_singleton()->get_debug_enabled()) {
+				_update_navigation_debug_edge_connections();
+			}
+		} break;
+#endif // DEBUG_ENABLED
+
 		case NOTIFICATION_TRANSFORM_CHANGED: {
 			Transform3D new_xform = get_global_transform();
 			if (new_xform == last_transform) {
@@ -1159,12 +1212,136 @@ RID GridMap::get_bake_mesh_instance(int p_idx) {
 
 GridMap::GridMap() {
 	set_notify_transform(true);
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton_mut()->connect("map_changed", callable_mp(this, &GridMap::_navigation_map_changed));
+	NavigationServer3D::get_singleton_mut()->connect("navigation_debug_changed", callable_mp(this, &GridMap::_update_navigation_debug_edge_connections));
+#endif // DEBUG_ENABLED
+}
+
+#ifdef DEBUG_ENABLED
+void GridMap::_update_navigation_debug_edge_connections() {
+	if (bake_navigation) {
+		for (const KeyValue<OctantKey, Octant *> &E : octant_map) {
+			_update_octant_navigation_debug_edge_connections_mesh(E.key);
+		}
+	}
 }
 
+void GridMap::_navigation_map_changed(RID p_map) {
+	if (bake_navigation && is_inside_tree() && p_map == get_world_3d()->get_navigation_map()) {
+		_update_navigation_debug_edge_connections();
+	}
+}
+#endif // DEBUG_ENABLED
+
 GridMap::~GridMap() {
 	if (!mesh_library.is_null()) {
 		mesh_library->unregister_owner(this);
 	}
 
 	clear();
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton_mut()->disconnect("map_changed", callable_mp(this, &GridMap::_navigation_map_changed));
+	NavigationServer3D::get_singleton_mut()->disconnect("navigation_debug_changed", callable_mp(this, &GridMap::_update_navigation_debug_edge_connections));
+#endif // DEBUG_ENABLED
+}
+
+#ifdef DEBUG_ENABLED
+void GridMap::_update_octant_navigation_debug_edge_connections_mesh(const OctantKey &p_key) {
+	ERR_FAIL_COND(!octant_map.has(p_key));
+	Octant &g = *octant_map[p_key];
+
+	if (!NavigationServer3D::get_singleton()->get_debug_enabled()) {
+		if (g.navigation_debug_edge_connections_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false);
+		}
+		return;
+	}
+
+	if (!is_inside_tree()) {
+		return;
+	}
+
+	if (!bake_navigation) {
+		if (g.navigation_debug_edge_connections_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false);
+		}
+		return;
+	}
+
+	if (!g.navigation_debug_edge_connections_instance.is_valid()) {
+		g.navigation_debug_edge_connections_instance = RenderingServer::get_singleton()->instance_create();
+	}
+
+	if (!g.navigation_debug_edge_connections_mesh.is_valid()) {
+		g.navigation_debug_edge_connections_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	}
+
+	g.navigation_debug_edge_connections_mesh->clear_surfaces();
+
+	float edge_connection_margin = NavigationServer3D::get_singleton()->map_get_edge_connection_margin(get_world_3d()->get_navigation_map());
+	float half_edge_connection_margin = edge_connection_margin * 0.5;
+
+	Vector<Vector3> vertex_array;
+
+	for (KeyValue<IndexKey, Octant::NavMesh> &F : g.navmesh_ids) {
+		if (cell_map.has(F.key) && F.value.region.is_valid()) {
+			int connections_count = NavigationServer3D::get_singleton()->region_get_connections_count(F.value.region);
+			if (connections_count == 0) {
+				continue;
+			}
+
+			for (int i = 0; i < connections_count; i++) {
+				Vector3 connection_pathway_start = NavigationServer3D::get_singleton()->region_get_connection_pathway_start(F.value.region, i);
+				Vector3 connection_pathway_end = NavigationServer3D::get_singleton()->region_get_connection_pathway_end(F.value.region, i);
+
+				Vector3 direction_start_end = connection_pathway_start.direction_to(connection_pathway_end);
+				Vector3 direction_end_start = connection_pathway_end.direction_to(connection_pathway_start);
+
+				Vector3 start_right_dir = direction_start_end.cross(Vector3(0, 1, 0));
+				Vector3 start_left_dir = -start_right_dir;
+
+				Vector3 end_right_dir = direction_end_start.cross(Vector3(0, 1, 0));
+				Vector3 end_left_dir = -end_right_dir;
+
+				Vector3 left_start_pos = connection_pathway_start + (start_left_dir * half_edge_connection_margin);
+				Vector3 right_start_pos = connection_pathway_start + (start_right_dir * half_edge_connection_margin);
+				Vector3 left_end_pos = connection_pathway_end + (end_right_dir * half_edge_connection_margin);
+				Vector3 right_end_pos = connection_pathway_end + (end_left_dir * half_edge_connection_margin);
+
+				vertex_array.push_back(right_end_pos);
+				vertex_array.push_back(left_start_pos);
+				vertex_array.push_back(right_start_pos);
+
+				vertex_array.push_back(left_end_pos);
+				vertex_array.push_back(right_end_pos);
+				vertex_array.push_back(right_start_pos);
+			}
+		}
+	}
+
+	if (vertex_array.size() == 0) {
+		return;
+	}
+
+	Ref<StandardMaterial3D> edge_connections_material = NavigationServer3D::get_singleton_mut()->get_debug_navigation_edge_connections_material();
+
+	Array mesh_array;
+	mesh_array.resize(Mesh::ARRAY_MAX);
+	mesh_array[Mesh::ARRAY_VERTEX] = vertex_array;
+
+	g.navigation_debug_edge_connections_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, mesh_array);
+	g.navigation_debug_edge_connections_mesh->surface_set_material(0, edge_connections_material);
+
+	RS::get_singleton()->instance_set_base(g.navigation_debug_edge_connections_instance, g.navigation_debug_edge_connections_mesh->get_rid());
+	RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, is_visible_in_tree());
+	if (is_inside_tree()) {
+		RS::get_singleton()->instance_set_scenario(g.navigation_debug_edge_connections_instance, get_world_3d()->get_scenario());
+	}
+
+	bool enable_edge_connections = NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_connections();
+	if (!enable_edge_connections) {
+		RS::get_singleton()->instance_set_visible(g.navigation_debug_edge_connections_instance, false);
+	}
 }
+#endif // DEBUG_ENABLED
diff --git a/modules/gridmap/grid_map.h b/modules/gridmap/grid_map.h
index 078a1d9de5..e9934108d9 100644
--- a/modules/gridmap/grid_map.h
+++ b/modules/gridmap/grid_map.h
@@ -117,6 +117,10 @@ class GridMap : public Node3D {
 		HashSet<IndexKey> cells;
 		RID collision_debug;
 		RID collision_debug_instance;
+#ifdef DEBUG_ENABLED
+		RID navigation_debug_edge_connections_instance;
+		Ref<ArrayMesh> navigation_debug_edge_connections_mesh;
+#endif // DEBUG_ENABLED
 
 		bool dirty = false;
 		RID static_body;
@@ -186,6 +190,11 @@ class GridMap : public Node3D {
 	bool _octant_update(const OctantKey &p_key);
 	void _octant_clean_up(const OctantKey &p_key);
 	void _octant_transform(const OctantKey &p_key);
+#ifdef DEBUG_ENABLED
+	void _update_octant_navigation_debug_edge_connections_mesh(const OctantKey &p_key);
+	void _navigation_map_changed(RID p_map);
+	void _update_navigation_debug_edge_connections();
+#endif // DEBUG_ENABLED
 	bool awaiting_update = false;
 
 	void _queue_octants_dirty();