Improve NavigationObstacle2D debug visuals

Improves NavigationObstacle2D debug visuals by aligning them closer to their aleady improved 3D counterpart.

Author: smix8

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

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<Vector2> &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());
+	if (get_vertices().size() < 3) {
+		return;
+	}
 
-		Color debug_static_obstacle_face_color;
+	if (!NavigationServer2D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static()) {
+		return;
+	}
 
-		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();
-		}
+	RenderingServer *rs = RenderingServer::get_singleton();
 
-		Vector<Vector2> debug_obstacle_polygon_vertices = get_vertices();
+	rs->mesh_clear(debug_mesh_rid);
 
-		Vector<Color> debug_obstacle_polygon_colors;
-		debug_obstacle_polygon_colors.resize(debug_obstacle_polygon_vertices.size());
-		debug_obstacle_polygon_colors.fill(debug_static_obstacle_face_color);
+	const int vertex_count = vertices.size();
 
-		RS::get_singleton()->canvas_item_add_polygon(debug_canvas_item, get_global_transform().xform(debug_obstacle_polygon_vertices), debug_obstacle_polygon_colors);
+	Vector<Vector2> edge_vertex_array;
+	edge_vertex_array.resize(vertex_count * 4);
 
-		Color debug_static_obstacle_edge_color;
+	Vector2 *edge_vertex_array_ptrw = edge_vertex_array.ptrw();
 
-		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();
-		}
+	int vertex_index = 0;
 
-		Vector<Vector2> 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());
+	for (int i = 0; i < vertex_count; i++) {
+		Vector2 point = vertices[i];
+		Vector2 next_point = vertices[(i + 1) % vertex_count];
 
-		Vector<Color> debug_obstacle_line_colors;
-		debug_obstacle_line_colors.resize(debug_obstacle_line_vertices.size());
-		debug_obstacle_line_colors.fill(debug_static_obstacle_edge_color);
+		Vector2 direction = next_point.direction_to(point);
+		Vector2 arrow_dir = -direction.orthogonal();
+		Vector2 edge_middle = point + ((next_point - point) * 0.5);
 
-		// 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);
+		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<Color> 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

scene/2d/navigation_obstacle_2d.h

@@ -44,6 +44,8 @@ class NavigationObstacle2D : public Node2D {
 	real_t radius = 0.0;
 
 	Vector<Vector2> 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 @@ class NavigationObstacle2D : public Node2D {
 	void set_vertices(const Vector<Vector2> &p_vertices);
 	const Vector<Vector2> &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;
 

scene/3d/navigation_obstacle_3d.cpp

@@ -258,6 +258,19 @@ NavigationObstacle3D::~NavigationObstacle3D() {
 void NavigationObstacle3D::set_vertices(const Vector<Vector3> &p_vertices) {
 	vertices = p_vertices;
 
+	Vector<Vector2> 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<Vector2> 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<int> triangulated_polygon_2d_indices = Geometry2D::triangulate_polygon(polygon_2d_vertices);
-
 	Ref<StandardMaterial3D> 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());

scene/3d/navigation_obstacle_3d.h

@@ -45,6 +45,8 @@ class NavigationObstacle3D : public Node3D {
 	real_t radius = 0.0;
 
 	Vector<Vector3> vertices;
+	bool vertices_are_clockwise = true;
+	bool vertices_are_valid = true;
 
 	bool avoidance_enabled = true;
 	uint32_t avoidance_layers = 1;
@@ -96,6 +98,9 @@ class NavigationObstacle3D : public Node3D {
 	void set_vertices(const Vector<Vector3> &p_vertices);
 	const Vector<Vector3> &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;