6 files changed, 33 insertions, 16 deletions

diff --git a/doc/classes/NavigationServer.xml b/doc/classes/NavigationServer.xml
index 5eb9f34ead..1f65a6004e 100644
--- a/doc/classes/NavigationServer.xml
+++ b/doc/classes/NavigationServer.xml
@@ -358,13 +358,15 @@
 				Control activation of this server.
 			</description>
 		</method>
-		<method name="step">
+		<method name="process">
 			<return type="void">
 			</return>
 			<argument index="0" name="delta_time" type="float">
 			</argument>
 			<description>
-				Steps the server. This is not threadsafe and must be called in single thread.
+				Process the collision avoidance agents.
+				The result of this process is needed by the physics server, so this must be called in the main thread.
+				Note: This function is not thread safe.
 			</description>
 		</method>
 	</methods>
diff --git a/main/main.cpp b/main/main.cpp
index e3747fc48c..d8a9cc87a7 100644
--- a/main/main.cpp
+++ b/main/main.cpp
@@ -203,6 +203,7 @@ void finalize_physics() {
 
 void initialize_navigation_server() {
 	ERR_FAIL_COND(navigation_server != NULL);
+
 	navigation_server = NavigationServerManager::new_default_server();
 	navigation_2d_server = memnew(Navigation2DServer);
 }
@@ -210,6 +211,7 @@ void initialize_navigation_server() {
 void finalize_navigation_server() {
 	memdelete(navigation_server);
 	navigation_server = NULL;
+
 	memdelete(navigation_2d_server);
 	navigation_2d_server = NULL;
 }
@@ -2037,9 +2039,9 @@ bool Main::iteration() {
 			break;
 		}
 
-		message_queue->flush();
+		NavigationServer::get_singleton_mut()->process(frame_slice * time_scale);
 
-		NavigationServer::get_singleton_mut()->step(frame_slice * time_scale);
+		message_queue->flush();
 
 		PhysicsServer::get_singleton()->step(frame_slice * time_scale);
 
diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
index 1f1783802d..4129d1f65a 100644
--- a/modules/gdnavigation/gd_navigation_server.cpp
+++ b/modules/gdnavigation/gd_navigation_server.cpp
@@ -121,6 +121,7 @@ GdNavigationServer::GdNavigationServer() :
 }
 
 GdNavigationServer::~GdNavigationServer() {
+	flush_queries();
 	memdelete(operations_mutex);
 	memdelete(commands_mutex);
 }
@@ -474,12 +475,9 @@ void GdNavigationServer::set_active(bool p_active) const {
 	mut_this->operations_mutex->unlock();
 }
 
-void GdNavigationServer::step(real_t p_delta_time) {
-	if (!active) {
-		return;
-	}
-
-	// With c++ we can't be 100% sure this is called in single thread so use the mutex.
+void GdNavigationServer::flush_queries() {
+	// In c++ we can't be sure that this is performed in the main thread
+	// even with mutable functions.
 	commands_mutex->lock();
 	operations_mutex->lock();
 	for (size_t i(0); i < commands.size(); i++) {
@@ -489,13 +487,24 @@ void GdNavigationServer::step(real_t p_delta_time) {
 	commands.clear();
 	operations_mutex->unlock();
 	commands_mutex->unlock();
+}
+
+void GdNavigationServer::process(real_t p_delta_time) {
+	flush_queries();
 
-	// These are internal operations so don't need to be shielded.
+	if (!active) {
+		return;
+	}
+
+	// In c++ we can't be sure that this is performed in the main thread
+	// even with mutable functions.
+	operations_mutex->lock();
 	for (int i(0); i < active_maps.size(); i++) {
 		active_maps[i]->sync();
 		active_maps[i]->step(p_delta_time);
 		active_maps[i]->dispatch_callbacks();
 	}
+	operations_mutex->unlock();
 }
 
 #undef COMMAND_1
diff --git a/modules/gdnavigation/gd_navigation_server.h b/modules/gdnavigation/gd_navigation_server.h
index 7fa5979c31..0400acf1a3 100644
--- a/modules/gdnavigation/gd_navigation_server.h
+++ b/modules/gdnavigation/gd_navigation_server.h
@@ -131,7 +131,9 @@ public:
 	COMMAND_1(free, RID, p_object);
 
 	virtual void set_active(bool p_active) const;
-	virtual void step(real_t p_delta_time);
+
+	void flush_queries();
+	virtual void process(real_t p_delta_time);
 };
 
 #undef COMMAND_1
diff --git a/servers/navigation_server.cpp b/servers/navigation_server.cpp
index f31795fb35..f2b727ac47 100644
--- a/servers/navigation_server.cpp
+++ b/servers/navigation_server.cpp
@@ -75,7 +75,7 @@ void NavigationServer::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("free", "object"), &NavigationServer::free);
 
 	ClassDB::bind_method(D_METHOD("set_active", "active"), &NavigationServer::set_active);
-	ClassDB::bind_method(D_METHOD("step", "delta_time"), &NavigationServer::step);
+	ClassDB::bind_method(D_METHOD("process", "delta_time"), &NavigationServer::process);
 }
 
 const NavigationServer *NavigationServer::get_singleton() {
diff --git a/servers/navigation_server.h b/servers/navigation_server.h
index d4d95d72d4..2587e53ab2 100644
--- a/servers/navigation_server.h
+++ b/servers/navigation_server.h
@@ -175,9 +175,11 @@ public:
 	/// Control activation of this server.
 	virtual void set_active(bool p_active) const = 0;
 
-	/// Step the server
-	/// NOTE: This function is not Threadsafe and MUST be called in single thread.
-	virtual void step(real_t delta_time) = 0;
+	/// Process the collision avoidance agents.
+	/// The result of this process is needed by the physics server,
+	/// so this must be called in the main thread.
+	/// Note: This function is not thread safe.
+	virtual void process(real_t delta_time) = 0;
 
 	NavigationServer();
 	virtual ~NavigationServer();