1 files changed, 54 insertions, 14 deletions

diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
index 2780e2a931..4129d1f65a 100644
--- a/modules/gdnavigation/gd_navigation_server.cpp
+++ b/modules/gdnavigation/gd_navigation_server.cpp
@@ -115,12 +115,16 @@
 
 GdNavigationServer::GdNavigationServer() :
 		NavigationServer(),
+		commands_mutex(Mutex::create()),
+		operations_mutex(Mutex::create()),
 		active(true) {
-	commands_mutex = Mutex::create();
-	operations_mutex = Mutex::create();
 }
 
-GdNavigationServer::~GdNavigationServer() {}
+GdNavigationServer::~GdNavigationServer() {
+	flush_queries();
+	memdelete(operations_mutex);
+	memdelete(commands_mutex);
+}
 
 void GdNavigationServer::add_command(SetCommand *command) const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
@@ -167,7 +171,7 @@ COMMAND_2(map_set_up, RID, p_map, Vector3, p_up) {
 }
 
 Vector3 GdNavigationServer::map_get_up(RID p_map) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, Vector3());
 
 	return map->get_up();
@@ -181,7 +185,7 @@ COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
 }
 
 real_t GdNavigationServer::map_get_cell_size(RID p_map) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, 0);
 
 	return map->get_cell_size();
@@ -195,19 +199,47 @@ COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margi
 }
 
 real_t GdNavigationServer::map_get_edge_connection_margin(RID p_map) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, 0);
 
 	return map->get_edge_connection_margin();
 }
 
 Vector<Vector3> GdNavigationServer::map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize) const {
-	NavMap *map = map_owner.getornull(p_map);
+	const NavMap *map = map_owner.getornull(p_map);
 	ERR_FAIL_COND_V(map == NULL, Vector<Vector3>());
 
 	return map->get_path(p_origin, p_destination, p_optimize);
 }
 
+Vector3 GdNavigationServer::map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, Vector3());
+
+	return map->get_closest_point_to_segment(p_from, p_to, p_use_collision);
+}
+
+Vector3 GdNavigationServer::map_get_closest_point(RID p_map, const Vector3 &p_point) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, Vector3());
+
+	return map->get_closest_point(p_point);
+}
+
+Vector3 GdNavigationServer::map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, Vector3());
+
+	return map->get_closest_point_normal(p_point);
+}
+
+RID GdNavigationServer::map_get_closest_point_owner(RID p_map, const Vector3 &p_point) const {
+	const NavMap *map = map_owner.getornull(p_map);
+	ERR_FAIL_COND_V(map == NULL, RID());
+
+	return map->get_closest_point_owner(p_point);
+}
+
 RID GdNavigationServer::region_create() const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
 	mut_this->operations_mutex->lock();
@@ -443,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++) {
@@ -458,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();
+
+	if (!active) {
+		return;
+	}
 
-	// These are internal operations so don't need to be shielded.
+	// 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