Correct way of combining A* and funnel

Question

I implemented a pathfinding algorithm like this:

• There is a mesh of triangles that defines the area the player can walk.

• An A* algorithm is used for finding a path from the start triangle to the end triangle.

• A funnel algorithm is used for optimizing the path returned by A*.

The result looks like this:

• The red lines mark the triangles of the mesh.

• The blue lines are the path returned by A*. The path connects the centres of the triangles that are found.

• The green lines are the path returned by the funnel algorithm.

In the screenshot above, it can be clearly seen that the green path is not optimal. The optimal path would be a straight line from start to end.

The blue path causes the problem. Since A* just uses the centres of the triangle as a heuristic to determine the length of the path from one triangle to the next, it thinks the best way would be to go through the three triangles below rather than the single triangle above. The blue path is then passed to the funnel algorithm, which only "sees" the triangles connected by the blue path; it cannot find the optimal path, because it is given the wrong set of triangles to work with.

So, I suppose the bug is caused by the heuristic of A* failing to determine the correct length. How to improve this? What is a better heuristic than just measuring the distances of the centres of the triangles?

Here's the main loop of the A* algorithm:

while ((pCurrent = removeOpenTriangWithLowestEstimatedScore()) != nullptr)
{
    if (pCurrent == pGoal)
    {
        break;
    }

    m_mapClosed[pCurrent] = true;

    for (const CTriangle *pNeigh : pCurrent->neighboursConst())
    {
        ASSERT (pNeigh != nullptr);

        if (isInClosedSet (pNeigh))
        {
            continue; // the neighbour has already been evaluated
        }

        if (!pNeigh->enabled())
        {
            m_mapClosed[pNeigh] = true;
            continue;  // the neighbour is disabled
        }

        auto itNeigh = findInOpenSet (pNeigh);

        if (itNeigh == m_lstOpen.end())
        {
            // Add pNeigh to the open set.
            tas.pT = pNeigh;
            tas.dScore = DBL_MAX;
            m_lstOpen.push_back (tas);
            itNeigh = m_lstOpen.end();
            itNeigh--;
        }

        // Estimate the distance from the neighbour to the goal.

        double dNeighToGoal = pNeigh->distanceOfCentres (*pGoal);

        if (dNeighToGoal < dNearestDist)
        {
            dNearestDist = dNeighToGoal;
            pNearestTriangle = pNeigh;
        }

        // Compute the actual score from start to the neighbour.

        double dScore = actualScore (pCurrent) + pCurrent->distanceOfCentres (*pNeigh);

        if (dScore >= actualScore (pNeigh))
        {
            continue; // this is not a better path
        }

        // This path is the best until now. Record it!

        m_mapCameFrom[pNeigh] = pCurrent;
        m_mapActualScore[pNeigh] = dScore;

        updateEstimatedScore (itNeigh, pNeigh, dScore + dNeighToGoal);
    }
}

And here's the main loop of the funnel algorithm:

for (it++; it != lstTriangles.end(); pPrev = *(it++))
{
    const CTriangle *pT = *it;
    ASSERT (pT != nullptr);
    ASSERT (pPrev != nullptr);

    getOrderedVertices (*pT, *pPrev, ptA, ptB, ptC);

    // ptA is the new vertex, ptB and ptC are shared by pPrev.

    if (isTurnedAway (ptApex, ptA, ptB))
    {
        ptApex = ptB;
        vecResult.push_back (ptApex);
    }
    else if (isTurnedAway (ptApex, ptC, ptA))
    {
        ptApex = ptC;
        vecResult.push_back (ptApex);
    }
}

Answer 1

Since A* just uses the centres of the triangle as a heuristic to determine the length of the path from one triangle to the next...

You don't have to implement A* this way. You can use discretized polygon-to-polygon distances to accumulate your path length g, but use the Euclidean distance to your actual destination point (not the center of its containing polygon) for your heuristic estimate h. This remains an admissible heuristic, because it never over-estimates the length of the shortest path to the destination.

(Since your polygons are convex, you can also treat the cost of the final leg as the distance to the final border edge plus the straight line distance from there to the destination, rather than going via the polygon's center)

Doing it this way, A* will prefer to explore the upper triangle first, because even though the polygon-to-polygon chain distances are the same on the upper or lower path, the heuristic value is smaller on the upper route.