Add agent pause mode to NavigationServer

Adds agent pause mode to NavigationServer.

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);

modules/navigation/godot_navigation_server.h

@@ -184,6 +184,8 @@ public:
 	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 @@ public:
 	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);

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;
+}

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 @@ public:
 	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.

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) {

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;
+}

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 @@ public:
 	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: