Merge pull request #71543 from smix8/navigation_agent_debug_4.x

Add NavigationAgent Path Debug Visualization

4 files changed, 171 insertions, 0 deletions

diff --git a/servers/navigation_server_2d.cpp b/servers/navigation_server_2d.cpp
index 2e4087c1de..85ba8ed431 100644
--- a/servers/navigation_server_2d.cpp
+++ b/servers/navigation_server_2d.cpp
@@ -207,6 +207,30 @@ void NavigationServer2D::set_debug_navigation_enable_edge_connections(const bool
 bool NavigationServer2D::get_debug_navigation_enable_edge_connections() const {
 	return NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_connections();
 }
+
+void NavigationServer2D::set_debug_navigation_agent_path_color(const Color &p_color) {
+	NavigationServer3D::get_singleton()->set_debug_navigation_agent_path_color(p_color);
+}
+
+Color NavigationServer2D::get_debug_navigation_agent_path_color() const {
+	return NavigationServer3D::get_singleton()->get_debug_navigation_agent_path_color();
+}
+
+void NavigationServer2D::set_debug_navigation_enable_agent_paths(const bool p_value) {
+	NavigationServer3D::get_singleton()->set_debug_navigation_enable_agent_paths(p_value);
+}
+
+bool NavigationServer2D::get_debug_navigation_enable_agent_paths() const {
+	return NavigationServer3D::get_singleton()->get_debug_navigation_enable_agent_paths();
+}
+
+void NavigationServer2D::set_debug_navigation_agent_path_point_size(float p_point_size) {
+	NavigationServer3D::get_singleton()->set_debug_navigation_agent_path_point_size(p_point_size);
+}
+
+float NavigationServer2D::get_debug_navigation_agent_path_point_size() const {
+	return NavigationServer3D::get_singleton()->get_debug_navigation_agent_path_point_size();
+}
 #endif // DEBUG_ENABLED
 
 void NavigationServer2D::_bind_methods() {
@@ -286,14 +310,26 @@ void NavigationServer2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("free_rid", "rid"), &NavigationServer2D::free);
 
 	ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map")));
+
+	ADD_SIGNAL(MethodInfo("navigation_debug_changed"));
 }
 
 NavigationServer2D::NavigationServer2D() {
 	singleton = this;
 	ERR_FAIL_COND_MSG(!NavigationServer3D::get_singleton(), "The Navigation3D singleton should be initialized before the 2D one.");
 	NavigationServer3D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationServer2D::_emit_map_changed));
+
+#ifdef DEBUG_ENABLED
+	NavigationServer3D::get_singleton()->connect(SNAME("navigation_debug_changed"), callable_mp(this, &NavigationServer2D::_emit_navigation_debug_changed_signal));
+#endif // DEBUG_ENABLED
 }
 
+#ifdef DEBUG_ENABLED
+void NavigationServer2D::_emit_navigation_debug_changed_signal() {
+	emit_signal(SNAME("navigation_debug_changed"));
+}
+#endif // DEBUG_ENABLED
+
 NavigationServer2D::~NavigationServer2D() {
 	singleton = nullptr;
 }
diff --git a/servers/navigation_server_2d.h b/servers/navigation_server_2d.h
index cd18476182..746389404b 100644
--- a/servers/navigation_server_2d.h
+++ b/servers/navigation_server_2d.h
@@ -254,6 +254,20 @@ public:
 
 	void set_debug_navigation_enable_edge_connections(const bool p_value);
 	bool get_debug_navigation_enable_edge_connections() const;
+
+	void set_debug_navigation_agent_path_color(const Color &p_color);
+	Color get_debug_navigation_agent_path_color() const;
+
+	void set_debug_navigation_enable_agent_paths(const bool p_value);
+	bool get_debug_navigation_enable_agent_paths() const;
+
+	void set_debug_navigation_agent_path_point_size(float p_point_size);
+	float get_debug_navigation_agent_path_point_size() const;
+#endif // DEBUG_ENABLED
+
+#ifdef DEBUG_ENABLED
+private:
+	void _emit_navigation_debug_changed_signal();
 #endif // DEBUG_ENABLED
 };
 
diff --git a/servers/navigation_server_3d.cpp b/servers/navigation_server_3d.cpp
index 8f2cff0e04..70897ae75c 100644
--- a/servers/navigation_server_3d.cpp
+++ b/servers/navigation_server_3d.cpp
@@ -159,6 +159,7 @@ NavigationServer3D::NavigationServer3D() {
 	debug_navigation_geometry_face_disabled_color = GLOBAL_DEF("debug/shapes/navigation/geometry_face_disabled_color", Color(0.5, 0.5, 0.5, 0.4));
 	debug_navigation_link_connection_color = GLOBAL_DEF("debug/shapes/navigation/link_connection_color", Color(1.0, 0.5, 1.0, 1.0));
 	debug_navigation_link_connection_disabled_color = GLOBAL_DEF("debug/shapes/navigation/link_connection_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
+	debug_navigation_agent_path_color = GLOBAL_DEF("debug/shapes/navigation/agent_path_color", Color(1.0, 0.0, 0.0, 1.0));
 
 	debug_navigation_enable_edge_connections = GLOBAL_DEF("debug/shapes/navigation/enable_edge_connections", true);
 	debug_navigation_enable_edge_connections_xray = GLOBAL_DEF("debug/shapes/navigation/enable_edge_connections_xray", true);
@@ -168,6 +169,11 @@ NavigationServer3D::NavigationServer3D() {
 	debug_navigation_enable_link_connections = GLOBAL_DEF("debug/shapes/navigation/enable_link_connections", true);
 	debug_navigation_enable_link_connections_xray = GLOBAL_DEF("debug/shapes/navigation/enable_link_connections_xray", true);
 
+	debug_navigation_enable_agent_paths = GLOBAL_DEF("debug/shapes/navigation/enable_agent_paths", true);
+	debug_navigation_enable_agent_paths_xray = GLOBAL_DEF("debug/shapes/navigation/enable_agent_paths_xray", true);
+
+	debug_navigation_agent_path_point_size = GLOBAL_DEF("debug/shapes/navigation/agent_path_point_size", 4.0);
+
 	if (Engine::get_singleton()->is_editor_hint()) {
 		// enable NavigationServer3D when in Editor or else navigation mesh edge connections are invisible
 		// on runtime tests SceneTree has "Visible Navigation" set and main iteration takes care of this
@@ -329,6 +335,42 @@ Ref<StandardMaterial3D> NavigationServer3D::get_debug_navigation_link_connection
 	return debug_navigation_link_connections_disabled_material;
 }
 
+Ref<StandardMaterial3D> NavigationServer3D::get_debug_navigation_agent_path_line_material() {
+	if (debug_navigation_agent_path_line_material.is_valid()) {
+		return debug_navigation_agent_path_line_material;
+	}
+
+	Ref<StandardMaterial3D> material = Ref<StandardMaterial3D>(memnew(StandardMaterial3D));
+	material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+	material->set_albedo(debug_navigation_agent_path_color);
+	if (debug_navigation_enable_agent_paths_xray) {
+		material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true);
+	}
+	material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MAX - 2);
+
+	debug_navigation_agent_path_line_material = material;
+	return debug_navigation_agent_path_line_material;
+}
+
+Ref<StandardMaterial3D> NavigationServer3D::get_debug_navigation_agent_path_point_material() {
+	if (debug_navigation_agent_path_point_material.is_valid()) {
+		return debug_navigation_agent_path_point_material;
+	}
+
+	Ref<StandardMaterial3D> material = Ref<StandardMaterial3D>(memnew(StandardMaterial3D));
+	material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+	material->set_albedo(debug_navigation_agent_path_color);
+	material->set_flag(StandardMaterial3D::FLAG_USE_POINT_SIZE, true);
+	material->set_point_size(debug_navigation_agent_path_point_size);
+	if (debug_navigation_enable_agent_paths_xray) {
+		material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true);
+	}
+	material->set_render_priority(StandardMaterial3D::RENDER_PRIORITY_MAX - 2);
+
+	debug_navigation_agent_path_point_material = material;
+	return debug_navigation_agent_path_point_material;
+}
+
 void NavigationServer3D::set_debug_navigation_edge_connection_color(const Color &p_color) {
 	debug_navigation_edge_connection_color = p_color;
 	if (debug_navigation_edge_connections_material.is_valid()) {
@@ -406,6 +448,31 @@ Color NavigationServer3D::get_debug_navigation_link_connection_disabled_color()
 	return debug_navigation_link_connection_disabled_color;
 }
 
+void NavigationServer3D::set_debug_navigation_agent_path_point_size(float p_point_size) {
+	debug_navigation_agent_path_point_size = MAX(0.1, p_point_size);
+	if (debug_navigation_agent_path_point_material.is_valid()) {
+		debug_navigation_agent_path_point_material->set_point_size(debug_navigation_agent_path_point_size);
+	}
+}
+
+float NavigationServer3D::get_debug_navigation_agent_path_point_size() const {
+	return debug_navigation_agent_path_point_size;
+}
+
+void NavigationServer3D::set_debug_navigation_agent_path_color(const Color &p_color) {
+	debug_navigation_agent_path_color = p_color;
+	if (debug_navigation_agent_path_line_material.is_valid()) {
+		debug_navigation_agent_path_line_material->set_albedo(debug_navigation_agent_path_color);
+	}
+	if (debug_navigation_agent_path_point_material.is_valid()) {
+		debug_navigation_agent_path_point_material->set_albedo(debug_navigation_agent_path_color);
+	}
+}
+
+Color NavigationServer3D::get_debug_navigation_agent_path_color() const {
+	return debug_navigation_agent_path_color;
+}
+
 void NavigationServer3D::set_debug_navigation_enable_edge_connections(const bool p_value) {
 	debug_navigation_enable_edge_connections = p_value;
 	debug_dirty = true;
@@ -494,6 +561,37 @@ void NavigationServer3D::set_debug_enabled(bool p_enabled) {
 bool NavigationServer3D::get_debug_enabled() const {
 	return debug_enabled;
 }
+
+void NavigationServer3D::set_debug_navigation_enable_agent_paths(const bool p_value) {
+	if (debug_navigation_enable_agent_paths != p_value) {
+		debug_dirty = true;
+	}
+
+	debug_navigation_enable_agent_paths = p_value;
+
+	if (debug_dirty) {
+		call_deferred("_emit_navigation_debug_changed_signal");
+	}
+}
+
+bool NavigationServer3D::get_debug_navigation_enable_agent_paths() const {
+	return debug_navigation_enable_agent_paths;
+}
+
+void NavigationServer3D::set_debug_navigation_enable_agent_paths_xray(const bool p_value) {
+	debug_navigation_enable_agent_paths_xray = p_value;
+	if (debug_navigation_agent_path_line_material.is_valid()) {
+		debug_navigation_agent_path_line_material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, debug_navigation_enable_agent_paths_xray);
+	}
+	if (debug_navigation_agent_path_point_material.is_valid()) {
+		debug_navigation_agent_path_point_material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, debug_navigation_enable_agent_paths_xray);
+	}
+}
+
+bool NavigationServer3D::get_debug_navigation_enable_agent_paths_xray() const {
+	return debug_navigation_enable_agent_paths_xray;
+}
+
 #endif // DEBUG_ENABLED
 
 ///////////////////////////////////////////////////////
diff --git a/servers/navigation_server_3d.h b/servers/navigation_server_3d.h
index 9c468d1b10..bc4bdf2a30 100644
--- a/servers/navigation_server_3d.h
+++ b/servers/navigation_server_3d.h
@@ -287,6 +287,9 @@ private:
 	Color debug_navigation_geometry_face_disabled_color = Color(0.5, 0.5, 0.5, 0.4);
 	Color debug_navigation_link_connection_color = Color(1.0, 0.5, 1.0, 1.0);
 	Color debug_navigation_link_connection_disabled_color = Color(0.5, 0.5, 0.5, 1.0);
+	Color debug_navigation_agent_path_color = Color(1.0, 0.0, 0.0, 1.0);
+
+	float debug_navigation_agent_path_point_size = 4.0;
 
 	bool debug_navigation_enable_edge_connections = true;
 	bool debug_navigation_enable_edge_connections_xray = true;
@@ -295,6 +298,8 @@ private:
 	bool debug_navigation_enable_geometry_face_random_color = true;
 	bool debug_navigation_enable_link_connections = true;
 	bool debug_navigation_enable_link_connections_xray = true;
+	bool debug_navigation_enable_agent_paths = true;
+	bool debug_navigation_enable_agent_paths_xray = true;
 
 	Ref<StandardMaterial3D> debug_navigation_geometry_edge_material;
 	Ref<StandardMaterial3D> debug_navigation_geometry_face_material;
@@ -304,6 +309,9 @@ private:
 	Ref<StandardMaterial3D> debug_navigation_link_connections_material;
 	Ref<StandardMaterial3D> debug_navigation_link_connections_disabled_material;
 
+	Ref<StandardMaterial3D> debug_navigation_agent_path_line_material;
+	Ref<StandardMaterial3D> debug_navigation_agent_path_point_material;
+
 public:
 	void set_debug_enabled(bool p_enabled);
 	bool get_debug_enabled() const;
@@ -329,6 +337,9 @@ public:
 	void set_debug_navigation_link_connection_disabled_color(const Color &p_color);
 	Color get_debug_navigation_link_connection_disabled_color() const;
 
+	void set_debug_navigation_agent_path_color(const Color &p_color);
+	Color get_debug_navigation_agent_path_color() const;
+
 	void set_debug_navigation_enable_edge_connections(const bool p_value);
 	bool get_debug_navigation_enable_edge_connections() const;
 
@@ -350,6 +361,15 @@ public:
 	void set_debug_navigation_enable_link_connections_xray(const bool p_value);
 	bool get_debug_navigation_enable_link_connections_xray() const;
 
+	void set_debug_navigation_enable_agent_paths(const bool p_value);
+	bool get_debug_navigation_enable_agent_paths() const;
+
+	void set_debug_navigation_enable_agent_paths_xray(const bool p_value);
+	bool get_debug_navigation_enable_agent_paths_xray() const;
+
+	void set_debug_navigation_agent_path_point_size(float p_point_size);
+	float get_debug_navigation_agent_path_point_size() const;
+
 	Ref<StandardMaterial3D> get_debug_navigation_geometry_face_material();
 	Ref<StandardMaterial3D> get_debug_navigation_geometry_edge_material();
 	Ref<StandardMaterial3D> get_debug_navigation_geometry_face_disabled_material();
@@ -357,6 +377,9 @@ public:
 	Ref<StandardMaterial3D> get_debug_navigation_edge_connections_material();
 	Ref<StandardMaterial3D> get_debug_navigation_link_connections_material();
 	Ref<StandardMaterial3D> get_debug_navigation_link_connections_disabled_material();
+
+	Ref<StandardMaterial3D> get_debug_navigation_agent_path_line_material();
+	Ref<StandardMaterial3D> get_debug_navigation_agent_path_point_material();
 #endif // DEBUG_ENABLED
 };