3 files changed, 24 insertions, 10 deletions

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/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp
index d3e2f8f388..c3880f89b6 100644
--- a/modules/gdnavigation/nav_map.cpp
+++ b/modules/gdnavigation/nav_map.cpp
@@ -545,8 +545,11 @@ void NavMap::add_region(NavRegion *p_region) {
 }
 
 void NavMap::remove_region(NavRegion *p_region) {
-	regions.push_back(p_region);
-	regenerate_links = true;
+	std::vector<NavRegion *>::iterator it = std::find(regions.begin(), regions.end(), p_region);
+	if (it != regions.end()) {
+		regions.erase(it);
+		regenerate_links = true;
+	}
 }
 
 bool NavMap::has_agent(RvoAgent *agent) const {