Here I am assuming that you already know what is going on with A* and heuristics, because it seems the problem is elsewhere.
I'm having trouble implementing the heuristic that computes the cost between nodes since the way the entity will move depends on the final path.
Yes, The funnel algorithm is carried out after the path is selected. We can only find the shortest path before path straightening.
The path on the right in the figure is indeed shorter than the path on the left:

The accuracy of the final path depends on how uniform the mesh is, the more evenly all paths are straightened, the more correct the result. For example the path on the right can be selected largely because of a particularly thin triangle in a suitable place:

If you change the split shape of this triangle so that it is not so thin, things will be much better:

There happens to be an algorithm that is often used for navmesh generation called Delaunay triangulation. It can improve the shape of triangles inside the mesh and avoid some edge cases.

And some modern pathfinding suites use convex polygons instead of triangles as pathfinding nodes, which reduces the number of nodes and also reduces the impact of malformed triangles. The Hertel-Mehlhorn Algorithm
is the standard approach for doing this.
In summary, you can:
- Improve the shape of the mesh during the navmesh generation phase(by re-triangulation), making it more uniform, which will reduce the difference in shortest paths before and after straightening.
- Artificially adding some vertices can also help.
- You can also use polygonal meshes instead of triangular meshes.
- Then use a heuristic function to find only the shortest path before straightening.
- Finally straighten the path.
Edit:
Why do I use midpoints of sides instead of triangle centers as pathfinding nodes in the example graph?
This is actually to reduce the error before and after the path is straightened. When a path passes through two adjacent triangles, after being straightened, it must pass through the common side of the two triangles. Its intersection with the common edge may be anywhere along the edge. To minimize the error before and after straightening the path, or try to make the error of different paths more stable, we can minimize the error of this intersection point.
This is an example. If the midpoints of adjacent sides are used as nodes, then the maximum error is |AB|/2
. If the center of the triangle is used as the node, then the maximum error is >|AB|
. Using midpoints of triangle sides as nodes gives more stable results.
