Add NavigationPathQuery objects and NavigationServer query_path()

[smix8]

Adds NavigationPathQueryParameters and NavigationPathQueryResult objects in 2D and 3D similar to e.g. physics query parameters and results.

Can be used with 2D or 3D NavigationServer query_path() to get a customized navigation path considering additional options.

Does not affect the normal pathfinding with NavigationAgent nodes or NavigationServer.map_get_path() at all. Without customized options returns the same path as map_get_path() for now.

Removed for this pr most other options that are still developed and not finished yet (like #2527 or #3812). If there is only a single function or an enum has only a single value that is why. This is basically only the core left as many other pr's and features will depend on this. This query can be used as a base to fix issues #19011 and #60277 in the future by adding a property that controls how the pathfinding adds points or a simplifying method in path postprocessing.

This pr is also a prerequisite for #62115 or any other issue that is related to the currently used pathfinding algorithm and forced path post-processing. Since Godot seems that it wants to support both 2D and 3D pathfinding as free-form as well as tile / grid based with TileMap and GridMap (#61293) it is impossible to have just one pathfinding algorithm and one out of two forced path post-processing for everything like it is rightnow.

Usage is to create both objects first and store them into a var for reuse. Then use query_path() for updating the content of the result.

Example:

var query_parameters : NavigationPathQueryParameters3D = NavigationPathQueryParameters3D.new()
var query_result  : NavigationPathQueryResult3D = NavigationPathQueryResult3D.new()

# fill in parameters...

NavigationServer3D.query_path(query_parameters, query_result)
var path = query_result.get_path()

[akien-mga]

CC @AndreaCatania @Scony @lawnjelly

[lawnjelly]

Have written some comments. But really before adding this PR I think the navigation team should decide whether they are going to add async, and if so add this first because the API and implementation of all this stuff may need to be different if supporting async pathfinding.

[lawnjelly]

Here is an illustration of a problem of using the dot product to detect straight regions. A and B the dot product in each case is similar as the vectors are normalized. However the absolute error (the blue line) is huge when the distances between the waypoints is higher, which can result in agent walking into impassable area, getting stuck etc.

Alternatives: See Geometry::get_closest_point_to_segment_2d() and similar routines. You can use this to find the blue distance. Or just wait for navmesh ray tracing routine and use that to exactly eliminate path points without traversing over the edge of the navmesh.

Also, it might be worth considering whether to have the pathfinding method immediately in the query if there is only one supported so far. Maybe that could be added as part of the adding a separate method (because it might be used in a different way).

Another point is that with navmeshes, it is very common to have the agent keep track of which navpoly it is on. You could pass this along with the query, or a -1 or something if not known. This can greatly speed up things, because finding the nearest navpoly to a point can be a slow step. You can also do this for destinations, for instance if you are querying a path to a known location on a known navpoly.

If using NavPhysics, this information will be available on each nav agent (for free) as it is required for the physics to work.

[lawnjelly]

Overall it is getting there, just a few nitpicks above. :+1:

Probably most important is to ensure there are no possible race conditions when accessing the result (it depends how the two areas are synchronized, whether they are on separate threads etc). If there could be, you might need something like a (global?) mutex to prevent deletion while it is being filled.

[DarkKilauea]

I was looking around other game engines and I have an idea for the query_path API.

void query_path_async(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Callable p_query_result_callback);

The idea here is to put the callback into the method signature of query_path instead of placing on the result object that is pre-allocated. Then, the caller would provide both the parameters and the callback to get the result.

This is also more similar to how the physics APIs work, though they are purely synchronous. I'm a little torn on whether we should provide both a synchronous and asynchronous API, since I'm not sure all of the machinery for asynchronous path calculation is really worth it for most games. Right now, my PC can do thousands of path calculations at >60 FPS with a synchronous API.

For simple games, they may actually get a performance bump by opting out of the async API and avoiding the overhead.

The sync API would look like:

Ref<NavigationPathQueryResult3D> query_path_sync(const Ref<NavigationPathQueryParameters3D> &p_query_parameters) const;

[DarkKilauea]

I'd be really tempted to drop the asynchronous support here and push out a basic synchronous API similar to how the physics server works today. We know this paradigm works for current Godot games and may help unblock this PR and give us time to figure out the asynchronous logic later.

We can always add support later with a new method.

[lawnjelly]

I was looking around other game engines and I have an idea for the query_path API.

void query_path_async(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Callable p_query_result_callback);

The idea here is to put the callback into the method signature of query_path instead of placing on the result object that is pre-allocated. Then, the caller would provide both the parameters and the callback to get the result.

Yes I actually agree here. I'm not sure I understand the whole sending the result object? Is this a misunderstanding from one of my earlier comments?

I agree with the API suggested by @DarkKilauea , bar the problem of callable not being present in 3.x (I don't know whether we are planning on keeping these in sync). This is why I suggested to @smix8 to copy the example of the avoidance with the signal, as it is essentially doing the same thing.

If necessary, we could drop async from this PR, it may be quicker for me or another contributor to add it as it requires some familiarity with threading.

[smix8]

Removed the callback and processqueue from the pr, basically only the navquery core left now.

[DarkKilauea]

I believe we should move the query parameter and result objects next to the navigation server itself, instead of putting them into the module.

1. I want to be able to use them inside NavigationAgent without having to disable functionality is the module is disabled. This is particularly needed to make working with navigation links easier and more intuitive. Right now, there isn't a way to trigger when an agent enters a link and I'd like that capability to always be there.
2. We should define a minimum set of query features that is required for other navigation nodes to work. It's ok if future server implementations add features, but they should all implement the base set. Moving the types next to the server would allow other implementations to derive from our base parameters and result objects and then add their new features, with a cast used on the consumption side to gain access.

[smix8]

I agree but that module issue is not simply only for the navquery.

The current situation is that nearly every navigation pr that adds something new requires a lot of ifdef guards to even pass the recently added git check for Godot minimum build which is very annoying because the navigation module is in no state to be disabled as shown by issues like https://github.com/godotengine/godot/issues/36091 or the recent discussions on rocketchat (with no real solution) how to even properly disable such an intertwined module like navigation. Just look at this filechange mess currently required for nonavigation builds to not break in Godot https://github.com/godotengine/godot/pull/65562/files.

If the navigation module folder is intended to be only for the godot_navigation_server backend implementation that basically means we a) need a dummy server for navigation similar to physics and b) the entire stuff that has general use like the navigationmeshgenerator or the navquery stuff needs to be moved from the module navigation to a server navigation folder.

[smix8]

Changed the folder structure and moved the parameters / results from the godot navigation module folder to "servers/navigation" as they are considered a part of the NavigationServer that needs to be available for other classes or server implementations that replace the godot server module.

[DarkKilauea]

I did some bench-marking to try to understand the performance difference between this PR and #65680.

Code:

var nav_map_rid := get_world_2d().navigation_map;
var start_location := NavigationServer2D.map_get_closest_point(nav_map_rid, start.global_position);
var target_location := NavigationServer2D.map_get_closest_point(nav_map_rid, end.global_position);

var result := NavigationPathQueryResult2D.new();
var query := NavigationPathQueryParameters2D.new();
query.map = nav_map_rid;
query.start_position = start_location;
query.target_position = target_location;

for i in 10:
	var average_buffer := PackedInt32Array();
	
	var current_run_time = run_time_seconds;
	while(current_run_time >= 0.0):
		var cur_time := Time.get_ticks_usec();
		
		NavigationServer2D.query_path(query, result);
		
		var end_time := Time.get_ticks_usec();
		var duration := end_time - cur_time;
		var duration_in_sec := duration * 0.000001;
		
		current_run_time -= duration_in_sec;
		average_buffer.push_back(duration);
	
	var average_duration := 0;
	for dur in average_buffer:
		average_duration += dur;
	
	average_duration /= average_buffer.size();
	print("Average query time: %s usec" % average_duration);

Build Info:

> scons target=release_debug
scons: Reading SConscript files ...
Automatically detected platform: windows
Auto-detected 16 CPU cores available for build parallelism. Using 15 cores by default. You can override it with the -j argument.
Found MSVC version 14.3, arch x86_64
Building for platform "windows", architecture "x86_64", editor, target "release_debug".

Results:

Godot Engine v4.0.alpha.custom_build.489922a0e - https://godotengine.org
Vulkan API 1.2.0 - Using Vulkan Device #0: NVIDIA - NVIDIA GeForce RTX 2080

Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec
Average query time: 20 usec

See #65680 for details about the testing done there, but the short version is that this PR is about 13.04% (3 usec) faster.

[DarkKilauea]

Sorry for the sudden comment spam, didn't notice that GitHub does not post your comments until you "finish your review". After playing with this PR for a while, I'm happy with its overall shape. I left a few comments for improvement, but these can be completed later.

[smix8]

Removed the path len calculation and the "optimize" bool parameter that switched between the two available postprocessing options in map_get_path(). Added path postprocessing enum as a replacement for the outdated "optimize" parameter.

[DarkKilauea]

I'm happy with this PR. It should be good to merge.

[akien-mga]

Thanks!