Improve NavMeshQueries3D::polygons_get_closest_point_info performance

[kiroxas]

Changed the logic of the function polygons_get_closest_point_info in the NavMeshQueries3D class.

Instead of retriangulating each polygons, we iterate over each edge of the polygon, and check on which side lies the point we are looking for. If it's clockwise of all the sides it lies on the interior and we can just return the point. Otherwise, on the sides where it is on the exterior, we calculate the closest point for this edge and take the minimum of those.

All these timings are with the godot demo project navigation_mesh_chunks, with different sizes of chunks.

Timings

Those timings were wrong, there was an error in the code, check the post below for actual timings

(The smaller the chunkSize, the more polygons on the navmesh)
The new implementation has two benefits :

• less cycles per polygon which scales much better to huge navmeshes
• we early out if we find the point inside a polygon, which can reduce even further in some cases.

This is unclear if we want the early out or not, as if polygons do not have the same normal and the point we're searching does not belong to any plane where a polygon lies, the result could be wrong. If this is the desired case, we just need to remove the early break, and estimate the distance with a dot product between the point and the normal when we find it inside.

[kiroxas]

So it appears there was an error in the code, and it was missing one side check causing the early out to be taken when it should not.
This is fixed in current code, but timings are now less impacting than previously.

Timings

All the timings below are been taken using rdtsc. The lower, the better.

Previous Implementation with chunk size 256: 68 000 cycles
Previous Implementation with chunk size 32: 1 350 000 cycles

Edge Implementation with chunk size 256 : 56 000 cycles (x 1.25 )
Edge Implementation with chunk size 32 : 1 160 000 cycles (x 1.16 )

So, when the early out is not taken, we have between 15% and 25% improvement.
I also hardened the early out, to only be taken if the projected point lies into the plane, which will not change anything for 2D, but will give correct points for 3D.

TLDR: Expect a 15% speed up on points outside of the navmesh, and possibly a lot more if the point lies inside.

[smix8]

I did some testing and it seem to work fine and the speed up is noticeable on larger maps.

I would appreciate if more people could test it considering that in the past these kind of more math heavy optimization changes regularly made small edge case regresssions show up only very late.

I am still willing to merge not knowing if this will be the case because the performance gain is very good and this is a frequently used function in projects.

Just a fair warning note, because this is a hot function for some projects it especially needs to work reliable. In the worst case if things turn out broken in the RC stages we might need to revert this on a hurry.

Still needs a code review.

[akien-mga]

Thanks!