From 23ea3ab8d1e31458e79905940d133eecb89537b7 Mon Sep 17 00:00:00 2001 From: smix8 <52464204+smix8@users.noreply.github.com> Date: Sat, 21 Dec 2024 19:44:00 +0100 Subject: [PATCH] Improve NavigationObstacle2D debug visuals Improves NavigationObstacle2D debug visuals by aligning them closer to their aleady improved 3D counterpart. --- .../navigation_obstacle_3d_editor_plugin.cpp | 14 +-- scene/2d/navigation_obstacle_2d.cpp | 101 +++++++++++------- scene/2d/navigation_obstacle_2d.h | 7 ++ scene/3d/navigation_obstacle_3d.cpp | 28 ++--- scene/3d/navigation_obstacle_3d.h | 5 + 5 files changed, 95 insertions(+), 60 deletions(-) diff --git a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp index ee385b6dcea..465a42c5597 100644 --- a/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp +++ b/editor/plugins/navigation_obstacle_3d_editor_plugin.cpp @@ -103,20 +103,12 @@ void NavigationObstacle3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) { lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(point.x, height, point.z)); } - Vector polygon_2d_vertices; - polygon_2d_vertices.resize(vertex_count); - for (int i = 0; i < vertex_count; i++) { - const Vector3 &vert = vertices[i]; - polygon_2d_vertices.write[i] = Vector2(vert.x, vert.z); - } - Vector triangulated_polygon_2d_indices = Geometry2D::triangulate_polygon(polygon_2d_vertices); - NavigationServer3D *ns3d = NavigationServer3D::get_singleton(); - if (triangulated_polygon_2d_indices.is_empty()) { - p_gizmo->add_lines(lines_mesh_vertices, ns3d->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material()); - } else { + if (obstacle->are_vertices_valid()) { p_gizmo->add_lines(lines_mesh_vertices, ns3d->get_debug_navigation_avoidance_static_obstacle_pushout_edge_material()); + } else { + p_gizmo->add_lines(lines_mesh_vertices, ns3d->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material()); } p_gizmo->add_collision_segments(lines_mesh_vertices); diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp index 46bcda9c6dd..a3967a573bb 100644 --- a/scene/2d/navigation_obstacle_2d.cpp +++ b/scene/2d/navigation_obstacle_2d.cpp @@ -187,6 +187,7 @@ NavigationObstacle2D::NavigationObstacle2D() { #ifdef DEBUG_ENABLED debug_canvas_item = RenderingServer::get_singleton()->canvas_item_create(); + debug_mesh_rid = RenderingServer::get_singleton()->mesh_create(); #endif // DEBUG_ENABLED } @@ -197,6 +198,10 @@ NavigationObstacle2D::~NavigationObstacle2D() { obstacle = RID(); #ifdef DEBUG_ENABLED + if (debug_mesh_rid.is_valid()) { + RenderingServer::get_singleton()->free(debug_mesh_rid); + debug_mesh_rid = RID(); + } if (debug_canvas_item.is_valid()) { RenderingServer::get_singleton()->free(debug_canvas_item); debug_canvas_item = RID(); @@ -206,6 +211,10 @@ NavigationObstacle2D::~NavigationObstacle2D() { void NavigationObstacle2D::set_vertices(const Vector &p_vertices) { vertices = p_vertices; + + vertices_are_clockwise = !Geometry2D::is_polygon_clockwise(vertices); // Geometry2D is inverted. + vertices_are_valid = !Geometry2D::triangulate_polygon(vertices).is_empty(); + const Transform2D node_transform = is_inside_tree() ? get_global_transform() : Transform2D(); NavigationServer2D::get_singleton()->obstacle_set_vertices(obstacle, node_transform.xform(vertices)); #ifdef DEBUG_ENABLED @@ -370,43 +379,61 @@ void NavigationObstacle2D::_update_fake_agent_radius_debug() { #ifdef DEBUG_ENABLED void NavigationObstacle2D::_update_static_obstacle_debug() { - if (get_vertices().size() > 2 && NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) { - bool obstacle_pushes_inward = Geometry2D::is_polygon_clockwise(get_vertices()); - - Color debug_static_obstacle_face_color; - - if (obstacle_pushes_inward) { - debug_static_obstacle_face_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_color(); - } else { - debug_static_obstacle_face_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_color(); - } - - Vector debug_obstacle_polygon_vertices = get_vertices(); - - Vector debug_obstacle_polygon_colors; - debug_obstacle_polygon_colors.resize(debug_obstacle_polygon_vertices.size()); - debug_obstacle_polygon_colors.fill(debug_static_obstacle_face_color); - - RS::get_singleton()->canvas_item_add_polygon(debug_canvas_item, get_global_transform().xform(debug_obstacle_polygon_vertices), debug_obstacle_polygon_colors); - - Color debug_static_obstacle_edge_color; - - if (obstacle_pushes_inward) { - debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_color(); - } else { - debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_color(); - } - - Vector debug_obstacle_line_vertices = get_vertices(); - debug_obstacle_line_vertices.push_back(debug_obstacle_line_vertices[0]); - debug_obstacle_line_vertices.resize(debug_obstacle_line_vertices.size()); - - Vector debug_obstacle_line_colors; - debug_obstacle_line_colors.resize(debug_obstacle_line_vertices.size()); - debug_obstacle_line_colors.fill(debug_static_obstacle_edge_color); - - // Transforming the vertices directly instead of the canvas item in order to not affect the circle shape by non-uniform scales. - RS::get_singleton()->canvas_item_add_polyline(debug_canvas_item, get_global_transform().xform(debug_obstacle_line_vertices), debug_obstacle_line_colors, 4.0); + if (get_vertices().size() < 3) { + return; } + + if (!NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) { + return; + } + + RenderingServer *rs = RenderingServer::get_singleton(); + + rs->mesh_clear(debug_mesh_rid); + + const int vertex_count = vertices.size(); + + Vector edge_vertex_array; + edge_vertex_array.resize(vertex_count * 4); + + Vector2 *edge_vertex_array_ptrw = edge_vertex_array.ptrw(); + + int vertex_index = 0; + + for (int i = 0; i < vertex_count; i++) { + Vector2 point = vertices[i]; + Vector2 next_point = vertices[(i + 1) % vertex_count]; + + Vector2 direction = next_point.direction_to(point); + Vector2 arrow_dir = -direction.orthogonal(); + Vector2 edge_middle = point + ((next_point - point) * 0.5); + + edge_vertex_array_ptrw[vertex_index++] = edge_middle; + edge_vertex_array_ptrw[vertex_index++] = edge_middle + (arrow_dir * 10.0); + + edge_vertex_array_ptrw[vertex_index++] = point; + edge_vertex_array_ptrw[vertex_index++] = next_point; + } + + Color debug_static_obstacle_edge_color; + + if (are_vertices_valid()) { + debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_color(); + } else { + debug_static_obstacle_edge_color = NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_color(); + } + + Vector line_color_array; + line_color_array.resize(edge_vertex_array.size()); + line_color_array.fill(debug_static_obstacle_edge_color); + + Array edge_mesh_array; + edge_mesh_array.resize(Mesh::ARRAY_MAX); + edge_mesh_array[Mesh::ARRAY_VERTEX] = edge_vertex_array; + edge_mesh_array[Mesh::ARRAY_COLOR] = line_color_array; + + rs->mesh_add_surface_from_arrays(debug_mesh_rid, RS::PRIMITIVE_LINES, edge_mesh_array, Array(), Dictionary(), RS::ARRAY_FLAG_USE_2D_VERTICES); + + rs->canvas_item_add_mesh(debug_canvas_item, debug_mesh_rid, get_global_transform()); } #endif // DEBUG_ENABLED diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h index 6e3596acf43..1acead6a64a 100644 --- a/scene/2d/navigation_obstacle_2d.h +++ b/scene/2d/navigation_obstacle_2d.h @@ -44,6 +44,8 @@ class NavigationObstacle2D : public Node2D { real_t radius = 0.0; Vector vertices; + bool vertices_are_clockwise = true; + bool vertices_are_valid = true; bool avoidance_enabled = true; uint32_t avoidance_layers = 1; @@ -60,6 +62,8 @@ class NavigationObstacle2D : public Node2D { #ifdef DEBUG_ENABLED private: RID debug_canvas_item; + RID debug_mesh_rid; + void _update_fake_agent_radius_debug(); void _update_static_obstacle_debug(); #endif // DEBUG_ENABLED @@ -86,6 +90,9 @@ public: void set_vertices(const Vector &p_vertices); const Vector &get_vertices() const { return vertices; } + bool are_vertices_clockwise() const { return vertices_are_clockwise; } + bool are_vertices_valid() const { return vertices_are_valid; } + void set_avoidance_layers(uint32_t p_layers); uint32_t get_avoidance_layers() const; diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp index 7be01589f46..47e27c39772 100644 --- a/scene/3d/navigation_obstacle_3d.cpp +++ b/scene/3d/navigation_obstacle_3d.cpp @@ -258,6 +258,19 @@ NavigationObstacle3D::~NavigationObstacle3D() { void NavigationObstacle3D::set_vertices(const Vector &p_vertices) { vertices = p_vertices; + Vector vertices_2d; + vertices_2d.resize(vertices.size()); + + const Vector3 *vertices_ptr = vertices.ptr(); + Vector2 *vertices_2d_ptrw = vertices_2d.ptrw(); + + for (int i = 0; i < vertices.size(); i++) { + vertices_2d_ptrw[i] = Vector2(vertices_ptr[i].x, vertices_ptr[i].z); + } + + vertices_are_clockwise = !Geometry2D::is_polygon_clockwise(vertices_2d); // Geometry2D is inverted. + vertices_are_valid = !Geometry2D::triangulate_polygon(vertices_2d).is_empty(); + const Basis basis = is_inside_tree() ? get_global_basis() : get_basis(); const float rotation_y = is_inside_tree() ? get_global_rotation().y : get_rotation().y; const Vector3 safe_scale = basis.get_scale().abs().maxf(0.001); @@ -604,21 +617,12 @@ void NavigationObstacle3D::_update_static_obstacle_debug() { rs->mesh_add_surface_from_arrays(static_obstacle_debug_mesh_rid, RS::PRIMITIVE_LINES, edge_mesh_array); - Vector polygon_2d_vertices; - polygon_2d_vertices.resize(vertex_count); - for (int i = 0; i < vertex_count; i++) { - const Vector3 &vert = vertices[i]; - polygon_2d_vertices.write[i] = Vector2(vert.x, vert.z); - } - - Vector triangulated_polygon_2d_indices = Geometry2D::triangulate_polygon(polygon_2d_vertices); - Ref edge_material; - if (triangulated_polygon_2d_indices.is_empty()) { - edge_material = ns3d->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material(); - } else { + if (are_vertices_valid()) { edge_material = ns3d->get_debug_navigation_avoidance_static_obstacle_pushout_edge_material(); + } else { + edge_material = ns3d->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material(); } rs->instance_set_surface_override_material(static_obstacle_debug_instance_rid, 0, edge_material->get_rid()); diff --git a/scene/3d/navigation_obstacle_3d.h b/scene/3d/navigation_obstacle_3d.h index 25f376e68b6..f18f9356004 100644 --- a/scene/3d/navigation_obstacle_3d.h +++ b/scene/3d/navigation_obstacle_3d.h @@ -45,6 +45,8 @@ class NavigationObstacle3D : public Node3D { real_t radius = 0.0; Vector vertices; + bool vertices_are_clockwise = true; + bool vertices_are_valid = true; bool avoidance_enabled = true; uint32_t avoidance_layers = 1; @@ -96,6 +98,9 @@ public: void set_vertices(const Vector &p_vertices); const Vector &get_vertices() const { return vertices; } + bool are_vertices_clockwise() const { return vertices_are_clockwise; } + bool are_vertices_valid() const { return vertices_are_valid; } + void set_avoidance_layers(uint32_t p_layers); uint32_t get_avoidance_layers() const;