1 files changed, 74 insertions, 11 deletions

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index b0b52a6732..c72b61014b 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -802,9 +802,44 @@ RID GodotNavigationServer::obstacle_create() {
 	RID rid = obstacle_owner.make_rid();
 	NavObstacle *obstacle = obstacle_owner.get_or_null(rid);
 	obstacle->set_self(rid);
+
+	RID agent_rid = agent_owner.make_rid();
+	NavAgent *agent = agent_owner.get_or_null(agent_rid);
+	agent->set_self(agent_rid);
+
+	obstacle->set_agent(agent);
+
 	return rid;
 }
 
+COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+
+	obstacle->set_avoidance_enabled(p_enabled);
+}
+
+bool GodotNavigationServer::obstacle_get_avoidance_enabled(RID p_obstacle) const {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND_V(obstacle == nullptr, false);
+
+	return obstacle->is_avoidance_enabled();
+}
+
+COMMAND_2(obstacle_set_use_3d_avoidance, RID, p_obstacle, bool, p_enabled) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+
+	obstacle->set_use_3d_avoidance(p_enabled);
+}
+
+bool GodotNavigationServer::obstacle_get_use_3d_avoidance(RID p_obstacle) const {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND_V(obstacle == nullptr, false);
+
+	return obstacle->get_use_3d_avoidance();
+}
+
 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);
@@ -837,12 +872,27 @@ RID GodotNavigationServer::obstacle_get_map(RID p_obstacle) const {
 	return RID();
 }
 
+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);
+	ERR_FAIL_COND(obstacle == nullptr);
+
+	obstacle->set_radius(p_radius);
+}
+
 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_velocity, RID, p_obstacle, Vector3, p_velocity) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_COND(obstacle == nullptr);
+
+	obstacle->set_velocity(p_velocity);
+}
+
 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);
@@ -917,29 +967,42 @@ COMMAND_1(free, RID, p_object) {
 		link_owner.free(p_object);
 
 	} else if (agent_owner.owns(p_object)) {
-		NavAgent *agent = agent_owner.get_or_null(p_object);
+		internal_free_agent(p_object);
+
+	} else if (obstacle_owner.owns(p_object)) {
+		internal_free_obstacle(p_object);
+
+	} else {
+		ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
+	}
+}
 
-		// Removes this agent from the map if assigned
+void GodotNavigationServer::internal_free_agent(RID p_object) {
+	NavAgent *agent = agent_owner.get_or_null(p_object);
+	if (agent) {
 		if (agent->get_map() != nullptr) {
 			agent->get_map()->remove_agent(agent);
 			agent->set_map(nullptr);
 		}
-
 		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
+void GodotNavigationServer::internal_free_obstacle(RID p_object) {
+	NavObstacle *obstacle = obstacle_owner.get_or_null(p_object);
+	if (obstacle) {
 		if (obstacle->get_map() != nullptr) {
 			obstacle->get_map()->remove_obstacle(obstacle);
 			obstacle->set_map(nullptr);
 		}
-
+		if (obstacle->get_agent()) {
+			if (obstacle->get_agent()->get_self() != RID()) {
+				RID _agent_rid = obstacle->get_agent()->get_self();
+				obstacle->set_agent(nullptr);
+				internal_free_agent(_agent_rid);
+			}
+		}
 		obstacle_owner.free(p_object);
-
-	} else {
-		ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
 	}
 }