1 files changed, 60 insertions, 30 deletions

diff --git a/modules/gdnavigation/gd_navigation_server.cpp b/modules/gdnavigation/gd_navigation_server.cpp
index 2780e2a931..a1f6ddfedc 100644
--- a/modules/gdnavigation/gd_navigation_server.cpp
+++ b/modules/gdnavigation/gd_navigation_server.cpp
@@ -41,7 +41,7 @@
 */
 
 /// Creates a struct for each function and a function that once called creates
-/// an instance of that struct with the submited parameters.
+/// an instance of that struct with the submitted parameters.
 /// Then, that struct is stored in an array; the `sync` function consume that array.
 
 #define COMMAND_1(F_NAME, T_0, D_0)                      \
@@ -116,26 +116,26 @@
 GdNavigationServer::GdNavigationServer() :
 		NavigationServer(),
 		active(true) {
-	commands_mutex = Mutex::create();
-	operations_mutex = Mutex::create();
 }
 
-GdNavigationServer::~GdNavigationServer() {}
+GdNavigationServer::~GdNavigationServer() {
+	flush_queries();
+}
 
 void GdNavigationServer::add_command(SetCommand *command) const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
-	commands_mutex->lock();
-	mut_this->commands.push_back(command);
-	commands_mutex->unlock();
+	{
+		MutexLock lock(commands_mutex);
+		mut_this->commands.push_back(command);
+	}
 }
 
 RID GdNavigationServer::map_create() const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
-	mut_this->operations_mutex->lock();
+	MutexLock lock(mut_this->operations_mutex);
 	NavMap *space = memnew(NavMap);
 	RID rid = map_owner.make_rid(space);
 	space->set_self(rid);
-	mut_this->operations_mutex->unlock();
 	return rid;
 }
 
@@ -167,7 +167,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 +181,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,26 +195,53 @@ 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();
+	MutexLock lock(mut_this->operations_mutex);
 	NavRegion *reg = memnew(NavRegion);
 	RID rid = region_owner.make_rid(reg);
 	reg->set_self(rid);
-	mut_this->operations_mutex->unlock();
 	return rid;
 }
 
@@ -266,11 +293,10 @@ void GdNavigationServer::region_bake_navmesh(Ref<NavigationMesh> r_mesh, Node *p
 
 RID GdNavigationServer::agent_create() const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
-	mut_this->operations_mutex->lock();
+	MutexLock lock(mut_this->operations_mutex);
 	RvoAgent *agent = memnew(RvoAgent());
 	RID rid = agent_owner.make_rid(agent);
 	agent->set_self(rid);
-	mut_this->operations_mutex->unlock();
 	return rid;
 }
 
@@ -438,28 +464,32 @@ COMMAND_1(free, RID, p_object) {
 
 void GdNavigationServer::set_active(bool p_active) const {
 	auto mut_this = const_cast<GdNavigationServer *>(this);
-	mut_this->operations_mutex->lock();
+	MutexLock lock(mut_this->operations_mutex);
 	mut_this->active = p_active;
-	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.
-	commands_mutex->lock();
-	operations_mutex->lock();
+void GdNavigationServer::flush_queries() {
+	// In c++ we can't be sure that this is performed in the main thread
+	// even with mutable functions.
+	MutexLock lock(commands_mutex);
+	MutexLock lock2(operations_mutex);
 	for (size_t i(0); i < commands.size(); i++) {
 		commands[i]->exec(this);
 		memdelete(commands[i]);
 	}
 	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.
+	MutexLock lock(operations_mutex);
 	for (int i(0); i < active_maps.size(); i++) {
 		active_maps[i]->sync();
 		active_maps[i]->step(p_delta_time);