From 11c84049fa12ff4bcbba53a1e9a4a3fdbffc50df Mon Sep 17 00:00:00 2001 From: Relintai Date: Fri, 9 Jun 2023 13:03:30 +0200 Subject: [PATCH] Ported from godot4: Fix NavigationMesh debug visuals for non-triangulated meshes Fixes NavigationMesh debug visuals for non-triangulated meshes. - smix8 https://github.com/godotengine/godot/commit/4490a3303bc8b234dcccdbfdafd3877c9a11cb4d --- scene/3d/navigation_mesh_instance.cpp | 83 +++++++++++++++++++-------- servers/navigation_server.cpp | 1 + 2 files changed, 59 insertions(+), 25 deletions(-) diff --git a/scene/3d/navigation_mesh_instance.cpp b/scene/3d/navigation_mesh_instance.cpp index 6b9bdc4e5..fc793a6fe 100644 --- a/scene/3d/navigation_mesh_instance.cpp +++ b/scene/3d/navigation_mesh_instance.cpp @@ -436,9 +436,6 @@ void NavigationMeshInstance::_update_debug_mesh() { debug_mesh->clear_surfaces(); - bool enabled_geometry_face_random_color = NavigationServer::get_singleton()->get_debug_navigation_enable_geometry_face_random_color(); - bool enabled_edge_lines = NavigationServer::get_singleton()->get_debug_navigation_enable_edge_lines(); - PoolVector vertices = navmesh->get_vertices(); if (vertices.size() == 0) { return; @@ -449,55 +446,89 @@ void NavigationMeshInstance::_update_debug_mesh() { return; } + bool enabled_geometry_face_random_color = NavigationServer::get_singleton()->get_debug_navigation_enable_geometry_face_random_color(); + bool enabled_edge_lines = NavigationServer::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 &polygon = navmesh->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 face_vertex_array; - face_vertex_array.resize(polygon_count * 3); + face_vertex_array.resize(vertex_count); Vector face_color_array; if (enabled_geometry_face_random_color) { - face_color_array.resize(polygon_count * 3); + face_color_array.resize(vertex_count); } Vector 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 = NavigationServer::get_singleton()->get_debug_navigation_geometry_face_color(); - Ref face_material = NavigationServer::get_singleton_mut()->get_debug_navigation_geometry_face_material(); - Ref line_material = NavigationServer::get_singleton_mut()->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 &polygon_indices = navmesh->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 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); + 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 face_material = NavigationServer::get_singleton_mut()->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; @@ -508,6 +539,8 @@ void NavigationMeshInstance::_update_debug_mesh() { debug_mesh->surface_set_material(0, face_material); if (enabled_edge_lines) { + Ref line_material = NavigationServer::get_singleton_mut()->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.cpp b/servers/navigation_server.cpp index fcc6f3b6c..30a39997b 100644 --- a/servers/navigation_server.cpp +++ b/servers/navigation_server.cpp @@ -229,6 +229,7 @@ Ref NavigationServer::get_debug_navigation_geometry_face_materi face_material->set_flag(SpatialMaterial::FLAG_UNSHADED, true); face_material->set_feature(SpatialMaterial::FEATURE_TRANSPARENT, true); face_material->set_albedo(debug_navigation_geometry_face_color); + face_material->set_cull_mode(SpatialMaterial::CULL_DISABLED); if (enabled_geometry_face_random_color) { face_material->set_flag(SpatialMaterial::FLAG_SRGB_VERTEX_COLOR, true); face_material->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);