Add agent pause mode to NavigationServer

Adds agent pause mode to NavigationServer.
398utubzyt · Jun 18, 2023 · ae9dd47 · ae9dd47
1 parent a83eb16
commit ae9dd47
Show file tree

Hide file tree

Showing 20 changed files with 194 additions and 26 deletions.
diff --git a/doc/classes/NavigationServer2D.xml b/doc/classes/NavigationServer2D.xml
@@ -38,6 +38,13 @@
 				Returns the navigation map [RID] the requested [param agent] is currently assigned to.
 			</description>
 		</method>
+		<method name="agent_get_paused" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="agent" type="RID" />
+			<description>
+				Returns [code]true[/code] if the specified [param agent] is paused.
+			</description>
+		</method>
 		<method name="agent_is_map_changed" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="agent" type="RID" />
@@ -119,6 +126,14 @@
 				Sets the maximum distance to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
 			</description>
 		</method>
+		<method name="agent_set_paused">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="paused" type="bool" />
+			<description>
+				If [param paused] is true the specified [param agent] will not be processed, e.g. calculate avoidance velocities or receive avoidance callbacks.
+			</description>
+		</method>
 		<method name="agent_set_position">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
@@ -478,6 +493,13 @@
 				Returns the navigation map [RID] the requested [param obstacle] is currently assigned to.
 			</description>
 		</method>
+		<method name="obstacle_get_paused" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="obstacle" type="RID" />
+			<description>
+				Returns [code]true[/code] if the specified [param obstacle] is paused.
+			</description>
+		</method>
 		<method name="obstacle_set_avoidance_enabled">
 			<return type="void" />
 			<param index="0" name="obstacle" type="RID" />
@@ -502,6 +524,14 @@
 				Sets the navigation map [RID] for the obstacle.
 			</description>
 		</method>
+		<method name="obstacle_set_paused">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="paused" type="bool" />
+			<description>
+				If [param paused] is true the specified [param obstacle] will not be processed, e.g. affect avoidance velocities.
+			</description>
+		</method>
 		<method name="obstacle_set_position">
 			<return type="void" />
 			<param index="0" name="obstacle" type="RID" />

diff --git a/doc/classes/NavigationServer3D.xml b/doc/classes/NavigationServer3D.xml
@@ -38,6 +38,13 @@
 				Returns the navigation map [RID] the requested [param agent] is currently assigned to.
 			</description>
 		</method>
+		<method name="agent_get_paused" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="agent" type="RID" />
+			<description>
+				Returns [code]true[/code] if the specified [param agent] is paused.
+			</description>
+		</method>
 		<method name="agent_get_use_3d_avoidance" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="agent" type="RID" />
@@ -134,6 +141,14 @@
 				Sets the maximum distance to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
 			</description>
 		</method>
+		<method name="agent_set_paused">
+			<return type="void" />
+			<param index="0" name="agent" type="RID" />
+			<param index="1" name="paused" type="bool" />
+			<description>
+				If [param paused] is true the specified [param agent] will not be processed, e.g. calculate avoidance velocities or receive avoidance callbacks.
+			</description>
+		</method>
 		<method name="agent_set_position">
 			<return type="void" />
 			<param index="0" name="agent" type="RID" />
@@ -567,6 +582,13 @@
 				Returns the navigation map [RID] the requested [param obstacle] is currently assigned to.
 			</description>
 		</method>
+		<method name="obstacle_get_paused" qualifiers="const">
+			<return type="bool" />
+			<param index="0" name="obstacle" type="RID" />
+			<description>
+				Returns [code]true[/code] if the specified [param obstacle] is paused.
+			</description>
+		</method>
 		<method name="obstacle_get_use_3d_avoidance" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="obstacle" type="RID" />
@@ -606,6 +628,14 @@
 				Assigns the [param obstacle] to a navigation map.
 			</description>
 		</method>
+		<method name="obstacle_set_paused">
+			<return type="void" />
+			<param index="0" name="obstacle" type="RID" />
+			<param index="1" name="paused" type="bool" />
+			<description>
+				If [param paused] is true the specified [param obstacle] will not be processed, e.g. affect avoidance velocities.
+			</description>
+		</method>
 		<method name="obstacle_set_position">
 			<return type="void" />
 			<param index="0" name="obstacle" type="RID" />

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
@@ -696,6 +696,20 @@ COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
 	}
 }
 
+COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused) {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+
+	agent->set_paused(p_paused);
+}
+
+bool GodotNavigationServer::agent_get_paused(RID p_agent) const {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND_V(agent == nullptr, false);
+
+	return agent->get_paused();
+}
+
 COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
@@ -889,6 +903,20 @@ RID GodotNavigationServer::obstacle_get_map(RID p_obstacle) const {
 	return RID();
 }
 
+COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+
+	obstacle->set_paused(p_paused);
+}
+
+bool GodotNavigationServer::obstacle_get_paused(RID p_obstacle) const {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND_V(obstacle == nullptr, false);
+
+	return obstacle->get_paused();
+}
+
 COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius) {
 	ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
 	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);

diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
@@ -184,6 +184,8 @@ class GodotNavigationServer : public NavigationServer3D {
 	virtual bool agent_get_use_3d_avoidance(RID p_agent) const override;
 	COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
 	virtual RID agent_get_map(RID p_agent) const override;
+	COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused);
+	virtual bool agent_get_paused(RID p_agent) const override;
 	COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance);
 	COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
 	COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon);
@@ -207,6 +209,8 @@ class GodotNavigationServer : public NavigationServer3D {
 	virtual bool obstacle_get_use_3d_avoidance(RID p_obstacle) const override;
 	COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map);
 	virtual RID obstacle_get_map(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused);
+	virtual bool obstacle_get_paused(RID p_obstacle) const override;
 	COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius);
 	COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector3, p_velocity);
 	COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position);

diff --git a/modules/navigation/nav_agent.cpp b/modules/navigation/nav_agent.cpp
@@ -342,3 +342,23 @@ const Dictionary NavAgent::get_avoidance_data() const {
 	}
 	return _avoidance_data;
 }
+
+void NavAgent::set_paused(bool p_paused) {
+	if (paused == p_paused) {
+		return;
+	}
+
+	paused = p_paused;
+
+	if (map) {
+		if (paused) {
+			map->remove_agent_as_controlled(this);
+		} else {
+			map->set_agent_as_controlled(this);
+		}
+	}
+}
+
+bool NavAgent::get_paused() const {
+	return paused;
+}
diff --git a/modules/navigation/nav_agent.h b/modules/navigation/nav_agent.h
@@ -73,6 +73,7 @@ class NavAgent : public NavRid {
 	bool agent_dirty = true;
 
 	uint32_t map_update_id = 0;
+	bool paused = false;
 
 public:
 	NavAgent();
@@ -138,6 +139,9 @@ class NavAgent : public NavRid {
 	void set_avoidance_priority(real_t p_priority);
 	real_t get_avoidance_priority() const { return avoidance_priority; };
 
+	void set_paused(bool p_paused);
+	bool get_paused() const;
+
 	bool check_dirty();
 
 	// Updates this agent with rvo data after the rvo simulation avoidance step.

diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp
@@ -628,6 +628,11 @@ bool NavMap::has_obstacle(NavObstacle *obstacle) const {
 }
 
 void NavMap::add_obstacle(NavObstacle *obstacle) {
+	if (obstacle->get_paused()) {
+		// No point in adding a paused obstacle, it will add itself when unpaused again.
+		return;
+	}
+
 	if (!has_obstacle(obstacle)) {
 		obstacles.push_back(obstacle);
 		obstacles_dirty = true;
@@ -644,6 +649,12 @@ void NavMap::remove_obstacle(NavObstacle *obstacle) {
 
 void NavMap::set_agent_as_controlled(NavAgent *agent) {
 	remove_agent_as_controlled(agent);
+
+	if (agent->get_paused()) {
+		// No point in adding a paused agent, it will add itself when unpaused again.
+		return;
+	}
+
 	if (agent->get_use_3d_avoidance()) {
 		int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
 		if (agent_3d_index < 0) {

diff --git a/modules/navigation/nav_obstacle.cpp b/modules/navigation/nav_obstacle.cpp
@@ -186,6 +186,7 @@ void NavObstacle::internal_update_agent() {
 		agent->set_neighbor_distance(0.0);
 		agent->set_avoidance_priority(1.0);
 		agent->set_map(map);
+		agent->set_paused(paused);
 		agent->set_radius(radius);
 		agent->set_height(height);
 		agent->set_position(position);
@@ -194,3 +195,24 @@ void NavObstacle::internal_update_agent() {
 		agent->set_use_3d_avoidance(use_3d_avoidance);
 	}
 }
+
+void NavObstacle::set_paused(bool p_paused) {
+	if (paused == p_paused) {
+		return;
+	}
+
+	paused = p_paused;
+
+	if (map) {
+		if (paused) {
+			map->remove_obstacle(this);
+		} else {
+			map->add_obstacle(this);
+		}
+	}
+	internal_update_agent();
+}
+
+bool NavObstacle::get_paused() const {
+	return paused;
+}
diff --git a/modules/navigation/nav_obstacle.h b/modules/navigation/nav_obstacle.h
@@ -56,6 +56,7 @@ class NavObstacle : public NavRid {
 	bool obstacle_dirty = true;
 
 	uint32_t map_update_id = 0;
+	bool paused = false;
 
 public:
 	NavObstacle();
@@ -93,6 +94,9 @@ class NavObstacle : public NavRid {
 	void set_avoidance_layers(uint32_t p_layers);
 	uint32_t get_avoidance_layers() const { return avoidance_layers; };
 
+	void set_paused(bool p_paused);
+	bool get_paused() const;
+
 	bool check_dirty();
 
 private:

diff --git a/scene/2d/navigation_agent_2d.cpp b/scene/2d/navigation_agent_2d.cpp
@@ -246,22 +246,14 @@ void NavigationAgent2D::_notification(int p_what) {
 		} 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();
+			if (agent_parent) {
+				NavigationServer2D::get_singleton()->agent_set_paused(get_rid(), !agent_parent->can_process());
 			}
 		} 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();
+			if (agent_parent) {
+				NavigationServer2D::get_singleton()->agent_set_paused(get_rid(), !agent_parent->can_process());
 			}
 		} break;
 

diff --git a/scene/2d/navigation_agent_2d.h b/scene/2d/navigation_agent_2d.h
@@ -43,7 +43,6 @@ class NavigationAgent2D : public Node {
 	Node2D *agent_parent = nullptr;
 
 	RID agent;
-	RID map_before_pause;
 	RID map_override;
 
 	bool avoidance_enabled = false;

diff --git a/scene/2d/navigation_obstacle_2d.cpp b/scene/2d/navigation_obstacle_2d.cpp
@@ -96,6 +96,7 @@ void NavigationObstacle2D::_notification(int p_what) {
 				_update_map(map_before_pause);
 				map_before_pause = RID();
 			}
+			NavigationServer2D::get_singleton()->obstacle_set_paused(obstacle, !can_process());
 		} break;
 
 		case NOTIFICATION_UNPAUSED: {
@@ -106,6 +107,7 @@ void NavigationObstacle2D::_notification(int p_what) {
 				_update_map(map_before_pause);
 				map_before_pause = RID();
 			}
+			NavigationServer2D::get_singleton()->obstacle_set_paused(obstacle, !can_process());
 		} break;
 
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {

diff --git a/scene/3d/navigation_agent_3d.cpp b/scene/3d/navigation_agent_3d.cpp
@@ -261,22 +261,14 @@ void NavigationAgent3D::_notification(int p_what) {
 		} 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();
+			if (agent_parent) {
+				NavigationServer3D::get_singleton()->agent_set_paused(get_rid(), !agent_parent->can_process());
 			}
 		} 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();
+			if (agent_parent) {
+				NavigationServer3D::get_singleton()->agent_set_paused(get_rid(), !agent_parent->can_process());
 			}
 		} break;
 

diff --git a/scene/3d/navigation_agent_3d.h b/scene/3d/navigation_agent_3d.h
@@ -43,7 +43,6 @@ class NavigationAgent3D : public Node {
 	Node3D *agent_parent = nullptr;
 
 	RID agent;
-	RID map_before_pause;
 	RID map_override;
 
 	bool avoidance_enabled = false;

diff --git a/scene/3d/navigation_obstacle_3d.cpp b/scene/3d/navigation_obstacle_3d.cpp
@@ -120,6 +120,7 @@ void NavigationObstacle3D::_notification(int p_what) {
 				_update_map(map_before_pause);
 				map_before_pause = RID();
 			}
+			NavigationServer3D::get_singleton()->obstacle_set_paused(obstacle, !can_process());
 		} break;
 
 		case NOTIFICATION_UNPAUSED: {
@@ -130,6 +131,7 @@ void NavigationObstacle3D::_notification(int p_what) {
 				_update_map(map_before_pause);
 				map_before_pause = RID();
 			}
+			NavigationServer3D::get_singleton()->obstacle_set_paused(obstacle, !can_process());
 		} break;
 
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {