From 63dcb9aa80a2c77053033ed3c39b4fe5ed6f229b Mon Sep 17 00:00:00 2001
From: smix8 <52464204+smix8@users.noreply.github.com>
Date: Sun, 26 Jun 2022 12:43:01 +0200
Subject: Add NavigationPathQuery

Adds NavigationPathQueryParameters objects that can be used with NavigationServer.query_path() to query a customized navigation path.
---
 modules/navigation/godot_navigation_server.cpp | 26 ++++++++++++++++++++++++++
 modules/navigation/godot_navigation_server.h   |  2 ++
 2 files changed, 28 insertions(+)

(limited to 'modules/navigation')

diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index 9e5d666a51..8ca73a3adb 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -810,6 +810,32 @@ void GodotNavigationServer::process(real_t p_delta_time) {
 	}
 }
 
+NavigationUtilities::PathQueryResult GodotNavigationServer::_query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const {
+	NavigationUtilities::PathQueryResult r_query_result;
+
+	const NavMap *map = map_owner.get_or_null(p_parameters.map);
+	ERR_FAIL_COND_V(map == nullptr, r_query_result);
+
+	// run the pathfinding
+
+	if (p_parameters.pathfinding_algorithm == NavigationUtilities::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR) {
+		// while postprocessing is still part of map.get_path() need to check and route it here for the correct "optimize" post-processing
+		if (p_parameters.path_postprocessing == NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL) {
+			r_query_result.path = map->get_path(p_parameters.start_position, p_parameters.target_position, true, p_parameters.navigation_layers);
+		} else if (p_parameters.path_postprocessing == NavigationUtilities::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED) {
+			r_query_result.path = map->get_path(p_parameters.start_position, p_parameters.target_position, false, p_parameters.navigation_layers);
+		}
+	} else {
+		return r_query_result;
+	}
+
+	// add path postprocessing
+
+	// add path stats
+
+	return r_query_result;
+}
+
 #undef COMMAND_1
 #undef COMMAND_2
 #undef COMMAND_4
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index e6ef7e3bb1..ab5e722d35 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -175,6 +175,8 @@ public:
 
 	void flush_queries();
 	virtual void process(real_t p_delta_time) override;
+
+	virtual NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override;
 };
 
 #undef COMMAND_1
-- 
cgit v1.2.3