Merge pull request #76148 from smix8/navigation_debug_nontri_meshes_4.x

Fix NavigationMesh debug visuals for non-triangulated meshes

2 files changed, 59 insertions, 25 deletions

diff --git a/scene/3d/navigation_region_3d.cpp b/scene/3d/navigation_region_3d.cpp
index 22a6ec3517..6af8999b89 100644
--- a/scene/3d/navigation_region_3d.cpp
+++ b/scene/3d/navigation_region_3d.cpp
@@ -431,9 +431,6 @@ void NavigationRegion3D::_update_debug_mesh() {
 
 	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 = navigation_mesh->get_vertices();
 	if (vertices.size() == 0) {
 		return;
@@ -444,55 +441,89 @@ void NavigationRegion3D::_update_debug_mesh() {
 		return;
 	}
 
+	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();
+
+	int vertex_count = 0;
+	int line_count = 0;
+
+	for (int i = 0; i < polygon_count; i++) {
+		const Vector<int> &polygon = navigation_mesh->get_polygon(i);
+		int polygon_size = polygon.size();
+		if (polygon_size < 3) {
+			continue;
+		}
+		line_count += polygon_size * 2;
+		vertex_count += (polygon_size - 2) * 3;
+	}
+
 	Vector<Vector3> face_vertex_array;
-	face_vertex_array.resize(polygon_count * 3);
+	face_vertex_array.resize(vertex_count);
 
 	Vector<Color> face_color_array;
 	if (enabled_geometry_face_random_color) {
-		face_color_array.resize(polygon_count * 3);
+		face_color_array.resize(vertex_count);
 	}
 
 	Vector<Vector3> line_vertex_array;
 	if (enabled_edge_lines) {
-		line_vertex_array.resize(polygon_count * 6);
+		line_vertex_array.resize(line_count);
 	}
 
 	Color debug_navigation_geometry_face_color = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color();
 
-	Ref<StandardMaterial3D> face_material = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_material();
-	Ref<StandardMaterial3D> line_material = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_edge_material();
-
 	RandomPCG rand;
 	Color polygon_color = debug_navigation_geometry_face_color;
 
-	for (int i = 0; i < polygon_count; i++) {
+	int face_vertex_index = 0;
+	int line_vertex_index = 0;
+
+	Vector3 *face_vertex_array_ptrw = face_vertex_array.ptrw();
+	Color *face_color_array_ptrw = face_color_array.ptrw();
+	Vector3 *line_vertex_array_ptrw = line_vertex_array.ptrw();
+
+	for (int polygon_index = 0; polygon_index < polygon_count; polygon_index++) {
+		const Vector<int> &polygon_indices = navigation_mesh->get_polygon(polygon_index);
+		int polygon_indices_size = polygon_indices.size();
+		if (polygon_indices_size < 3) {
+			continue;
+		}
+
 		if (enabled_geometry_face_random_color) {
 			// Generate the polygon color, slightly randomly modified from the settings one.
 			polygon_color.set_hsv(debug_navigation_geometry_face_color.get_h() + rand.random(-1.0, 1.0) * 0.1, debug_navigation_geometry_face_color.get_s(), debug_navigation_geometry_face_color.get_v() + rand.random(-1.0, 1.0) * 0.2);
 			polygon_color.a = debug_navigation_geometry_face_color.a;
 		}
 
-		Vector<int> polygon = navigation_mesh->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);
+		for (int polygon_indices_index = 0; polygon_indices_index < polygon_indices_size - 2; polygon_indices_index++) {
+			face_vertex_array_ptrw[face_vertex_index] = vertices[polygon_indices[0]];
+			face_vertex_array_ptrw[face_vertex_index + 1] = vertices[polygon_indices[polygon_indices_index + 1]];
+			face_vertex_array_ptrw[face_vertex_index + 2] = vertices[polygon_indices[polygon_indices_index + 2]];
+			if (enabled_geometry_face_random_color) {
+				face_color_array_ptrw[face_vertex_index] = polygon_color;
+				face_color_array_ptrw[face_vertex_index + 1] = polygon_color;
+				face_color_array_ptrw[face_vertex_index + 2] = polygon_color;
+			}
+			face_vertex_index += 3;
 		}
 
 		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]]);
+			for (int polygon_indices_index = 0; polygon_indices_index < polygon_indices_size; polygon_indices_index++) {
+				line_vertex_array_ptrw[line_vertex_index] = vertices[polygon_indices[polygon_indices_index]];
+				line_vertex_index += 1;
+				if (polygon_indices_index + 1 == polygon_indices_size) {
+					line_vertex_array_ptrw[line_vertex_index] = vertices[polygon_indices[0]];
+					line_vertex_index += 1;
+				} else {
+					line_vertex_array_ptrw[line_vertex_index] = vertices[polygon_indices[polygon_indices_index + 1]];
+					line_vertex_index += 1;
+				}
+			}
 		}
 	}
 
+	Ref<StandardMaterial3D> face_material = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_material();
+
 	Array face_mesh_array;
 	face_mesh_array.resize(Mesh::ARRAY_MAX);
 	face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array;
@@ -503,6 +534,8 @@ void NavigationRegion3D::_update_debug_mesh() {
 	debug_mesh->surface_set_material(0, face_material);
 
 	if (enabled_edge_lines) {
+		Ref<StandardMaterial3D> line_material = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_edge_material();
+
 		Array line_mesh_array;
 		line_mesh_array.resize(Mesh::ARRAY_MAX);
 		line_mesh_array[Mesh::ARRAY_VERTEX] = line_vertex_array;
diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp
index f180ceaa6b..10f5e71c91 100644
--- a/servers/navigation_server_3d.cpp
+++ b/servers/navigation_server_3d.cpp
@@ -226,6 +226,7 @@ Ref<StandardMaterial3D> NavigationServer3D::get_debug_navigation_geometry_face_m
 	face_material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
 	face_material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
 	face_material->set_albedo(get_debug_navigation_geometry_face_color());
+	face_material->set_cull_mode(StandardMaterial3D::CULL_DISABLED);
 	if (enabled_geometry_face_random_color) {
 		face_material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
 		face_material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);