Fix NavObjects map assignments

Fixes NavObject map assignments.

modules/navigation/godot_navigation_server.cpp

@@ -358,22 +358,9 @@ COMMAND_2(region_set_map, RID, p_region, RID, p_map) {
 	NavRegion *region = region_owner.get_or_null(p_region);
 	ERR_FAIL_COND(region == nullptr);
 
-	if (region->get_map() != nullptr) {
-		if (region->get_map()->get_self() == p_map) {
-			return; // Pointless
-		}
-
-		region->get_map()->remove_region(region);
-		region->set_map(nullptr);
-	}
-
-	if (p_map.is_valid()) {
-		NavMap *map = map_owner.get_or_null(p_map);
-		ERR_FAIL_COND(map == nullptr);
+	NavMap *map = map_owner.get_or_null(p_map);
 
-		map->add_region(region);
-		region->set_map(map);
-	}
+	region->set_map(map);
 }
 
 COMMAND_2(region_set_transform, RID, p_region, Transform3D, p_transform) {
@@ -506,22 +493,9 @@ COMMAND_2(link_set_map, RID, p_link, RID, p_map) {
 	NavLink *link = link_owner.get_or_null(p_link);
 	ERR_FAIL_COND(link == nullptr);
 
-	if (link->get_map() != nullptr) {
-		if (link->get_map()->get_self() == p_map) {
-			return; // Pointless
-		}
-
-		link->get_map()->remove_link(link);
-		link->set_map(nullptr);
-	}
-
-	if (p_map.is_valid()) {
-		NavMap *map = map_owner.get_or_null(p_map);
-		ERR_FAIL_COND(map == nullptr);
+	NavMap *map = map_owner.get_or_null(p_map);
 
-		map->add_link(link);
-		link->set_map(map);
-	}
+	link->set_map(map);
 }
 
 RID GodotNavigationServer::link_get_map(const RID p_link) const {
@@ -673,27 +647,9 @@ 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);
 
-	if (agent->get_map()) {
-		if (agent->get_map()->get_self() == p_map) {
-			return; // Pointless
-		}
-
-		agent->get_map()->remove_agent(agent);
-	}
-
-	agent->set_map(nullptr);
-
-	if (p_map.is_valid()) {
-		NavMap *map = map_owner.get_or_null(p_map);
-		ERR_FAIL_COND(map == nullptr);
-
-		agent->set_map(map);
-		map->add_agent(agent);
+	NavMap *map = map_owner.get_or_null(p_map);
 
-		if (agent->has_avoidance_callback()) {
-			map->set_agent_as_controlled(agent);
-		}
-	}
+	agent->set_map(map);
 }
 
 COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused) {
@@ -875,23 +831,9 @@ 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);
+	NavMap *map = map_owner.get_or_null(p_map);
 
-		obstacle->set_map(map);
-		map->add_obstacle(obstacle);
-	}
+	obstacle->set_map(map);
 }
 
 RID GodotNavigationServer::obstacle_get_map(RID p_obstacle) const {
@@ -1050,17 +992,16 @@ void GodotNavigationServer::internal_free_agent(RID p_object) {
 void GodotNavigationServer::internal_free_obstacle(RID p_object) {
 	NavObstacle *obstacle = obstacle_owner.get_or_null(p_object);
 	if (obstacle) {
+		NavAgent *obstacle_agent = obstacle->get_agent();
+		if (obstacle_agent) {
+			RID _agent_rid = obstacle_agent->get_self();
+			internal_free_agent(_agent_rid);
+			obstacle->set_agent(nullptr);
+		}
 		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);
 	}
 }

modules/navigation/nav_agent.cpp

@@ -90,8 +90,23 @@ void NavAgent::_update_rvo_agent_properties() {
 }
 
 void NavAgent::set_map(NavMap *p_map) {
+	if (map == p_map) {
+		return;
+	}
+
+	if (map) {
+		map->remove_agent(this);
+	}
+
 	map = p_map;
 	agent_dirty = true;
+
+	if (map) {
+		map->add_agent(this);
+		if (avoidance_enabled) {
+			map->set_agent_as_controlled(this);
+		}
+	}
 }
 
 bool NavAgent::is_map_changed() {

modules/navigation/nav_link.cpp

@@ -36,8 +36,17 @@ void NavLink::set_map(NavMap *p_map) {
 	if (map == p_map) {
 		return;
 	}
+
+	if (map) {
+		map->remove_link(this);
+	}
+
 	map = p_map;
 	link_dirty = true;
+
+	if (map) {
+		map->add_link(this);
+	}
 }
 
 void NavLink::set_bidirectional(bool p_bidirectional) {

modules/navigation/nav_region.cpp

@@ -36,10 +36,18 @@ void NavRegion::set_map(NavMap *p_map) {
 	if (map == p_map) {
 		return;
 	}
+
+	if (map) {
+		map->remove_region(this);
+	}
+
 	map = p_map;
 	polygons_dirty = true;
-	if (!map) {
-		connections.clear();
+
+	connections.clear();
+
+	if (map) {
+		map->add_region(this);
 	}
 }