Simplify NavMeshQueries3D::_query_task_build_path_corridor

[kiroxas]

Add improvements discussed in this PR.

Added distance_squared instead of distance.
Moved some declarations outside the inner loop.
Collapsed the insertion cases into the same branch.

[smix8]

Looks promising. Needs a rebase now that #100823 has been merged.

[smix8]

Tested this PR and sorry to report that it breaks the pathfinding or at least the current established behavior.

When the target polygon is not reachable, e.g. a region is disabled between start and target polygons, the path with this PR gets frequently stuck on the current polygon. In master it picks the polygon closest to the target that can be reached.

[kiroxas]

Tested this PR and sorry to report that it breaks the pathfinding or at least the current established behavior.

When the target polygon is not reachable, e.g. a region is disabled between start and target polygons, the path with this PR gets frequently stuck on the current polygon. In master it picks the polygon closest to the target that can be reached.

Do you have a test case you can share for this ? So I can look at what's wrong with it.

[kiroxas]

Tested this PR and sorry to report that it breaks the pathfinding or at least the current established behavior.

When the target polygon is not reachable, e.g. a region is disabled between start and target polygons, the path with this PR gets frequently stuck on the current polygon. In master it picks the polygon closest to the target that can be reached.

Forgot to reset some values in the branch where we cannot reach the desired end. Tested it with your test case, and I now have the same behaviour as master. Thank you for testing and catching this.

[akien-mga]

Thanks!

[novhack]

Hi! My navigation script suddenly started outputting thousands of these errors and it seems that it might be caused by changes in this PR. This wasn't happening with Godot master build from yesterday.

E 0:00:21:0068   target_navigation.gd:31 @ _physics_process(): Heap element index is out of range.
  <C++ Error>    Index p_index = 4294967295 is out of bounds (_buffer.size() = 33).
  <C++ Source>   modules/navigation/3d/../nav_utils.h:241 @ shift()
  <Stack Trace>  target_navigation.gd:31 @ _physics_process()

The navigation itself doesn't seem to be broken though.

The navigation script in question is very basic: https://gist.github.com/novhack/495c32bb99b3e3ba44539e442935c87d

EDIT: I locally reverted this PR and all errors are gone.

[kiroxas]

Hi! My navigation script suddenly started outputting thousands of these errors and it seems that it might be caused by changes in this PR. This wasn't happening with Godot master build from yesterday.

E 0:00:21:0068   target_navigation.gd:31 @ _physics_process(): Heap element index is out of range.
  <C++ Error>    Index p_index = 4294967295 is out of bounds (_buffer.size() = 33).
  <C++ Source>   modules/navigation/3d/../nav_utils.h:241 @ shift()
  <Stack Trace>  target_navigation.gd:31 @ _physics_process()

The navigation itself doesn't seem to be broken though.

The navigation script in question is very basic: https://gist.github.com/novhack/495c32bb99b3e3ba44539e442935c87d

EDIT: I locally reverted this PR and all errors are gone.

Thank you for reporting this, and sorry for the incovenience. Looking at the code, I think I know where this is coming from. I'll draft something to fix this.

[kiroxas]

@novhack Sorry for the inconvenience, but if you could try this PR, to check if it fix the issue for you, that would be helpful.

[novhack]

No need to apologize I'm happy to help and I really appreciate your work. I'll get to it as soon as possible. :)

[novhack]

Tested and your new PR seems to have solved the issue. :)