Change the A* heuristic for navigation to be admissible, guaranteeing the shortest path across regions and links with varied travel cost

[sandygk]

Describe the project you are working on

This would affect any project with navigation with regions or links with different travel costs that need to compute the actual shortest path.

Describe the problem or limitation you are having in your project

Currently, the heuristic used in the implementation of the A* algorithm to find the shortest path for navigation agents is not admissible. This means the heuristic function may overestimate the cost to get to the goal, which causes the algorithm to not return the optimal. As a result, the travel cost of regions and links are ignored in the majority of cases.

In the A algorithm, the cost is computed as f(n)=g(n)+h(n) where g(n) is the cost already computed and h(n) is the heuristic cost from the node n to the goal. This heuristic cost should not exceed the actual cost from n to the goal, otherwise the A can't ensure the minimum cost path.

In the current implementation,h(n) is added to the cost in the following line:

https://github.com/godotengine/godot/blob/5ee983188de97ae027f9b9c1443438063f708a7e/modules/navigation/nav_map.cpp#L364

Notice how the travel cost of the current polygon is used to estimate the cost to the goal in the heuristic. This is incorrect since the current polygon could have a high travel cost and there could be the shortest path using a different Nav Region or Nav Link. See the following images where the path is optimal path is ignored due to this issue:

Scene 4 of the attached project: it goes directly to the target instead of using the link

Scene 5 of the attached project: it goes directly to the target instead of using the Nav Region with Travel Cost 2

Describe the feature / enhancement and how it helps to overcome the problem or limitation

What I suggest is to change the heuristic to be admissible, to achieve this, instead of multiplying the distance to the target by the travel cost of the current polygon, we can multiply it by the minimum travel cost in the map. Or, if we want to optimize it further, we can multiply it instead by the minimum travel cost of the reachable Nav Regions and Nav Links in the map. With this change, the above-mentioned examples yield the right result (I tested locally).

Describe how your proposal will work, with code, pseudo-code, mock-ups, and/or diagrams

This line will change from:

https://github.com/godotengine/godot/blob/5ee983188de97ae027f9b9c1443438063f708a7e/modules/navigation/nav_map.cpp#L364

To either:

option 1)

cost += (np->entry.distance_to(end_point) * min_travel_cost;

min_travel_cost will be computed on the sync method, when we iterate over all nav regions and links.

option 2)

cost += (np->entry.distance_to(end_point) * min_reachable_travel_cost;

min_reachable_travel_cost will be computed on the get_path method, by doing a DFS over all reachable Nav regions and Links

If this enhancement will not be used often, can it be worked around with a few lines of script?

no

Is there a reason why this should be core and not an add-on in the asset library?

This is an improvement on an already establish module

[sandygk]

I am more than happy to implement this change once we decide what option we want to go with.

[Calinou]

• Related to https://github.com/godotengine/godot-proposals/issues/8496.