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.

9 files changed, 92 insertions, 2 deletions

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index 42aad77979..cc9d05da47 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 91549d75f0..59a7dc819f 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 1447e25e8c..2a401190d0 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 b594aa3bb2..0320c6c917 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 2a0ef14e73..948cf5b61a 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 86c11b3789..fb80fe6348 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 283b99a24f..6a88bd13e2 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 fa6a633dee..c6eda1f9cd 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 542d603a0a..0ddde64c0e 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;