Rework Navigation Avoidance

Rework Navigation Avoidance.

1 files changed, 172 insertions, 18 deletions

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index ee9687cf3d..63423e655c 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -271,6 +271,18 @@ TypedArray<RID> GodotNavigationServer::map_get_agents(RID p_map) const {
 	return agents_rids;
 }
 
+TypedArray<RID> GodotNavigationServer::map_get_obstacles(RID p_map) const {
+	TypedArray<RID> obstacles_rids;
+	const NavMap *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_COND_V(map == nullptr, obstacles_rids);
+	const LocalVector<NavObstacle *> obstacles = map->get_obstacles();
+	obstacles_rids.resize(obstacles.size());
+	for (uint32_t i = 0; i < obstacles.size(); i++) {
+		obstacles_rids[i] = obstacles[i]->get_self();
+	}
+	return obstacles_rids;
+}
+
 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());
@@ -584,6 +596,34 @@ RID GodotNavigationServer::agent_create() {
 	return rid;
 }
 
+COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled) {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+
+	agent->set_avoidance_enabled(p_enabled);
+}
+
+bool GodotNavigationServer::agent_get_avoidance_enabled(RID p_agent) const {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND_V(agent == nullptr, false);
+
+	return agent->is_avoidance_enabled();
+}
+
+COMMAND_2(agent_set_use_3d_avoidance, RID, p_agent, bool, p_enabled) {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+
+	agent->set_use_3d_avoidance(p_enabled);
+}
+
+bool GodotNavigationServer::agent_get_use_3d_avoidance(RID p_agent) const {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND_V(agent == nullptr, false);
+
+	return agent->get_use_3d_avoidance();
+}
+
 COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
@@ -605,7 +645,7 @@ COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
 		agent->set_map(map);
 		map->add_agent(agent);
 
-		if (agent->has_callback()) {
+		if (agent->has_avoidance_callback()) {
 			map->set_agent_as_controlled(agent);
 		}
 	}
@@ -615,63 +655,75 @@ 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);
 
-	agent->get_agent()->neighborDist_ = p_distance;
+	agent->set_neighbor_distance(p_distance);
 }
 
 COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->maxNeighbors_ = p_count;
+	agent->set_max_neighbors(p_count);
 }
 
-COMMAND_2(agent_set_time_horizon, RID, p_agent, real_t, p_time) {
+COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon) {
+	ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive.");
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->timeHorizon_ = p_time;
+	agent->set_time_horizon_agents(p_time_horizon);
+}
+
+COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon) {
+	ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizion must be positive.");
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+
+	agent->set_time_horizon_obstacles(p_time_horizon);
 }
 
 COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) {
+	ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->radius_ = p_radius;
+	agent->set_radius(p_radius);
 }
 
-COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
+COMMAND_2(agent_set_height, RID, p_agent, real_t, p_height) {
+	ERR_FAIL_COND_MSG(p_height < 0.0, "Height must be positive.");
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->maxSpeed_ = p_max_speed;
+	agent->set_height(p_height);
 }
 
-COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) {
+COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
+	ERR_FAIL_COND_MSG(p_max_speed < 0.0, "Max speed must be positive.");
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->velocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+	agent->set_max_speed(p_max_speed);
 }
 
-COMMAND_2(agent_set_target_velocity, RID, p_agent, Vector3, p_velocity) {
+COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->prefVelocity_ = RVO::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+	agent->set_velocity(p_velocity);
 }
 
-COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) {
+COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector3, p_velocity) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->position_ = RVO::Vector3(p_position.x, p_position.y, p_position.z);
+	agent->set_velocity_forced(p_velocity);
 }
 
-COMMAND_2(agent_set_ignore_y, RID, p_agent, bool, p_ignore) {
+COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->get_agent()->ignore_y_ = p_ignore;
+	agent->set_position(p_position);
 }
 
 bool GodotNavigationServer::agent_is_map_changed(RID p_agent) const {
@@ -681,11 +733,11 @@ bool GodotNavigationServer::agent_is_map_changed(RID p_agent) const {
 	return agent->is_map_changed();
 }
 
-COMMAND_2(agent_set_callback, RID, p_agent, Callable, p_callback) {
+COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback) {
 	NavAgent *agent = agent_owner.get_or_null(p_agent);
 	ERR_FAIL_COND(agent == nullptr);
 
-	agent->set_callback(p_callback);
+	agent->set_avoidance_callback(p_callback);
 
 	if (agent->get_map()) {
 		if (p_callback.is_valid()) {
@@ -696,6 +748,91 @@ COMMAND_2(agent_set_callback, RID, p_agent, Callable, p_callback) {
 	}
 }
 
+COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers) {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+	agent->set_avoidance_layers(p_layers);
+}
+
+COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask) {
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+	agent->set_avoidance_mask(p_mask);
+}
+
+COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority) {
+	ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	NavAgent *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_COND(agent == nullptr);
+	agent->set_avoidance_priority(p_priority);
+}
+
+RID GodotNavigationServer::obstacle_create() {
+	GodotNavigationServer *mut_this = const_cast<GodotNavigationServer *>(this);
+	MutexLock lock(mut_this->operations_mutex);
+	RID rid = obstacle_owner.make_rid();
+	NavObstacle *obstacle = obstacle_owner.get_or_null(rid);
+	obstacle->set_self(rid);
+	return rid;
+}
+
+COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+
+	if (obstacle->get_map()) {
+		if (obstacle->get_map()->get_self() == p_map) {
+			return; // Pointless
+		}
+
+		obstacle->get_map()->remove_obstacle(obstacle);
+	}
+
+	obstacle->set_map(nullptr);
+
+	if (p_map.is_valid()) {
+		NavMap *map = map_owner.get_or_null(p_map);
+		ERR_FAIL_COND(map == nullptr);
+
+		obstacle->set_map(map);
+		map->add_obstacle(obstacle);
+	}
+}
+
+RID GodotNavigationServer::obstacle_get_map(RID p_obstacle) const {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND_V(obstacle == nullptr, RID());
+	if (obstacle->get_map()) {
+		return obstacle->get_map()->get_self();
+	}
+	return RID();
+}
+
+COMMAND_2(obstacle_set_height, RID, p_obstacle, real_t, p_height) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+	obstacle->set_height(p_height);
+}
+
+COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+	obstacle->set_position(p_position);
+}
+
+void GodotNavigationServer::obstacle_set_vertices(RID p_obstacle, const Vector<Vector3> &p_vertices) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+	obstacle->set_vertices(p_vertices);
+}
+
+COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+	obstacle->set_avoidance_layers(p_layers);
+}
+
 COMMAND_1(free, RID, p_object) {
 	if (map_owner.owns(p_object)) {
 		NavMap *map = map_owner.get_or_null(p_object);
@@ -718,6 +855,12 @@ COMMAND_1(free, RID, p_object) {
 			agent->set_map(nullptr);
 		}
 
+		// Remove any assigned obstacles
+		for (NavObstacle *obstacle : map->get_obstacles()) {
+			map->remove_obstacle(obstacle);
+			obstacle->set_map(nullptr);
+		}
+
 		int map_index = active_maps.find(map);
 		active_maps.remove_at(map_index);
 		active_maps_update_id.remove_at(map_index);
@@ -756,6 +899,17 @@ COMMAND_1(free, RID, p_object) {
 
 		agent_owner.free(p_object);
 
+	} else if (obstacle_owner.owns(p_object)) {
+		NavObstacle *obstacle = obstacle_owner.get_or_null(p_object);
+
+		// Removes this agent from the map if assigned
+		if (obstacle->get_map() != nullptr) {
+			obstacle->get_map()->remove_obstacle(obstacle);
+			obstacle->set_map(nullptr);
+		}
+
+		obstacle_owner.free(p_object);
+
 	} else {
 		ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
 	}