1 files changed, 83 insertions, 68 deletions

diff --git a/scene/resources/navigation_mesh.cpp b/scene/resources/navigation_mesh.cpp
index a808ead66b..90ea879012 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 // DEBUG_ENABLED
+
 void NavigationMesh::create_from_mesh(const Ref<Mesh> &p_mesh) {
 	ERR_FAIL_COND(p_mesh.is_null());
 
@@ -337,89 +341,98 @@ void NavigationMesh::clear_polygons() {
 	polygons.clear();
 }
 
-Ref<Mesh> NavigationMesh::get_debug_mesh() {
+#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;
 	}
 
-	Vector<Vector3> vertices = get_vertices();
-	const Vector3 *vr = vertices.ptr();
-	List<Face3> faces;
-	for (int i = 0; i < get_polygon_count(); i++) {
-		Vector<int> p = get_polygon(i);
-
-		for (int j = 2; j < p.size(); j++) {
-			Face3 f;
-			f.vertex[0] = vr[p[0]];
-			f.vertex[1] = vr[p[j - 1]];
-			f.vertex[2] = vr[p[j]];
+	if (!debug_mesh.is_valid()) {
+		debug_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	} else {
+		debug_mesh->clear_surfaces();
+	}
 
-			faces.push_back(f);
-		}
+	if (vertices.size() == 0) {
+		return debug_mesh;
 	}
 
-	HashMap<_EdgeKey, bool, _EdgeKey> edge_map;
-	Vector<Vector3> tmeshfaces;
-	tmeshfaces.resize(faces.size() * 3);
-
-	{
-		Vector3 *tw = tmeshfaces.ptrw();
-		int tidx = 0;
-
-		for (const Face3 &f : faces) {
-			for (int j = 0; j < 3; j++) {
-				tw[tidx++] = f.vertex[j];
-				_EdgeKey ek;
-				ek.from = f.vertex[j].snapped(Vector3(CMP_EPSILON, CMP_EPSILON, CMP_EPSILON));
-				ek.to = f.vertex[(j + 1) % 3].snapped(Vector3(CMP_EPSILON, CMP_EPSILON, CMP_EPSILON));
-				if (ek.from < ek.to) {
-					SWAP(ek.from, ek.to);
-				}
-
-				HashMap<_EdgeKey, bool, _EdgeKey>::Iterator F = edge_map.find(ek);
-
-				if (F) {
-					F->value = false;
-
-				} else {
-					edge_map[ek] = true;
-				}
-			}
-		}
+	int polygon_count = get_polygon_count();
+
+	if (polygon_count < 1) {
+		// no face, no play
+		return debug_mesh;
 	}
-	List<Vector3> lines;
 
-	for (const KeyValue<_EdgeKey, bool> &E : edge_map) {
-		if (E.value) {
-			lines.push_back(E.key.from);
-			lines.push_back(E.key.to);
-		}
+	// 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]]);
 	}
 
-	Vector<Vector3> varr;
-	varr.resize(lines.size());
-	{
-		Vector3 *w = varr.ptrw();
-		int idx = 0;
-		for (const Vector3 &E : lines) {
-			w[idx++] = E;
+	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()));
+
+			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 = Ref<ArrayMesh>(memnew(ArrayMesh));
+	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(0, debug_geometry_face_material);
 
-	if (!lines.size()) {
-		return debug_mesh;
-	}
+	// 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);
 
-	Array arr;
-	arr.resize(Mesh::ARRAY_MAX);
-	arr[Mesh::ARRAY_VERTEX] = varr;
+		for (int i = 0; i < polygon_count; i++) {
+			Vector<int> polygon = get_polygon(i);
 
-	debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_LINES, arr);
+			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(1, debug_geometry_edge_material);
+	}
 
 	return debug_mesh;
 }
+#endif // DEBUG_ENABLED
 
 void NavigationMesh::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_sample_partition_type", "sample_partition_type"), &NavigationMesh::set_sample_partition_type);
@@ -513,8 +526,10 @@ void NavigationMesh::_bind_methods() {
 	ADD_GROUP("Geometry", "geometry_");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "geometry_parsed_geometry_type", PROPERTY_HINT_ENUM, "Mesh Instances,Static Colliders,Both"), "set_parsed_geometry_type", "get_parsed_geometry_type");
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "geometry_collision_mask", PROPERTY_HINT_LAYERS_3D_PHYSICS), "set_collision_mask", "get_collision_mask");
+	ADD_PROPERTY_DEFAULT("geometry_collision_mask", 0xFFFFFFFF);
 	ADD_PROPERTY(PropertyInfo(Variant::INT, "geometry_source_geometry_mode", PROPERTY_HINT_ENUM, "NavMesh Children, Group With Children, Group Explicit"), "set_source_geometry_mode", "get_source_geometry_mode");
 	ADD_PROPERTY(PropertyInfo(Variant::STRING, "geometry_source_group_name"), "set_source_group_name", "get_source_group_name");
+	ADD_PROPERTY_DEFAULT("geometry_source_group_name", StringName("navmesh"));
 	ADD_GROUP("Cells", "cell_");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "cell_size", PROPERTY_HINT_RANGE, "0.01,500.0,0.01,or_greater,suffix:m"), "set_cell_size", "get_cell_size");
 	ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "cell_height", PROPERTY_HINT_RANGE, "0.01,500.0,0.01,or_greater,suffix:m"), "set_cell_height", "get_cell_height");
@@ -557,17 +572,17 @@ void NavigationMesh::_bind_methods() {
 	BIND_ENUM_CONSTANT(SOURCE_GEOMETRY_MAX);
 }
 
-void NavigationMesh::_validate_property(PropertyInfo &property) const {
-	if (property.name == "geometry/collision_mask") {
+void NavigationMesh::_validate_property(PropertyInfo &p_property) const {
+	if (p_property.name == "geometry_collision_mask") {
 		if (parsed_geometry_type == PARSED_GEOMETRY_MESH_INSTANCES) {
-			property.usage = PROPERTY_USAGE_NONE;
+			p_property.usage = PROPERTY_USAGE_NONE;
 			return;
 		}
 	}
 
-	if (property.name == "geometry/source_group_name") {
+	if (p_property.name == "geometry_source_group_name") {
 		if (source_geometry_mode == SOURCE_GEOMETRY_NAVMESH_CHILDREN) {
-			property.usage = PROPERTY_USAGE_NONE;
+			p_property.usage = PROPERTY_USAGE_NONE;
 			return;
 		}
 	}