Merge pull request #62601 from smix8/navigation_3d_debug_4.x

5 files changed, 451 insertions, 36 deletions

diff --git a/scene/2d/navigation_region_2d.cpp b/scene/2d/navigation_region_2d.cpp
index 6e8fd891cb..6f189a57e8 100644
--- a/scene/2d/navigation_region_2d.cpp
+++ b/scene/2d/navigation_region_2d.cpp
@@ -35,6 +35,7 @@
 #include "core/os/mutex.h"
 #include "scene/resources/world_2d.h"
 #include "servers/navigation_server_2d.h"
+#include "servers/navigation_server_3d.h"
 
 #include "thirdparty/misc/polypartition.h"
 
@@ -371,9 +372,11 @@ void NavigationRegion2D::set_enabled(bool p_enabled) {
 		NavigationServer2D::get_singleton_mut()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
 	}
 
-	if (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) {
+#ifdef DEBUG_ENABLED
+	if (Engine::get_singleton()->is_editor_hint() || NavigationServer3D::get_singleton()->get_debug_enabled()) {
 		update();
 	}
+#endif // DEBUG_ENABLED
 }
 
 bool NavigationRegion2D::is_enabled() const {
@@ -462,7 +465,8 @@ void NavigationRegion2D::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_DRAW: {
-			if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || get_tree()->is_debugging_navigation_hint()) && navpoly.is_valid()) {
+#ifdef DEBUG_ENABLED
+			if (is_inside_tree() && (Engine::get_singleton()->is_editor_hint() || NavigationServer3D::get_singleton()->get_debug_enabled()) && navpoly.is_valid()) {
 				Vector<Vector2> verts = navpoly->get_vertices();
 				if (verts.size() < 3) {
 					return;
@@ -470,11 +474,11 @@ void NavigationRegion2D::_notification(int p_what) {
 
 				Color color;
 				if (enabled) {
-					color = get_tree()->get_debug_navigation_color();
+					color = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color();
 				} else {
-					color = get_tree()->get_debug_navigation_disabled_color();
+					color = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_disabled_color();
 				}
-				Color doors_color = color.lightened(0.2);
+				Color doors_color = NavigationServer3D::get_singleton()->get_debug_navigation_edge_connection_color();
 
 				RandomPCG rand;
 
@@ -516,6 +520,7 @@ void NavigationRegion2D::_notification(int p_what) {
 					draw_arc(b, radius, angle - Math_PI / 2.0, angle + Math_PI / 2.0, 10, doors_color);
 				}
 			}
+#endif // DEBUG_ENABLED
 		} break;
 	}
 }
@@ -552,10 +557,13 @@ void NavigationRegion2D::_navpoly_changed() {
 		NavigationServer2D::get_singleton()->region_set_navpoly(region, navpoly);
 	}
 }
+
 void NavigationRegion2D::_map_changed(RID p_map) {
-	if (enabled && get_world_2d()->get_navigation_map() == p_map) {
+#ifdef DEBUG_ENABLED
+	if (is_inside_tree() && get_world_2d()->get_navigation_map() == p_map) {
 		update();
 	}
+#endif // DEBUG_ENABLED
 }
 
 TypedArray<String> NavigationRegion2D::get_configuration_warnings() const {
@@ -605,8 +613,18 @@ NavigationRegion2D::NavigationRegion2D() {
 	region = NavigationServer2D::get_singleton()->region_create();
 	NavigationServer2D::get_singleton()->region_set_enter_cost(region, get_enter_cost());
 	NavigationServer2D::get_singleton()->region_set_travel_cost(region, get_travel_cost());
+
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton_mut()->connect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
+	NavigationServer3D::get_singleton_mut()->connect("navigation_debug_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
+#endif // DEBUG_ENABLED
 }
 
 NavigationRegion2D::~NavigationRegion2D() {
 	NavigationServer2D::get_singleton()->free(region);
+
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton_mut()->disconnect("map_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
+	NavigationServer3D::get_singleton_mut()->disconnect("navigation_debug_changed", callable_mp(this, &NavigationRegion2D::_map_changed));
+#endif // DEBUG_ENABLED
 }
diff --git a/scene/3d/navigation_region_3d.cpp b/scene/3d/navigation_region_3d.cpp
index 2a8149c6f6..0abb0e8dea 100644
--- a/scene/3d/navigation_region_3d.cpp
+++ b/scene/3d/navigation_region_3d.cpp
@@ -49,14 +49,29 @@ void NavigationRegion3D::set_enabled(bool p_enabled) {
 		NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
 	}
 
-	if (debug_view) {
-		MeshInstance3D *dm = Object::cast_to<MeshInstance3D>(debug_view);
-		if (is_enabled()) {
-			dm->set_material_override(get_tree()->get_debug_navigation_material());
+#ifdef DEBUG_ENABLED
+	if (debug_instance.is_valid()) {
+		if (!is_enabled()) {
+			if (debug_mesh.is_valid()) {
+				if (debug_mesh->get_surface_count() > 0) {
+					RS::get_singleton()->instance_set_surface_override_material(debug_instance, 0, NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_face_disabled_material()->get_rid());
+				}
+				if (debug_mesh->get_surface_count() > 1) {
+					RS::get_singleton()->instance_set_surface_override_material(debug_instance, 1, NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_edge_disabled_material()->get_rid());
+				}
+			}
 		} else {
-			dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
+			if (debug_mesh.is_valid()) {
+				if (debug_mesh->get_surface_count() > 0) {
+					RS::get_singleton()->instance_set_surface_override_material(debug_instance, 0, RID());
+				}
+				if (debug_mesh->get_surface_count() > 1) {
+					RS::get_singleton()->instance_set_surface_override_material(debug_instance, 1, RID());
+				}
+			}
 		}
 	}
+#endif // DEBUG_ENABLED
 
 	update_gizmos();
 }
@@ -124,30 +139,36 @@ void NavigationRegion3D::_notification(int p_what) {
 				NavigationServer3D::get_singleton()->region_set_map(region, get_world_3d()->get_navigation_map());
 			}
 
-			if (navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
-				MeshInstance3D *dm = memnew(MeshInstance3D);
-				dm->set_mesh(navmesh->get_debug_mesh());
-				if (is_enabled()) {
-					dm->set_material_override(get_tree()->get_debug_navigation_material());
-				} else {
-					dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
-				}
-				add_child(dm);
-				debug_view = dm;
+#ifdef DEBUG_ENABLED
+			if (NavigationServer3D::get_singleton()->get_debug_enabled()) {
+				_update_debug_mesh();
 			}
+#endif // DEBUG_ENABLED
+
 		} break;
 
 		case NOTIFICATION_TRANSFORM_CHANGED: {
 			NavigationServer3D::get_singleton()->region_set_transform(region, get_global_transform());
+
+#ifdef DEBUG_ENABLED
+			if (is_inside_tree() && debug_instance.is_valid()) {
+				RS::get_singleton()->instance_set_transform(debug_instance, get_global_transform());
+			}
+#endif // DEBUG_ENABLED
+
 		} break;
 
 		case NOTIFICATION_EXIT_TREE: {
 			NavigationServer3D::get_singleton()->region_set_map(region, RID());
 
-			if (debug_view) {
-				debug_view->queue_delete();
-				debug_view = nullptr;
+#ifdef DEBUG_ENABLED
+			if (debug_instance.is_valid()) {
+				RS::get_singleton()->instance_set_visible(debug_instance, false);
+			}
+			if (debug_edge_connections_instance.is_valid()) {
+				RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, false);
 			}
+#endif // DEBUG_ENABLED
 		} break;
 	}
 }
@@ -169,20 +190,21 @@ void NavigationRegion3D::set_navigation_mesh(const Ref<NavigationMesh> &p_navmes
 
 	NavigationServer3D::get_singleton()->region_set_navmesh(region, p_navmesh);
 
-	if (debug_view == nullptr && is_inside_tree() && navmesh.is_valid() && get_tree()->is_debugging_navigation_hint()) {
-		MeshInstance3D *dm = memnew(MeshInstance3D);
-		dm->set_mesh(navmesh->get_debug_mesh());
-		if (is_enabled()) {
-			dm->set_material_override(get_tree()->get_debug_navigation_material());
+#ifdef DEBUG_ENABLED
+	if (is_inside_tree() && NavigationServer3D::get_singleton()->get_debug_enabled()) {
+		if (navmesh.is_valid()) {
+			_update_debug_mesh();
+			_update_debug_edge_connections_mesh();
 		} else {
-			dm->set_material_override(get_tree()->get_debug_navigation_disabled_material());
+			if (debug_instance.is_valid()) {
+				RS::get_singleton()->instance_set_visible(debug_instance, false);
+			}
+			if (debug_edge_connections_instance.is_valid()) {
+				RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, false);
+			}
 		}
-		add_child(dm);
-		debug_view = dm;
-	}
-	if (debug_view && navmesh.is_valid()) {
-		Object::cast_to<MeshInstance3D>(debug_view)->set_mesh(navmesh->get_debug_mesh());
 	}
+#endif // DEBUG_ENABLED
 
 	emit_signal(SNAME("navigation_mesh_changed"));
 
@@ -287,13 +309,31 @@ void NavigationRegion3D::_bind_methods() {
 void NavigationRegion3D::_navigation_changed() {
 	update_gizmos();
 	update_configuration_warnings();
+
+#ifdef DEBUG_ENABLED
+	_update_debug_edge_connections_mesh();
+#endif // DEBUG_ENABLED
+}
+
+#ifdef DEBUG_ENABLED
+void NavigationRegion3D::_navigation_map_changed(RID p_map) {
+	if (is_inside_tree() && p_map == get_world_3d()->get_navigation_map()) {
+		_update_debug_edge_connections_mesh();
+	}
 }
+#endif // DEBUG_ENABLED
 
 NavigationRegion3D::NavigationRegion3D() {
 	set_notify_transform(true);
 	region = NavigationServer3D::get_singleton()->region_create();
 	NavigationServer3D::get_singleton()->region_set_enter_cost(region, get_enter_cost());
 	NavigationServer3D::get_singleton()->region_set_travel_cost(region, get_travel_cost());
+
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton_mut()->connect("map_changed", callable_mp(this, &NavigationRegion3D::_navigation_map_changed));
+	NavigationServer3D::get_singleton_mut()->connect("navigation_debug_changed", callable_mp(this, &NavigationRegion3D::_update_debug_mesh));
+	NavigationServer3D::get_singleton_mut()->connect("navigation_debug_changed", callable_mp(this, &NavigationRegion3D::_update_debug_edge_connections_mesh));
+#endif // DEBUG_ENABLED
 }
 
 NavigationRegion3D::~NavigationRegion3D() {
@@ -301,4 +341,245 @@ NavigationRegion3D::~NavigationRegion3D() {
 		navmesh->disconnect("changed", callable_mp(this, &NavigationRegion3D::_navigation_changed));
 	}
 	NavigationServer3D::get_singleton()->free(region);
+
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton_mut()->disconnect("map_changed", callable_mp(this, &NavigationRegion3D::_navigation_map_changed));
+	NavigationServer3D::get_singleton_mut()->disconnect("navigation_debug_changed", callable_mp(this, &NavigationRegion3D::_update_debug_mesh));
+	NavigationServer3D::get_singleton_mut()->disconnect("navigation_debug_changed", callable_mp(this, &NavigationRegion3D::_update_debug_edge_connections_mesh));
+	if (debug_instance.is_valid()) {
+		RenderingServer::get_singleton()->free(debug_instance);
+	}
+	if (debug_mesh.is_valid()) {
+		RenderingServer::get_singleton()->free(debug_mesh->get_rid());
+	}
+	if (debug_edge_connections_instance.is_valid()) {
+		RenderingServer::get_singleton()->free(debug_edge_connections_instance);
+	}
+	if (debug_edge_connections_mesh.is_valid()) {
+		RenderingServer::get_singleton()->free(debug_edge_connections_mesh->get_rid());
+	}
+#endif // DEBUG_ENABLED
+}
+
+#ifdef DEBUG_ENABLED
+void NavigationRegion3D::_update_debug_mesh() {
+	if (!NavigationServer3D::get_singleton()->get_debug_enabled()) {
+		if (debug_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(debug_instance, false);
+		}
+		return;
+	}
+
+	if (!navmesh.is_valid()) {
+		if (debug_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(debug_instance, false);
+		}
+		return;
+	}
+
+	if (!debug_instance.is_valid()) {
+		debug_instance = RenderingServer::get_singleton()->instance_create();
+	}
+
+	if (!debug_mesh.is_valid()) {
+		debug_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	}
+
+	debug_mesh->clear_surfaces();
+
+	bool enabled_geometry_face_random_color = NavigationServer3D::get_singleton()->get_debug_navigation_enable_geometry_face_random_color();
+	bool enabled_edge_lines = NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_lines();
+
+	Vector<Vector3> vertices = navmesh->get_vertices();
+	if (vertices.size() == 0) {
+		return;
+	}
+
+	int polygon_count = navmesh->get_polygon_count();
+	if (polygon_count == 0) {
+		return;
+	}
+
+	Vector<Vector3> face_vertex_array;
+	face_vertex_array.resize(polygon_count * 3);
+
+	Vector<Color> face_color_array;
+	if (enabled_geometry_face_random_color) {
+		face_color_array.resize(polygon_count * 3);
+	}
+
+	Vector<Vector3> line_vertex_array;
+	if (enabled_edge_lines) {
+		line_vertex_array.resize(polygon_count * 6);
+	}
+
+	Color debug_navigation_geometry_face_color = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color();
+
+	Ref<StandardMaterial3D> face_material = NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_face_material();
+	Ref<StandardMaterial3D> line_material = NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_edge_material();
+
+	Color polygon_color = debug_navigation_geometry_face_color;
+
+	for (int i = 0; i < polygon_count; i++) {
+		if (enabled_geometry_face_random_color) {
+			polygon_color = debug_navigation_geometry_face_color * (Color(Math::randf(), Math::randf(), Math::randf()));
+		}
+
+		Vector<int> polygon = navmesh->get_polygon(i);
+
+		face_vertex_array.push_back(vertices[polygon[0]]);
+		face_vertex_array.push_back(vertices[polygon[1]]);
+		face_vertex_array.push_back(vertices[polygon[2]]);
+		if (enabled_geometry_face_random_color) {
+			face_color_array.push_back(polygon_color);
+			face_color_array.push_back(polygon_color);
+			face_color_array.push_back(polygon_color);
+		}
+
+		if (enabled_edge_lines) {
+			line_vertex_array.push_back(vertices[polygon[0]]);
+			line_vertex_array.push_back(vertices[polygon[1]]);
+			line_vertex_array.push_back(vertices[polygon[1]]);
+			line_vertex_array.push_back(vertices[polygon[2]]);
+			line_vertex_array.push_back(vertices[polygon[2]]);
+			line_vertex_array.push_back(vertices[polygon[0]]);
+		}
+	}
+
+	Array face_mesh_array;
+	face_mesh_array.resize(Mesh::ARRAY_MAX);
+	face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array;
+	if (enabled_geometry_face_random_color) {
+		face_mesh_array[Mesh::ARRAY_COLOR] = face_color_array;
+	}
+	debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array);
+	debug_mesh->surface_set_material(0, face_material);
+
+	if (enabled_edge_lines) {
+		Array line_mesh_array;
+		line_mesh_array.resize(Mesh::ARRAY_MAX);
+		line_mesh_array[Mesh::ARRAY_VERTEX] = line_vertex_array;
+		debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, line_mesh_array);
+		debug_mesh->surface_set_material(1, line_material);
+	}
+
+	RS::get_singleton()->instance_set_base(debug_instance, debug_mesh->get_rid());
+	if (is_inside_tree()) {
+		RS::get_singleton()->instance_set_scenario(debug_instance, get_world_3d()->get_scenario());
+		RS::get_singleton()->instance_set_visible(debug_instance, is_visible_in_tree());
+	}
+	if (!is_enabled()) {
+		if (debug_mesh.is_valid()) {
+			if (debug_mesh->get_surface_count() > 0) {
+				RS::get_singleton()->instance_set_surface_override_material(debug_instance, 0, NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_face_disabled_material()->get_rid());
+			}
+			if (debug_mesh->get_surface_count() > 1) {
+				RS::get_singleton()->instance_set_surface_override_material(debug_instance, 1, NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_edge_disabled_material()->get_rid());
+			}
+		}
+	} else {
+		if (debug_mesh.is_valid()) {
+			if (debug_mesh->get_surface_count() > 0) {
+				RS::get_singleton()->instance_set_surface_override_material(debug_instance, 0, RID());
+			}
+			if (debug_mesh->get_surface_count() > 1) {
+				RS::get_singleton()->instance_set_surface_override_material(debug_instance, 1, RID());
+			}
+		}
+	}
+}
+#endif // DEBUG_ENABLED
+
+#ifdef DEBUG_ENABLED
+void NavigationRegion3D::_update_debug_edge_connections_mesh() {
+	if (!NavigationServer3D::get_singleton()->get_debug_enabled()) {
+		if (debug_edge_connections_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, false);
+		}
+		return;
+	}
+
+	if (!is_inside_tree()) {
+		return;
+	}
+
+	if (!navmesh.is_valid()) {
+		if (debug_edge_connections_instance.is_valid()) {
+			RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, false);
+		}
+		return;
+	}
+
+	if (!debug_edge_connections_instance.is_valid()) {
+		debug_edge_connections_instance = RenderingServer::get_singleton()->instance_create();
+	}
+
+	if (!debug_edge_connections_mesh.is_valid()) {
+		debug_edge_connections_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	}
+
+	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;
+	int connections_count = NavigationServer3D::get_singleton()->region_get_connections_count(region);
+
+	if (connections_count == 0) {
+		RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, false);
+		return;
+	}
+
+	Vector<Vector3> vertex_array;
+
+	for (int i = 0; i < connections_count; i++) {
+		Vector3 connection_pathway_start = NavigationServer3D::get_singleton()->region_get_connection_pathway_start(region, i);
+		Vector3 connection_pathway_end = NavigationServer3D::get_singleton()->region_get_connection_pathway_end(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;
+
+	debug_edge_connections_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, mesh_array);
+	debug_edge_connections_mesh->surface_set_material(0, edge_connections_material);
+
+	RS::get_singleton()->instance_set_base(debug_edge_connections_instance, debug_edge_connections_mesh->get_rid());
+	RS::get_singleton()->instance_set_visible(debug_edge_connections_instance, is_visible_in_tree());
+	if (is_inside_tree()) {
+		RS::get_singleton()->instance_set_scenario(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(debug_edge_connections_instance, false);
+	}
 }
+#endif // DEBUG_ENABLED
diff --git a/scene/3d/navigation_region_3d.h b/scene/3d/navigation_region_3d.h
index b15e97de6a..ba326abb46 100644
--- a/scene/3d/navigation_region_3d.h
+++ b/scene/3d/navigation_region_3d.h
@@ -44,11 +44,22 @@ class NavigationRegion3D : public Node3D {
 	real_t enter_cost = 0.0;
 	real_t travel_cost = 1.0;
 
-	Node *debug_view = nullptr;
 	Thread bake_thread;
 
 	void _navigation_changed();
 
+#ifdef DEBUG_ENABLED
+	RID debug_instance;
+	RID debug_edge_connections_instance;
+	Ref<ArrayMesh> debug_mesh;
+	Ref<ArrayMesh> debug_edge_connections_mesh;
+
+private:
+	void _update_debug_mesh();
+	void _update_debug_edge_connections_mesh();
+	void _navigation_map_changed(RID p_map);
+#endif // DEBUG_ENABLED
+
 protected:
 	void _notification(int p_what);
 	static void _bind_methods();
diff --git a/scene/resources/navigation_mesh.cpp b/scene/resources/navigation_mesh.cpp
index a808ead66b..ac5493efdc 100644
--- a/scene/resources/navigation_mesh.cpp
+++ b/scene/resources/navigation_mesh.cpp
@@ -30,6 +30,10 @@
 
 #include "navigation_mesh.h"
 
+#ifdef DEBUG_ENABLED
+#include "servers/navigation_server_3d.h"
+#endif
+
 void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
 	ERR_FAIL_COND(p_mesh.is_null());
 
@@ -337,6 +341,7 @@ void NavigationMesh::clear_polygons() {
 	polygons.clear();
 }
 
+#ifndef DISABLE_DEPRECATED
 Ref<Mesh> NavigationMesh::get_debug_mesh() {
 	if (debug_mesh.is_valid()) {
 		return debug_mesh;
@@ -420,6 +425,102 @@ Ref<Mesh> NavigationMesh::get_debug_mesh() {
 
 	return debug_mesh;
 }
+#endif // DISABLE_DEPRECATED
+
+#ifdef DEBUG_ENABLED
+Ref<ArrayMesh> NavigationMesh::_get_debug_mesh() {
+	if (debug_mesh.is_valid()) {
+		// Blocks further updates for now, code below is intended for dynamic updates e.g. when settings change.
+		return debug_mesh;
+	}
+
+	if (!debug_mesh.is_valid()) {
+		debug_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	} else {
+		debug_mesh->clear_surfaces();
+	}
+
+	if (vertices.size() == 0) {
+		return debug_mesh;
+	}
+
+	int polygon_count = get_polygon_count();
+
+	if (polygon_count < 1) {
+		// no face, no play
+		return debug_mesh;
+	}
+
+	// build geometry face surface
+	Vector<Vector3> face_vertex_array;
+	face_vertex_array.resize(polygon_count * 3);
+
+	for (int i = 0; i < polygon_count; i++) {
+		Vector<int> polygon = get_polygon(i);
+
+		face_vertex_array.push_back(vertices[polygon[0]]);
+		face_vertex_array.push_back(vertices[polygon[1]]);
+		face_vertex_array.push_back(vertices[polygon[2]]);
+	}
+
+	Array face_mesh_array;
+	face_mesh_array.resize(Mesh::ARRAY_MAX);
+	face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array;
+
+	// if enabled add vertex colors to colorize each face individually
+	bool enabled_geometry_face_random_color = NavigationServer3D::get_singleton()->get_debug_navigation_enable_geometry_face_random_color();
+	if (enabled_geometry_face_random_color) {
+		Color debug_navigation_geometry_face_color = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color();
+		Color polygon_color = debug_navigation_geometry_face_color;
+
+		Vector<Color> face_color_array;
+		face_color_array.resize(polygon_count * 3);
+
+		for (int i = 0; i < polygon_count; i++) {
+			polygon_color = debug_navigation_geometry_face_color * (Color(Math::randf(), Math::randf(), Math::randf()));
+
+			Vector<int> polygon = get_polygon(i);
+
+			face_color_array.push_back(polygon_color);
+			face_color_array.push_back(polygon_color);
+			face_color_array.push_back(polygon_color);
+		}
+		face_mesh_array[Mesh::ARRAY_COLOR] = face_color_array;
+	}
+
+	debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array);
+	Ref<StandardMaterial3D> debug_geometry_face_material = NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_face_material();
+	debug_mesh->surface_set_material(debug_mesh->get_surface_count(), debug_geometry_face_material);
+
+	// if enabled build geometry edge line surface
+	bool enabled_edge_lines = NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_lines();
+
+	if (enabled_edge_lines) {
+		Vector<Vector3> line_vertex_array;
+		line_vertex_array.resize(polygon_count * 6);
+
+		for (int i = 0; i < polygon_count; i++) {
+			Vector<int> polygon = get_polygon(i);
+
+			line_vertex_array.push_back(vertices[polygon[0]]);
+			line_vertex_array.push_back(vertices[polygon[1]]);
+			line_vertex_array.push_back(vertices[polygon[1]]);
+			line_vertex_array.push_back(vertices[polygon[2]]);
+			line_vertex_array.push_back(vertices[polygon[2]]);
+			line_vertex_array.push_back(vertices[polygon[0]]);
+		}
+
+		Array line_mesh_array;
+		line_mesh_array.resize(Mesh::ARRAY_MAX);
+		line_mesh_array[Mesh::ARRAY_VERTEX] = line_vertex_array;
+		debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, line_mesh_array);
+		Ref<StandardMaterial3D> debug_geometry_edge_material = NavigationServer3D::get_singleton_mut()->get_debug_navigation_geometry_edge_material();
+		debug_mesh->surface_set_material(debug_mesh->get_surface_count(), debug_geometry_edge_material);
+	}
+
+	return debug_mesh;
+}
+#endif
 
 void NavigationMesh::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_sample_partition_type", "sample_partition_type"), &NavigationMesh::set_sample_partition_type);
diff --git a/scene/resources/navigation_mesh.h b/scene/resources/navigation_mesh.h
index 40b275c792..79d8962d24 100644
--- a/scene/resources/navigation_mesh.h
+++ b/scene/resources/navigation_mesh.h
@@ -204,7 +204,11 @@ public:
 	Vector<int> get_polygon(int p_idx);
 	void clear_polygons();
 
+#ifndef DISABLE_DEPRECATED
 	Ref<Mesh> get_debug_mesh();
+#endif // DISABLE_DEPRECATED
+
+	Ref<ArrayMesh> _get_debug_mesh();
 
 	NavigationMesh();
 };