A* algorithm implementation in modern C++

Question

I've written the A* search algorithm in C++. My goal is primarily writing concise and understandable code, while making use of some new features of modern C++ (where appropriate) and having generally good performance (importance of these goals in the order they were mentioned).

The goal of the algorithm is to find the best (cheapest) path from a starting node to a target node on a graph. The input to the algorithm is an adjacency list, start and target node IDs and an (admissible and consistent) heuristic function that computes the estimated distance of two nodes. The adjacency list is a map (NodeID -> map (NodeID -> Cost)) and describes the edges in the graph and their cost. The adjacency list is assumed to be symmetric, i.e., adjacency[x][y] == adjacency[y][x].

On a high level, the algorithm works as follows:

1. Put the starting node into a priority queue boundary. This queue contains the set of not-yet-visited nodes that are connected to at least one already visited node by an edge
2. If boundary is empty, return the empty path
3. Take out the entry v from boundary with the least cumulative cost
4. If this node is the target node, reconstruct the path and return
5. Add all neighbors of node v to boundary that are not yet visited
6. Go to step 2

Do you have recommendations on how to improve the following code with regard to my above-mentioned goals? I'm specifically wondering whether the boundary.push part could be written more concise without a for loop. What other features of modern C++ did I miss that would improve this code? If you spot some issues with this code, feel free to mention them.

---

Header file (a_star.h):

#include <functional>
#include <map>
#include <vector>

typedef size_t NodeID;
typedef float Cost;
typedef std::pair<const NodeID, Cost> Edge;
typedef std::map<const NodeID, std::map<const NodeID, Cost>> AdjacencyList;
typedef std::vector<NodeID> Path;
typedef const std::function<Cost(NodeID, NodeID)> HeuristicFn;


Path a_star(AdjacencyList& adjacency,
            NodeID         start,
            NodeID         target,
            HeuristicFn&   heuristic);

Source file a_star.cpp:

#include "a_star.h"

#include <memory>
#include <numeric>
#include <queue>
#include <ranges>
#include <set>

struct PathNode {
    NodeID node; // node
    Cost total_cost; // cost of reaching node from starting node
    Cost estimated_cost; // lower bound on the cost for any path from starting to target node through this node
    std::shared_ptr<PathNode> previous; // previous record
};

bool operator>(const PathNode& left, const PathNode& right) {
    return left.estimated_cost > right.estimated_cost;
}

Path backtrace(PathNode& last_record) {
    PathNode current = last_record;
    Path path {current.node};
    while (current.previous != nullptr) {
        current = *current.previous;
        path.emplace_back(current.node);
    }
    std::reverse(path.begin(), path.end());
    return path;
}

Path a_star(AdjacencyList& adjacency,
            NodeID         start,
            NodeID         target,
            HeuristicFn&   heuristic) {
    std::set<NodeID> visited;
    std::priority_queue<PathNode, std::vector<PathNode>, std::greater<>> boundary;

    boundary.push({start, 0, heuristic(start, target), nullptr});
    visited.emplace(start);

    auto was_not_visited_yet = [visited] (Edge& entry) {
        return !visited.contains(entry.first);
    };

    for (; !boundary.empty(); boundary.pop()) {
        auto current = boundary.top();
        if (current.node == target) {
            return backtrace(current);
        }
        visited.emplace(current.node);

        auto edge_to_record = [current, heuristic, target] (Edge& edge) -> PathNode {
            Cost total_cost = current.total_cost + edge.second;
            Cost estimated_cost = total_cost + heuristic(current.node, target);
            return {edge.first, total_cost, estimated_cost, std::make_shared<PathNode>(current)};
        };

        auto neighbor_records = std::ranges::views::filter(adjacency[current.node], was_not_visited_yet)
                                | std::ranges::views::transform(edge_to_record);
        for (const PathNode& neighbor_record : neighbor_records) {
            boundary.push(neighbor_record);
        }
    }
    return {}; // no path to target
}
```

Answer 1

Missing and redundant const

Make sure that all pointer and reference parameters that point to data that should not be modified are passed as const. This includes last_record,adjacency and edge in the lambda. Note that you cannot use operator[] on adjacency anymore, instead use .at().

On the other hand, you used const in places where it is not necessary. For example, you don't have to explicitly make the first template parameter of std::map const, as it will do that for you, see std::map::value_type.

Range algorithms vs. good old for and if

Functional style programming can be really nice in some cases, but here it actually does not provide any benefits, and I think it's just simpler to write:

for (auto& [neighbor, cost]: adjacency.at(current.node)) {
    if (!visited.contains(neighbor))
        boundary.push(edge_to_record({neighbor, cost}));
}

Unnecessary use of std::shared_ptr

A std::shared_ptr is a rather heavy-weight pointer. Also, every time you call std::make_shared<current> you are making a heap-allocated copy of current, potentially multiple times for the same value of current. This is not ideal.

Instead, consider changing visited so it is a set or map of PathNodes. Since std::set, std::map and similar containers ensure the items they contain have stable pointers, you can then just store a raw pointer in PathNode and have it safely point to any node already in visited.

But do you need pointers at all? In the end you only care about the NodeIDs. So why not make PathNode::previous be a NodeID instead of a (smart) pointer?

Think about your data structures

There are some concerns about your use of std::set and std::map. First of all, these containers have \$O(\log N)\$ complexity for adding and finding entries. If you don't need the order of the elements to be preserved, you can instead use std::unordered_set and std::unordered_map for faster access times. But sometimes you don't even need to look up specific elements. For example, given a node, you don't care about looking up a specific neighbor, you only want to iterate over all its neighbors. So you could have written:

typedef std::unordered_map<NodeId, std::vector<Edge>> AdjacencyList;

These generic containers are great if you don't know much about NodeId, or if they can have any value. But if you know that NodeIds are unsigned numbers that are assigned sequentially starting from zero, and there are no gaps, then you can just use NodeId as an index into an array or vector. So:

typedef std::vector<std::vector<Edge>> AdjacencyList;

And this is the point where Davislor's comment that an adjacency matrix might be more efficient suddenly makes a lot more sense.

In this case, you could also replace std::set<NodeId> visited with a std::vector<bool> visited, which only uses one bit per node in the graph, and only requires one memory allocation to ever be made, whereas std::set will allocate memory for each individual item you store in it.

Having more compact datastructures reduces memory bandwidth and improves the efficiency of your cache. Having datastructures that have \$O(1)\$ operations instead of \$O(\log N)\$ ones will greatly speed up your algorithm for large graphs.

Answer 2

As I understand your code, a new node is pushed for every unvisited neighbour of the node that is visited.

A result of that is that the queue will contain, in many cases, duplicate nodes, or "almost duplicate" nodes with the same ID but a different previous and different total_cost. These duplicates are bad. If their estimated_cost is high, all they do is clog up the queue, but their estimated cost doesn't need to be high. The same part of the graph can be explored multiple times, even though only unvisited neighbours are pushed, because it would be pushed again before it has been visited.

There are some things you can do to avoid this. A simple trick is, when getting the top node out of the queue, skip it if it had been visited. That way some duplicates still clog up the queue, but at least they're not redundantly processed, and some duplicates are prevented by not processing useless nodes.

Another technique is to prevent duplicates from ever being created, but you need different data structures for that: a priority queue that can be efficiently indexed by node ID. You can make that with a binary heap that also maintains a map from node ID to index in the heap.