From 8dd0579bcd9ea6a0ba0cc3f5bd7259bbe6d6487b Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Thu, 2 Jan 2025 01:04:03 +0100
Subject: [PATCH] Add NavigationLink debug direction indicator

Adds direction indicator arrows to the NavigationLink debug depending on if a link is onewyay or bidirectional.
---
 .../navigation_link_3d_gizmo_plugin.cpp       | 28 +++++++++++++++
 scene/2d/navigation_link_2d.cpp               | 27 +++++++++++++++
 scene/3d/navigation_link_3d.cpp               | 34 +++++++++++++++++++
 3 files changed, 89 insertions(+)

diff --git a/editor/plugins/gizmos/navigation_link_3d_gizmo_plugin.cpp b/editor/plugins/gizmos/navigation_link_3d_gizmo_plugin.cpp
index 51d15a0a70f..e1a4db77c36 100644
--- a/editor/plugins/gizmos/navigation_link_3d_gizmo_plugin.cpp
+++ b/editor/plugins/gizmos/navigation_link_3d_gizmo_plugin.cpp
@@ -125,6 +125,34 @@ void NavigationLink3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
 		}
 	}
 
+	const Vector3 link_segment = end_position - start_position;
+	const Vector3 up = Vector3(0.0, 1.0, 0.0);
+	const float arror_len = 0.5;
+
+	{
+		Vector3 anchor = start_position + (link_segment * 0.75);
+		Vector3 direction = start_position.direction_to(end_position);
+		Vector3 arrow_dir = direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+
+		arrow_dir = -direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+	}
+
+	if (link->is_bidirectional()) {
+		Vector3 anchor = start_position + (link_segment * 0.25);
+		Vector3 direction = end_position.direction_to(start_position);
+		Vector3 arrow_dir = direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+
+		arrow_dir = -direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+	}
+
 	p_gizmo->add_lines(lines, link->is_enabled() ? link_material : link_material_disabled);
 	p_gizmo->add_collision_segments(lines);
 
diff --git a/scene/2d/navigation_link_2d.cpp b/scene/2d/navigation_link_2d.cpp
index 1db88abe684..790b9f3e232 100644
--- a/scene/2d/navigation_link_2d.cpp
+++ b/scene/2d/navigation_link_2d.cpp
@@ -198,6 +198,10 @@ void NavigationLink2D::set_bidirectional(bool p_bidirectional) {
 	bidirectional = p_bidirectional;
 
 	NavigationServer2D::get_singleton()->link_set_bidirectional(link, bidirectional);
+
+#ifdef DEBUG_ENABLED
+	queue_redraw();
+#endif // DEBUG_ENABLED
 }
 
 void NavigationLink2D::set_navigation_layers(uint32_t p_navigation_layers) {
@@ -396,6 +400,29 @@ void NavigationLink2D::_update_debug_mesh() {
 	draw_line(get_start_position(), get_end_position(), color);
 	draw_arc(get_start_position(), radius, 0, Math_TAU, 10, color);
 	draw_arc(get_end_position(), radius, 0, Math_TAU, 10, color);
+
+	const Vector2 link_segment = end_position - start_position;
+	const float arror_len = 5.0;
+
+	{
+		Vector2 anchor = start_position + (link_segment * 0.75);
+		Vector2 direction = start_position.direction_to(end_position);
+		Vector2 arrow_dir = -direction.orthogonal();
+		draw_line(anchor, anchor + (arrow_dir - direction) * arror_len, color);
+
+		arrow_dir = direction.orthogonal();
+		draw_line(anchor, anchor + (arrow_dir - direction) * arror_len, color);
+	}
+
+	if (is_bidirectional()) {
+		Vector2 anchor = start_position + (link_segment * 0.25);
+		Vector2 direction = end_position.direction_to(start_position);
+		Vector2 arrow_dir = -direction.orthogonal();
+		draw_line(anchor, anchor + (arrow_dir - direction) * arror_len, color);
+
+		arrow_dir = direction.orthogonal();
+		draw_line(anchor, anchor + (arrow_dir - direction) * arror_len, color);
+	}
 }
 #endif // DEBUG_ENABLED
 
diff --git a/scene/3d/navigation_link_3d.cpp b/scene/3d/navigation_link_3d.cpp
index 40199ea3ca3..8f9b7a60b51 100644
--- a/scene/3d/navigation_link_3d.cpp
+++ b/scene/3d/navigation_link_3d.cpp
@@ -122,6 +122,34 @@ void NavigationLink3D::_update_debug_mesh() {
 		}
 	}
 
+	const Vector3 link_segment = end_position - start_position;
+	const Vector3 up = Vector3(0.0, 1.0, 0.0);
+	const float arror_len = 0.5;
+
+	{
+		Vector3 anchor = start_position + (link_segment * 0.75);
+		Vector3 direction = start_position.direction_to(end_position);
+		Vector3 arrow_dir = direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+
+		arrow_dir = -direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+	}
+
+	if (is_bidirectional()) {
+		Vector3 anchor = start_position + (link_segment * 0.25);
+		Vector3 direction = end_position.direction_to(start_position);
+		Vector3 arrow_dir = direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+
+		arrow_dir = -direction.cross(up);
+		lines.push_back(anchor);
+		lines.push_back(anchor + (arrow_dir - direction) * arror_len);
+	}
+
 	Array mesh_array;
 	mesh_array.resize(Mesh::ARRAY_MAX);
 	mesh_array[Mesh::ARRAY_VERTEX] = lines;
@@ -321,6 +349,12 @@ void NavigationLink3D::set_bidirectional(bool p_bidirectional) {
 	bidirectional = p_bidirectional;
 
 	NavigationServer3D::get_singleton()->link_set_bidirectional(link, bidirectional);
+
+#ifdef DEBUG_ENABLED
+	_update_debug_mesh();
+#endif // DEBUG_ENABLED
+
+	update_gizmos();
 }
 
 void NavigationLink3D::set_navigation_layers(uint32_t p_navigation_layers) {