1 files changed, 101 insertions, 0 deletions

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