Implementing A*: The Priority Queue & Path Reconstruction

Time to turn the theory into working code. The open set needs to always hand back its lowest-f cell, exactly what std::priority_queue is for.

Neighbors

std::vector<GridPos> getNeighbors(GridPos p) {
    std::vector<GridPos> result;
    const int dr[] = {-1, 1, 0, 0};
    const int dc[] = {0, 0, -1, 1};

    for (int i = 0; i < 4; i++) {
        int nr = p.row + dr[i];
        int nc = p.col + dc[i];
        if (nr >= 0 && nr < GRID_ROWS && nc >= 0 && nc < GRID_COLS) {
            result.push_back(GridPos{nr, nc});
        }
    }
    return result;
}

int encode(GridPos p) { return p.row * GRID_COLS + p.col; }

getNeighbors checks the four 4-directional cells and only keeps ones still inside the grid's bounds. encode flattens a 2D GridPos into one int, so it can be used as a key in a hash map, std::unordered_map<GridPos, ...> would need a custom hash function, an int key sidesteps that entirely.

The search

std::vector<GridPos> findPath(GridPos start, GridPos goal) {
    auto cmp = [](const std::pair<float, GridPos>& a, const std::pair<float, GridPos>& b) {
        return a.first > b.first; // smallest f comes out first
    };
    std::priority_queue<std::pair<float, GridPos>, std::vector<std::pair<float, GridPos>>, decltype(cmp)> openSet(cmp);

    std::unordered_map<int, float> gScore;
    std::unordered_map<int, GridPos> cameFrom;
    std::unordered_set<int> closedSet;

    gScore[encode(start)] = 0.f;
    openSet.push({heuristic(start, goal), start});

    while (!openSet.empty()) {
        GridPos current = openSet.top().second;
        openSet.pop();

        if (current == goal) {
            return reconstructPath(cameFrom, current);
        }

        if (closedSet.count(encode(current))) continue; // stale entry, a better one was already processed
        closedSet.insert(encode(current));

        for (GridPos neighbor : getNeighbors(current)) {
            if (grid[neighbor.row][neighbor.col] == CellType::Wall) continue;
            if (closedSet.count(encode(neighbor))) continue;

            float tentativeG = gScore[encode(current)] + 1.f;
            int key = encode(neighbor);

            if (!gScore.count(key) || tentativeG < gScore[key]) {
                gScore[key] = tentativeG;
                cameFrom[key] = current;
                float f = tentativeG + heuristic(neighbor, goal);
                openSet.push({f, neighbor});
            }
        }
    }

    return {}; // no path exists
}

A couple of details worth pausing on:

• std::priority_queue is a max-heap by default, the custom cmp flips the comparison (a.first > b.first) so the smallest f comes out first instead.
• The same GridPos can be pushed onto openSet more than once, if a cheaper path to it is found after it's already queued. The closedSet.count(...) check right after popping catches and skips these stale, already-superseded entries instead of reprocessing them.
• tentativeG = gScore[current] + 1.f assumes every step costs exactly 1, true for a uniform grid with no difficult terrain, this is the g(n) from the last lesson, built up incrementally as the search progresses.

Reconstructing the path

std::vector<GridPos> reconstructPath(std::unordered_map<int, GridPos>& cameFrom, GridPos current) {
    std::vector<GridPos> path;
    path.push_back(current);

    int key = encode(current);
    while (cameFrom.count(key)) {
        current = cameFrom[key];
        key = encode(current);
        path.push_back(current);
    }

    std::reverse(path.begin(), path.end());
    return path;
}

cameFrom[key] was recorded every time a cheaper path to a cell was found, so walking it backward from the goal traces the actual shortest path, one predecessor at a time, all the way back to the start. Since that walk naturally produces the path backward (goal to start), std::reverse flips it into the order an enemy will actually walk it.