A-Star Search Algorithm won't find a valid path

Question

I'm trying to implement an A* algorithm for pathfinding in my 3D grid. I've been following a tutorial but I'm not getting a valid path. I've stepped through my code to find out what's going on, but I don't know how to solve the problem. For the most basic test I'm just using a 2-D grid (it's 3-D, but there's only one Z option, so basically 2-D).

Here's what it's doing:

So we start at 0,0 (orange) and want to get to 1,2 (green). First it calculates the two options for the orange square, north and east, and gets distances of 2 and 1.414 for F values of 3 and 2.414. It moves to the east square (0,1). Great. But now it calculates the two open squares from 0,1 which are 1,1 and 0,2, both of which have a g value of 2 and an h value (distance) of 1, making their F values both be 3.

Since their F values are 3 and we already have an option with an F value of 3 (1,0 from the starting point), these two options are ignored even though they are clearly the best options.

It then continues onward and switches to moving to 1,0 where it then calculates 1,1 as 3 again and 2,0 as 4.236. 1,1's f value is not bigger than our current f value though, so it's ignored and we move upward to 2,0.

2,0 can only move right so it does.

2,1 can only move down since 2,2 is an invalid square, but the f value of moving to 1,1 is saved as 3, so it's again ignored, leaving us with no valid path between 0,0 and 1,2. What am I missing?

Here's a snippet of my path loop. There's a bunch of custom structs in here, and I'm using TMap from Unreal Engine to store my closed list, but I don't think that matters to the question. Here's a quick and dirty about what these structs are:

• PCell: Holds cell coordinates
• PPair: Holds cell coordinates as a PCell and an F value
• FVectorInt: 3-D integer vector
• FPathCell: Holds parent coordinates, and f, g, and h values.
• cellDetails is a 3D dynamic array of FPathCell
• closedMap is a TMap with <key, value> as <IntVector, bool>

Also locationIsWalkable(FVectorInt, StepDirection) is just code that checks to see if the player can walk to a cell from a certain direction. You can ignore that part.

    std::set<PPair> openList;
    PPair originPair = PPair();
    originPair.cell = PCell(i, j, k);
    originPair.f = 0.0;
    openList.insert(originPair);

    bool foundDestination = false;
    FPathCell destPair;
    FVectorInt destCell;

    while (!openList.empty() && !foundDestination)
    {
        iterations++;
        PPair p = *openList.begin();
        
        //Remove vertex
        openList.erase(openList.begin());

        //Add vertex to closed list
        i = p.cell.i;
        j = p.cell.j;
        k = p.cell.k;
        closedMap.Remove(FIntVector(i, j, k));
        closedMap.Add(FIntVector(i, j, k), true);

        double gNew, hNew, fNew;

        //Generate movement options
        //Option 1: NORTH (+X)
        //Process if valid movement
        if (locationIsWalkable(FVectorInt(i + 1, j, k), StepDirection::North))
        {
            FVectorInt check = FVectorInt(i + 1, j, k);

            //If this cell is the destination
            if (check == destination)
            {
                foundDestination = true;
                
                //Set the parent of the destination cell
                cellDetails[check.x][check.y][check.z].parent_i = i;
                cellDetails[check.x][check.y][check.z].parent_j = j;
                cellDetails[check.x][check.y][check.z].parent_k = k;
                destPair = cellDetails[check.x][check.y][check.z];
                destCell = check;
                break;
            }

            //Else if this cell is not in the closed list
            else if (!closedMap.FindRef(FIntVector(check.x, check.y, check.z)))
            {
                gNew = cellDetails[i][j][k].g + 1;
                hNew = calculateHValue(check, destination);
                fNew = gNew + hNew;

                if (cellDetails[check.x][check.y][check.z].f == FLT_MAX ||
                    cellDetails[check.x][check.y][check.z].f > fNew) {
                    
                    PPair cellPair = PPair();
                    cellPair.cell = PCell(check.x, check.y, check.z);
                    cellPair.f = fNew;
                    openList.insert(cellPair);

                    cellDetails[check.x][check.y][check.z].f = fNew;
                    cellDetails[check.x][check.y][check.z].g = gNew;
                    cellDetails[check.x][check.y][check.z].h = hNew;
                    cellDetails[check.x][check.y][check.z].parent_i = i;
                    cellDetails[check.x][check.y][check.z].parent_j = j;
                    cellDetails[check.x][check.y][check.z].parent_k = k;
                }
            }
        }

        //11 other movement options
    }

inline bool operator<(const PPair& lhs, const PPair& rhs)
{
    return lhs.f < rhs.f;
}

There's 12 movement options (north, south, east, west, up+north, down+north, etc.), but they basically all use the same code and just swap out the check vector for the appropriate movements.

Here's the tutorial I followed

Answer 1

Since their F values are 3 and we already have an option with an F value of 3 (1,0 from the starting point), these two options are ignored even though they are clearly the best options.

This must be your mistake. These options shall not be 'ignored', but rather 'delayed till they are the next-best options'. The way it's done is that on every iteration of A* you ought to select the open cell with the lowest F-score.

In your example, once you expand 0,1 (to get 0,2 and 1,1), your open set should look like:

(1,0):3   (1,1):3   (0,2):3

(It can also be any other permutation of these, because they have the same scores.)

Now imagine that it chooses to visit 1,0. It adds 2,0 to the queue, but 1,1 and 0,2 should still be there:

(1,1):3   (0,2):3   (2,0):4.236

Since 2,0 has a higher F-score than 1,1 or 0,2, it will not be chosen yet. Instead your algorithm shall pick 1,1 or 0,2 at this iteration, thus arriving at the destination 1,2.

As for your code, you're using an std::set for the openList, which prevents having multiple instances with the same score within the queue. You can use multiset or priority_queue to combat that. However, A* can decrease the weight of nodes in the open set, and neither data-structure allows that operation in sub-linear time. By inserting the same node multiple times (every time its score decreases), and ignoring any pops after it was closed, you'll still get a correct, though sub-optimal, algorithm.

Proper A* implementations usually use binomial or Fibonnacci heaps. Unfortunately C++ doesn't have them. You can find libraries implementing these on the web.