A-Star for 2D Graph (not grid)

Question

This is my A-Star implementation for a 2D space of nodes but allowing to specifically set connections between nodes because my usecase does not have points tied to a grid (however for clarification reasons, the sample data down there does form a 3x3 grid with non-diagonal connections).

So basically it is A-Star on a graph with using euclidean distance as weights for moving between nodes, I believe.

    #include <iostream>
    #include <algorithm>
    #include <vector>
    #include <set>
    #include <cmath>
    #include <string>
    #include <limits>

struct Point {
    Point(int x = 0, int y = 0) : x(x), y(y) {};
    bool operator ==(const Point& o) const { return o.x == x && o.y == y; }
    int DistTo(Point & other) const { return(std::sqrt(std::pow(other.x - x, 2) + std::pow(other.y - y, 2))); };
    int x, y;

    void Print(std::string name) const
    {
        std::cout << name << " x: " << x << " y: " << y << std::endl;
    }
};

struct Node
{
    Node(Point position = Point(), Node * parent = nullptr) : position(position), parent(parent) {};

    Point position;

    int dist_to_start = 0;
    int total_dist = INT_MAX;

    std::shared_ptr<Node> parent;
    std::shared_ptr< std::vector<Node> > connections{ new std::vector<Node> };

    bool operator ==(const Node& o) const { return (o.position.x == position.x) && (o.position.y == position.y); }
    bool operator <(const Node& o) const { return (total_dist < o.total_dist); }

    void AddConnection(Node subnode)
    {
        connections->push_back(subnode);
    }
};


std::vector<Point> Astar(Point & start_pos, Point & end_pos, std::vector<Node> all_nodes)
{
    // Find the start and end nodes
    Node start_node;
    Node end_node;

    for (Node & node : all_nodes)
    {
        if (node.position == end_pos) // start and end swaped here because A-Star traces from end to start
            start_node = node;
        if (node.position == start_pos)
            end_node = node;
    }


    std::vector<Point> found_path; // Output path


    std::set<Node> open_list; // Using set to be ordered
    std::vector<Point> closed_list;

    open_list.emplace(start_node);


    while (!open_list.empty())
    {
        // Use node with lowest total_dist (the set is always sorted)
        Node current_node = *open_list.begin();

        // Remove smallest and add to closed list
        open_list.erase(open_list.begin());
        closed_list.push_back(current_node.position);


        // Found the goal
        if (current_node.position == end_node.position)
        {
            // Found the goal
            Node * parent = &current_node;

            // Jump from parent to parent to trace back
            while (parent != nullptr)
            {
                found_path.push_back(parent->position);
                parent = parent->parent.get();
            }
            return(found_path);
        }

        for (Node& node : *current_node.connections)
        {
            Node child(node);
            child.parent.reset(new Node(current_node));

            // Child is on the closed list
            if (std::find_if(closed_list.begin(), closed_list.end(), [&child](Point & pt) { return(pt == child.position); }) != closed_list.end())
                continue;

            // Create the total_dist, dist_to_start, and heurist_dist_to_end values for the new subnode

            // Calculated the total distance to the start by adding the distance from the subnode to the current node.
            // Note: This distance could be precomputed and just held together with the "connections"
            child.dist_to_start = current_node.dist_to_start + child.position.DistTo(current_node.position);


        // Total cost is the sum of past distance and the heuristic (which assumes just line-of-flight)
        child.total_dist = child.dist_to_start + child.position.SqrDistTo(end_node.position);

            // Child is already in the open list
            if (std::find_if(open_list.begin(), open_list.end(), [&child](const Node & node) { return((node == child) && (child.dist_to_start > node.dist_to_start)); }) != open_list.end())
                continue;

            // Add the child to the open list
            open_list.emplace(child);
        }

    }

    return(found_path);
}

Main method with test data (forms a 3x3 grid with non-diagonal connections):

 int main()
 {
    std::vector<Node> all_nodes;

    // Nodes

    Node nd_0_0{ Point{0,0} };
    Node nd_1_0{ Point{1,0} };
    Node nd_2_0{ Point{2,0} };

    Node nd_0_1{ Point{0,1} };
    Node nd_1_1{ Point{1,1} };
    Node nd_2_1{ Point{2,1} };

    Node nd_0_2{ Point{0,2} };
    Node nd_1_2{ Point{1,2} };
    Node nd_2_2{ Point{2,2} };


    // Connections

    nd_0_0.AddConnection(nd_1_0);
    nd_0_0.AddConnection(nd_0_1);

    nd_1_0.AddConnection(nd_0_0);
    nd_1_0.AddConnection(nd_2_0);
    nd_1_0.AddConnection(nd_1_1);

    nd_2_0.AddConnection(nd_1_0);
    nd_2_0.AddConnection(nd_2_1);

    nd_0_1.AddConnection(nd_0_0);
    nd_0_1.AddConnection(nd_1_1);
    nd_0_1.AddConnection(nd_0_2);

    nd_1_1.AddConnection(nd_0_1);
    nd_1_1.AddConnection(nd_1_0);
    nd_1_1.AddConnection(nd_2_1);
    nd_1_1.AddConnection(nd_1_2);

    nd_2_1.AddConnection(nd_2_0);
    nd_2_1.AddConnection(nd_1_1);
    nd_2_1.AddConnection(nd_2_2);

    nd_0_2.AddConnection(nd_0_1);
    nd_0_2.AddConnection(nd_1_2);

    nd_1_2.AddConnection(nd_0_2);
    nd_1_2.AddConnection(nd_2_2);
    nd_1_2.AddConnection(nd_1_1);

    nd_2_2.AddConnection(nd_1_2);
    nd_2_2.AddConnection(nd_2_1);


    all_nodes.push_back(nd_0_0);
    all_nodes.push_back(nd_1_0);
    all_nodes.push_back(nd_2_0);

    all_nodes.push_back(nd_0_1);
    all_nodes.push_back(nd_1_1);
    all_nodes.push_back(nd_2_1);

    all_nodes.push_back(nd_0_2);
    all_nodes.push_back(nd_1_2);
    all_nodes.push_back(nd_2_2);


    Point start_pos{ 0, 0 };
    Point end_pos{ 2, 2 };

    auto path = Astar(start_pos, end_pos, all_nodes);

    if (path.empty())
        std::cout << "No path :( \n";
    else
    {
        std::cout << "Path found :) \n";
        std::for_each(path.begin(), path.end(), [](const Point & pt) { pt.Print("Move to: "); });
    }
}

Output:

Path found :)
Move to: x: 0 y: 0
Move to: x: 0 y: 1
Move to: x: 0 y: 2
Move to: x: 1 y: 2
Move to: x: 2 y: 2

What would you suggest for improvements? Huge thanks in advance!

Answer 1

Use hypot()

C has a standard library function to calculate the hypothenuse of a right-angled triangle. You can use it to calculate a distance this way:

int DistTo(Point & other) const {
    return std::hypot(other.x - x, other.y - y);
}

Prefer '\n' instead of std::endl

Prefer using '\n' to end a line. std::endl is equivalent to '\n' plus a flush of the output buffer. Flushing unnecessarily can lead to bad performance.

Overload operator<< to add support for printing in your own classes

It's always nicer if you can make your classes work like existing types in the standard library. This includes printing them. Instead of having to call a member function named Print(), it's much nicer if you could just write:

Point pt;
...
std::cout << "Move to: " << pt << '\n';

To do this, you need to add an operator<< overload outside the class. To make it able to access your class's private members, declare it as a friend, like so:

struct Point {
    ...
    friend std::ostream &operator<<(ostream &os, const Point &point);
};

std::ostream &operator<<(ostream &os, const Point &point) {
    os << '(' << point.x << ", " << point.y << ')';
}

Your class doesn't have private member variables, so the friend declaration is not necessary, but it doesn't hurt either.

Wrong use std::shared_ptr for connections

Each Node can have connections to multiple other nodes. If you want to use std::shared_ptr to ensure proper ownership tracking, then connections must be a std::vector of std::shared_ptr<Node>s, not a std::shared_ptr of a std::vector<Node>. Otherwise, only the list structure itself is tracked. This is particularly important because copying a Node in your case no longer copies the list of connections, but just shares a reference to a single list.

Related to this:

Make the constructor of Node take a std::shared_ptr<Node> parent

Your constructor of class Node takes a raw pointer to a parent node, and then stores it into a std::shared_ptr<Node>. This is problematic, because you can write the following code:

Node child;

{
    Node parent;
    child = Node({}, &parent); // child.parent points to parent
} // lifetime of parent ends here, child.parent is now invalid

You have to ensure you have a std::shared_ptr of the parent node before calling the constructor of the child. Change the constructor to:

Node(Point position = {}, std::shared_ptr<Node> parent = {}): position(position), parent(parent) {};

Of course, this means you need to have shared pointers from the very start. So Astar() should get a vector of shared pointers:

std::vector<Point> Astar(Point &start_pos, Point &end_pos, const std::vector<std::shared_ptr<Node>> &all_nodes)
{
    std::shared_ptr<Node> start_node;
    std::shared_ptr<Node> end_node;

    for (auto &node: all_nodes)
    {
        ...
    }
    ...
}

If you want to avoid this, then alternatively:

Consider adding a class Graph that owns the Nodes

To avoid having to use std::shared_ptr everywhere, but to ensure pointers to parent nodes are always valid, consider adding a class Graph that manages the collection of Nodes. Internally, it could just use a std::vector<Node> to store the nodes; you just have to make sure that when you add and remove nodes from a Graph, that connections and parent/child relationships have been properly deleted beforehand.

If you have a class Graph, you can make Astar() a member function of it. It would look like:

class Graph {
    std::vector<Node> nodes;

public:
    Node &AddNode(Point position) {
        nodes.emplace_back(position);
        return nodes.back();
    }

    void AddConnection(Node &a, Node &b) {
        a.AddConnection(b);
    }

    ...

    std::vector<Point> Astar(Point & start_pos, Point & end_pos);
};

std::vector<Point> Graph::Astar(Point & start_pos, Point & end_pos)
{
    // Find the start and end nodes
    Node start_node;
    Node end_node;

    for (auto &node: nodes)
    {
        ...
    }

    ...
}

Make Astar() take start and end Nodes instead of Points

Instead of telling Astar() the start and end position, why not tell it the start and end Nodes to use? This avoids having to scan the whole list of nodes to find the ones matching the given positions, and avoids potential issues if a given position matches zero or more than one Node. So:

std::vector<Point> Astar(Node &start_node, Node &end_node, std::vector<Node> all_nodes);

Avoid copying variables unnecessarily

There's a lot of copying going on in your code, that might not be necessary. In particular, instead of making copies of Nodes all the time, it might be more efficient to just keep references or pointers to the elements of all_nodes.

Note that some information from the A* algorithm is stored in struct Node. So if you do use references/pointers instead of copies, then you have to ensure only one thread is running Astar() on a given vector of Nodes at any time.

Use std::any_of() instead of std::find_if() if you only need a boolean result

To check if a child is on the closed list, you can use std::any_of() instead of std::find_if():

if (std::any_of(closed_list.begin(), closed_list.end(), [&child](Point & pt){return pt == child.position);}))
    continue;

This saves a bit of typing. The same goes for checking if a child is on the closed list.

Answer 2

std::vector<Point> closed_list;

There are two operations that are applied to closed_list:

1. Add a point.
2. Check whether a given point is already in the list.

A vector can do that job, but there are other data structures that are meant to support that kind of use case, for example std::unordered_set (or an open-addressing variant thereof, which sadly isn't in std).

// Total cost is the sum of past distance and the heuristic (which assumes just line-of-flight)
child.total_dist = child.dist_to_start + child.position.SqrDistTo(end_node.position);

What is SqrDistTo? I didn't see the definition. If it does what it sounds like it would do, namely computing the distance squared (skipping the square root), then it is wrong: the heuristic distance can be larger than the actual distance which is a direct violation of the condition that A* places on the heuristic. Some "tactical overestimation" is sometimes acceptable despite being technically wrong, but squared distance overestimates by an unlimited amount, getting worse and worse for longer distances. If it does something else, or is just "filler" and not representative of the real code, maybe this point of feedback does not apply.