From 6b51ab66d83b7c85e16189eba280a00ab5ff9536 Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Tue, 17 May 2022 20:08:39 +0200
Subject: [PATCH] Make Navigation Agents and Obstacles respect parent process
 mode

Temporarily removes agent from navigation map when parent node cannot process due to SceneTree pause and process_mode property. Normal process_mode does not work as other agents would still avoid the paused agents because they were still active on the navigation map and the rvo world. Also fixes potential crash when region_get_map or agent_get_map is called while no map is set.
---
 .../navigation/godot_navigation_server.cpp    | 10 ++++++++--
 scene/2d/navigation_agent_2d.cpp              | 20 +++++++++++++++++++
 scene/2d/navigation_agent_2d.h                |  1 +
 scene/2d/navigation_obstacle_2d.cpp           | 20 +++++++++++++++++++
 scene/2d/navigation_obstacle_2d.h             |  1 +
 scene/3d/navigation_agent_3d.cpp              | 20 +++++++++++++++++++
 scene/3d/navigation_agent_3d.h                |  1 +
 scene/3d/navigation_obstacle_3d.cpp           | 20 +++++++++++++++++++
 scene/3d/navigation_obstacle_3d.h             |  1 +
 9 files changed, 92 insertions(+), 2 deletions(-)

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index 42aad77979c..cc9d05da479 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -256,13 +256,19 @@ Array GodotNavigationServer::map_get_agents(RID p_map) const {
 RID GodotNavigationServer::region_get_map(RID p_region) const {
 	NavRegion *region = region_owner.get_or_null(p_region);
 	ERR_FAIL_COND_V(region == nullptr, RID());
-	return region->get_map()->get_self();
+	if (region->get_map()) {
+		return region->get_map()->get_self();
+	}
+	return RID();
 }
 
 RID GodotNavigationServer::agent_get_map(RID p_agent) const {
 	RvoAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND_V(agent == nullptr, RID());
-	return agent->get_map()->get_self();
+	if (agent->get_map()) {
+		return agent->get_map()->get_self();
+	}
+	return RID();
 }
 
 RID GodotNavigationServer::region_create() const {
diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
index 91549d75f09..59a7dc819f9 100644
--- a/scene/2d/navigation_agent_2d.cpp
+++ b/scene/2d/navigation_agent_2d.cpp
@@ -102,6 +102,26 @@ void NavigationAgent2D::_notification(int p_what) {
 			set_physics_process_internal(true);
 		} break;
 
+		case NOTIFICATION_PAUSED: {
+			if (agent_parent && !agent_parent->can_process()) {
+				map_before_pause = NavigationServer2D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (agent_parent && agent_parent->can_process() && !(map_before_pause == RID())) {
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
+		case NOTIFICATION_UNPAUSED: {
+			if (agent_parent && !agent_parent->can_process()) {
+				map_before_pause = NavigationServer2D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (agent_parent && agent_parent->can_process() && !(map_before_pause == RID())) {
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
 		case NOTIFICATION_EXIT_TREE: {
 			agent_parent = nullptr;
 			set_physics_process_internal(false);
diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h
index 1447e25e8cb..2a401190d09 100644
--- a/scene/2d/navigation_agent_2d.h
+++ b/scene/2d/navigation_agent_2d.h
@@ -41,6 +41,7 @@ class NavigationAgent2D : public Node {
 	Node2D *agent_parent = nullptr;
 
 	RID agent;
+	RID map_before_pause;
 
 	uint32_t navigable_layers = 1;
 
diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
index b594aa3bb2f..0320c6c9175 100644
--- a/scene/2d/navigation_obstacle_2d.cpp
+++ b/scene/2d/navigation_obstacle_2d.cpp
@@ -81,6 +81,26 @@ void NavigationObstacle2D::_notification(int p_what) {
 			parent_node2d = nullptr;
 		} break;
 
+		case NOTIFICATION_PAUSED: {
+			if (parent_node2d && !parent_node2d->can_process()) {
+				map_before_pause = NavigationServer2D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (parent_node2d && parent_node2d->can_process() && !(map_before_pause == RID())) {
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
+		case NOTIFICATION_UNPAUSED: {
+			if (parent_node2d && !parent_node2d->can_process()) {
+				map_before_pause = NavigationServer2D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (parent_node2d && parent_node2d->can_process() && !(map_before_pause == RID())) {
+				NavigationServer2D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 			if (parent_node2d && parent_node2d->is_inside_tree()) {
 				NavigationServer2D::get_singleton()->agent_set_position(agent, parent_node2d->get_global_position());
diff --git a/scene/2d/navigation_obstacle_2d.h b/scene/2d/navigation_obstacle_2d.h
index 2a0ef14e732..948cf5b61ad 100644
--- a/scene/2d/navigation_obstacle_2d.h
+++ b/scene/2d/navigation_obstacle_2d.h
@@ -39,6 +39,7 @@ class NavigationObstacle2D : public Node {
 
 	Node2D *parent_node2d = nullptr;
 	RID agent;
+	RID map_before_pause;
 
 	bool estimate_radius = true;
 	real_t radius = 1.0;
diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp
index 86c11b3789f..fb80fe63487 100644
--- a/scene/3d/navigation_agent_3d.cpp
+++ b/scene/3d/navigation_agent_3d.cpp
@@ -113,6 +113,26 @@ void NavigationAgent3D::_notification(int p_what) {
 			set_physics_process_internal(false);
 		} break;
 
+		case NOTIFICATION_PAUSED: {
+			if (agent_parent && !agent_parent->can_process()) {
+				map_before_pause = NavigationServer3D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (agent_parent && agent_parent->can_process() && !(map_before_pause == RID())) {
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
+		case NOTIFICATION_UNPAUSED: {
+			if (agent_parent && !agent_parent->can_process()) {
+				map_before_pause = NavigationServer3D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (agent_parent && agent_parent->can_process() && !(map_before_pause == RID())) {
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 			if (agent_parent) {
 				NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
diff --git a/scene/3d/navigation_agent_3d.h b/scene/3d/navigation_agent_3d.h
index 283b99a24f0..6a88bd13e2b 100644
--- a/scene/3d/navigation_agent_3d.h
+++ b/scene/3d/navigation_agent_3d.h
@@ -41,6 +41,7 @@ class NavigationAgent3D : public Node {
 	Node3D *agent_parent = nullptr;
 
 	RID agent;
+	RID map_before_pause;
 
 	uint32_t navigable_layers = 1;
 
diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp
index fa6a633deeb..c6eda1f9cd7 100644
--- a/scene/3d/navigation_obstacle_3d.cpp
+++ b/scene/3d/navigation_obstacle_3d.cpp
@@ -80,6 +80,26 @@ void NavigationObstacle3D::_notification(int p_what) {
 			parent_node3d = nullptr;
 		} break;
 
+		case NOTIFICATION_PAUSED: {
+			if (parent_node3d && !parent_node3d->can_process()) {
+				map_before_pause = NavigationServer3D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (parent_node3d && parent_node3d->can_process() && !(map_before_pause == RID())) {
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
+		case NOTIFICATION_UNPAUSED: {
+			if (parent_node3d && !parent_node3d->can_process()) {
+				map_before_pause = NavigationServer3D::get_singleton()->agent_get_map(get_rid());
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), RID());
+			} else if (parent_node3d && parent_node3d->can_process() && !(map_before_pause == RID())) {
+				NavigationServer3D::get_singleton()->agent_set_map(get_rid(), map_before_pause);
+				map_before_pause = RID();
+			}
+		} break;
+
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
 			if (parent_node3d && parent_node3d->is_inside_tree()) {
 				NavigationServer3D::get_singleton()->agent_set_position(agent, parent_node3d->get_global_transform().origin);
diff --git a/scene/3d/navigation_obstacle_3d.h b/scene/3d/navigation_obstacle_3d.h
index 542d603a0a1..0ddde64c0eb 100644
--- a/scene/3d/navigation_obstacle_3d.h
+++ b/scene/3d/navigation_obstacle_3d.h
@@ -38,6 +38,7 @@ class NavigationObstacle3D : public Node {
 
 	Node3D *parent_node3d = nullptr;
 	RID agent;
+	RID map_before_pause;
 
 	bool estimate_radius = true;
 	real_t radius = 1.0;