Fix NavigationAgent position not always updating

Fixes NavigationAgent position not always updating.

scene/2d/navigation_agent_2d.cpp

@@ -258,11 +258,13 @@ void NavigationAgent2D::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+			if (agent_parent && avoidance_enabled) {
+				NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position());
+			}
 			if (agent_parent && target_position_submitted) {
 				if (velocity_submitted) {
 					velocity_submitted = false;
 					if (avoidance_enabled) {
-						NavigationServer2D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position());
 						NavigationServer2D::get_singleton()->agent_set_velocity(agent, velocity);
 					}
 				}

scene/3d/navigation_agent_3d.cpp

@@ -273,11 +273,13 @@ void NavigationAgent3D::_notification(int p_what) {
 		} break;
 
 		case NOTIFICATION_INTERNAL_PHYSICS_PROCESS: {
+			if (agent_parent && avoidance_enabled) {
+				NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_position());
+			}
 			if (agent_parent && target_position_submitted) {
 				if (velocity_submitted) {
 					velocity_submitted = false;
 					if (avoidance_enabled) {
-						NavigationServer3D::get_singleton()->agent_set_position(agent, agent_parent->get_global_transform().origin);
 						if (!use_3d_avoidance) {
 							stored_y_velocity = velocity.y;
 							velocity.y = 0.0;