Improve pathfinding performance by using a heap to store traversable polygons

[ershn]

Description

Improves pathfinding performance by doing 2 things.

• Makes retrieval of already visited polygons constant time instead of linear time
• Uses a heap to store the polygons to visit instead of a linked list
  • To implement this, introduces a generic heap structure that notifies of element index changes and also allows to update an arbitrary element's position (the existing functions in SortArray don't allow for that)

Closes godotengine/godot-proposals#8496.

Changes in time complexity

n: number of polygons visited until now (navigation_polys)
m: number of polygons marked for later visiting (traversable_polys)

In every scenario, m increases at a much slower rate than n.

• An individual neighbor polygon check/registration/update goes from O(n) to O(log2(m)) (or O(1) if the polygon has already been visited and no update is needed)
• Retrieving the next polygon to visit goes from O(m) to O(log2(m))

These changes should result in a big performance improvement on big maps.

Test project

Navigation path finding tests.zip

Benchmark numbers

Below are execution times for NavigationServer*D.map_get_path before and after this PR on different map sizes.

Map size	Iteration count	Average time - master	Average time - PR	Speed-up
15x10 cells (small)	1000	0.047ms	0.026ms	x1.80 faster
90x50 cells (normal)	500	5.976ms	0.688ms	x8.68 faster
240x160 cells (big)	10	465.4ms	11.3ms	x41.1 faster

As you can see, the bigger the map, the bigger the speed-up.

• The measurements were done on a build made with scons platform=windows (no debug info)
• Done on Windows 10.0.19045 with an AMD Ryzen 7 2700X (4.3 GHz max, 8 core, 16 threads)
• You can find the maps in the test project linked above

Further improvements

There is room for further improvement here.

• Right now memory is allocated on the heap on each path request. It's rather wasteful but doesn't need to. We could keep a pool of arrays that would be reused each request.
• Finding the start and end polygon before the actual pathfinding is currently an O(n) operation. We will invariably loop over every polygon each request. We could use a specialized data structure to improve performance here.
• With the current API, even if we're interested only in paths that actually reach the target, a path to the closest reachable polygon will always be calculated if the target is unreachable. That's two pathfinding requests even if we only want one. We could add a new option/method to cover this use case. That one requires a proposal though.

I intend to work on these if there is no problem with this PR.

[ershn]

I realized that the current implementation doesn't take care of multithreading.
Trying to find a path from multiple threads on the same map at the same time will currently cause a crash (that makes pathfinding even faster though <3).

I'm turning this PR back into a draft until I fix the problem.

[ershn]

I changed the approach to be multithreading friendly. Pathfinding requests now leave Polygon objects untouched.

1. On map construction (NavMap::sync), assign each Polygon an id which is its index in the vector
2. On entering map_get_path, create a NavigationPoly vector of the same size as the one above
3. Use the Polygon id to find the corresponding NavigationPoly in the vector and vice versa

Since it's not possible anymore for the NavigationPoly vector to grow in size during pathfinding, I also changed it so that now pointers are stored in the heap instead of vector indexes. It improved readability and also slightly increased performance.

I added a multithreading test to the test project and also rebased on master (checked that #85238 is still fixed).

I will squash the commits if nothing major comes out during the review.

[smix8]

I suggest moving this heap to a navigation module internal class/struct instead of a core template.

I talked with some core devs and there is a preference for a more "local" solution that does not touch core. That is also the primary hold-up for this pr.

[smix8]

With the merge window for 4.4 now open I would like to get this in but it requires rebase and the mentioned move of the new heap class from core to the navigation module. I would suggest to move it to the nav_utils.h file in the gd namespace where everything else currently is that is used across the navsystem.

If you are still around @ershn and want to do these changes for the PR appreciated, else just let me know and I will salvage the PR and give you credit. Either way thank you for doing the main work on this optimization and all the performance testing.

[ershn]

Sorry for the delay. I moved the heap class to nav_utils.h, rebased on master, squashed the commits and checked that nothing broke after that.
I wasn't entirely sure where to put the new unit test file so feel free to change that if necessary.

Thank you for the review smix8 !

[smix8]

I wasn't entirely sure where to put the new unit test file so feel free to change that if necessary.

@Scony is usually the test maniac ... ;)
... but I would say those tests would belong to the tests/servers/test_navigation_server_3d.h file but you can imo also just leave it in that separated file as is for now.

EDIT

@ershn I tested the rebased PR and while it works fine in the provided test project I am getting some random heap and polygon.id index crashes in other projects that use a NavigationAgent2D/3D or the NavigationPathQueryParameters2D/3D on ready.

ERROR: FATAL: Index p_index = 4294967295 is out of bounds (count = 77).
   at: LocalVector<struct gd::NavigationPoly,unsigned int,0,0>::operator [] (H:\files\github\godot\ershn_godot\core/templates/local_vector.h:177)

================================================================
CrashHandlerException: Program crashed
Engine version: Godot Engine v4.4.dev.custom_build (d3000d3640740127df8049a08978e9d5b74c46fb)
Dumping the backtrace. Please include this when reporting the bug to the project developer.
[0] LocalVector<gd::NavigationPoly,unsigned int,0,0>::operator[] (H:\files\github\godot\ershn_godot\core\templates\local_vector.h:177)
[1] NavMap::get_path (H:\files\github\godot\ershn_godot\modules\navigation\nav_map.cpp:273)
[2] GodotNavigationServer3D::_query_path (H:\files\github\godot\ershn_godot\modules\navigation\3d\godot_navigation_server_3d.cpp:1377)
[3] NavigationServer3D::query_path (H:\files\github\godot\ershn_godot\servers\navigation_server_3d.cpp:941)
[4] NavigationAgent3D::_update_navigation (H:\files\github\godot\ershn_godot\scene\3d\navigation_agent_3d.cpp:778)

The TileMap does a deferred setup so that may be why it works in the TileMap test project but crashes in other projects without TileMap? I tested both 2D and 3D and a new project, as soon as a NavigationAgent queries on the first frame it has like a 1 in 3 chance of causing the index crash with the get_path() function. Since the crash even reports the right polygon count something seem to stop the ids from being assigned or do they overflow? That some suspicious FLT_MAX error index.

[ershn]

Thank you for the tests !

The bug probably happened because the synthetic polygons generated for navigation links were not being taken care of during map sync. Their id was never updated from the default value of UINT32_MAX which resulted in invalid array accesses when trying to process them during pathfinding.

This should be fixed now. Could you test again with your test scenes ?

but you can imo also just leave it in that separated file as is for now

Ok, I'll leave the tests there for the time being then.

[smix8]

This should be fixed now. Could you test again with your test scenes ?

@ershn Yeah that was it, no more crashes or errors (so far 😝).

I fear this PR might need another rebase soon as it will collide with the other PRs that are currently in the 4.4 merge queue depending on what gets merged first, brace for impact!

[AThousandShips]

I'd say this should be moved into the module as it, unlike the other tests, is module specific

[ershn]

Ok. I moved it into tests/servers/test_navigation_server_3d.h (like smix8 suggested) if that's what you meant.

[AThousandShips]

I meant it should be moved to modules/navigation/tests, but I think that's okay too

[Scony]

In my view, the heap implementation is generic enough so that should placed be within core/templates. @smix8 who are the "core devs" that suggested otherwise?

If not core/templates then I don's see a reason why this fairly-generic class wouldn't be placed in some separate file like modules/navigation/heap.h or so. Same goes for tests - test_navigation_server_3d.h which is actually a bunch of module tests is not a good place for heap unit tests. I'd at least extract it to tests/servers/test_navigation_server_3d_heap.h and include in tests/servers/test_navigation-server_3d.h.

[smix8]

In my view, the heap implementation is generic enough so that should placed be within core/templates. @smix8 who are the "core devs" that suggested otherwise?

That was when I asked reduz with 1-2 other core people around for a review on this PR months ago. They all said this should be local to the navigation module and not part of core which is why I made that comment back in April to unblock this PR.

Sure it could be its own file, same for the tests, I don't mind either. Just don't want to drive ershn crazy with back and forth changes. We also could do the changes in a follow up PR if you want to reorganize some files.

[Scony]

Ok, let's do the refactoring in the followup PR.

[kleonc]

In my view, the heap implementation is generic enough so that should placed be within core/templates.

@Scony See this. Currently it's used only by the navigation so adding to core is not justified. If something else will need it as well then it will probably be fine to move it to core.
(I'm not a core team member but IIRC that's the reasoning.)

[akien-mga]

Thanks!