Find shortest path using A*

Question

I have this c++ program. It is about A* algorithm, but the way it is written, its very hard to be understood and very hard to read. How would you edit the code so it is more correct grammatically and easy to understand. I am fairly new to programming, any tip would be highly appreciated!

what the code does: It is a simple implementation of a A* algorithm. We were told that we should try to improve it in terms of code quality. Because our team is fairly new, we decided to ask you if you could give us some tips on how to improve the code - from variables to functions //everything that could actually be improved in some way.

#include <fcntl.h>
#include <iostream>
#include <vector>
#include <list>
#include <queue>
#include <set>
#include <cmath>

class Node {
public:
    Node(Node* parent, int row, int col, int g, int h) 
    : parent(parent), row(row), col(col), g(g), h(h), f(g + h) {        
    }

    Node* const parent;
    const int row;
    const int col;
    const int g;
    const int h;
    const int f; 
};

std::list<Node*> findShortestPath(int fromX, int fromY, int toX, int toY, const std::vector<std::vector<int>> map, int w, int h) {
    auto comp = [](Node* n1, Node* n2) {return n1->f > n2->f;};
    std::priority_queue<Node*, std::vector<Node*>, decltype(comp)> openNodes(comp);
    std::set<Node*> closedNodes;

    // Push the start node into the queue; heuristics is Manhattan distance to the end 
    openNodes.push(new Node(nullptr, fromX, fromY, 0, std::abs(fromX - toX) + std::abs(fromY - toY)));

    while(!openNodes.empty()) {
        // Get the node with least (f = cost + h) and remove it from the queue
        Node* currentNode = openNodes.top();
        openNodes.pop();

        closedNodes.insert(currentNode);

        // Get the neighbors of currentNode
        std::vector<Node*> neighbors;
        for (int i = -1; i <= 1; ++i) {
            for (int j = -1; j <= 1; ++j) {
                if (currentNode->row + i >= 0 && currentNode->row + i < w && currentNode->col + j >= 0 && currentNode->col + j < h && ((i == 0 && j != 0) || (i != 0 && j == 0)) && map[currentNode->row + i][currentNode->col + j] == 0) {
                    neighbors.push_back(new Node(currentNode, currentNode->row + i, currentNode->col + j, currentNode->g + 1, std::abs(currentNode->row + i - toX) + std::abs(currentNode->col + j - toY)));
                }
            }
        }

        // For each neighbor:
        // If it is the end node, return the path
        // If it is not in closed set, add it to open queue
        for (size_t i = 0; i < neighbors.size(); ++i) {
            if (neighbors[i]->row == toX && neighbors[i]->col == toY) {
                // Reconstruct the path because neighbor[i] is the last node
                Node* node = neighbors[i];
                std::list<Node*> route;
                while (node != nullptr) {
                    route.push_front(node);
                    node = node->parent;
                }
                return route;
            }

            if (closedNodes.find(neighbors[i]) != closedNodes.end()) {
                continue;
            }

            openNodes.push(neighbors[i]);       
        }
    }

    return std::list<Node*>();  // return empty list
}

int main() {
    std::list<Node*> route = findShortestPath(0, 0, 6, 6, {
            {0, 1, 1, 0, 0, 0, 0},
            {0, 1, 1, 0, 1, 1, 0},
            {0, 0, 0, 0, 1, 1, 0},
            {0, 1, 1, 1, 1, 1, 0},
            {0, 1, 1, 1, 1, 1, 0},
            {0, 1, 1, 1, 0, 0, 0},
            {0, 0, 0, 0, 0, 1, 0},
    }, 7, 7);

    // Print the path
    if (route.size() == 0) {
        std::cout << "No route to target!" << std::endl;
    } else {
        for (std::list<Node*>::iterator nodeIt = route.begin(); nodeIt != route.end(); ++nodeIt) {
            std::cout << "(" << (*nodeIt)->row << ", " << (*nodeIt)->col << ")->";
        }
    }
    return 0;
}

Answer 1

I don't work a lot with C++ these days, so I won't say too much about that, but here's what I've got:

• You've got a memory leak: you're not deleting the nodes that you've created. Anything that is created with new must be cleaned up with delete (likewise with new[] and delete[]). You'll need to read up in RAII and smart pointers.
• Use meaningful names. g, h, f, h, w are not very descriptive. Names like distance, distanceToTarget, totalCost, mapHeight and mapWidth would make your code easier to understand and to work with.
• The neighbors-adding code isn't very readable - it's almost 240 characters wide, and involves several distinct checks, which are easy to overlook (and that means it's easy to introduce a mistake). Use multiple lines, for example by adding a newline after each &&, and by putting each Node constructor argument on a separate line.
• The neighbors-adding code can also be simplified: instead of checking all tiles in a 3x3 area around the current tile, and then having to exclude several of them, it's easier to loop over an array of 4 (x,y) offsets: (0, -1), (1, 0), (0, 1) and (-1, 0).
• Another improvement is to use more utility functions. You're using Manhattan distances a lot, so create a function for it. The same goes for checking if a coordinate lies within the map boundaries. It would likely make the neighbors-adding code shorter, but more importantly, it makes your code easier to read.
• You're doing a lot with 2D coordinates, as (x, y) integer pairs. Making a Point class would make this easier, especially with overloaded + and - operators and other utility functions, such as that Manhattan distance function. It would also simplify the signature of your findShortestPath function (you can make it accept two points, rather than 4 integers).
• Similarly, a Map class can be useful: not only does it simplify findShortestPath's signature again (because width and height no longer need to be passed separately), it also allows you to move the boundary check and is-walkable logic out of your pathfinding code (they'd become member functions of your Map class). Another benefit is that it hides the details of how a map is stored in memory. If, for some reason, you need to use a different data structure for your map, then you only need to modify the internals of your Map class. Any other code won't need to be changed, as long as Map's interface remains the same.
• Personally, I would create a utility function that can parse a grid from a multi-line text file, or perhaps even from an image. That makes creating test cases easier than fiddling with a comma-separated 2D array in code.
• It's probably a good idea to write some automated tests for this. It should be relatively easy to do, because your function does not rely on any global state. It'll make refactoring your code less risky, and helps in preventing regression bugs. In my opinion it's not always economical to write tests, but this looks like a case where it will easily pay off.
• Note that A* works for any graph, not just for rectangular grids. It might be possible to pre-process a map to get a simplified navigation graph. For example, if your inputs only contain narrow paths, you could turn only crossroads and dead-ends into nodes, and determine the paths (and distances) between them. That gives you a much smaller graph, which means less work for your pathfinder later. I don't know if that's possible or worth the effort in your case, but it might give you some ideas.

Answer 2

Welcome to Code Review!

The code is very ugly and I'm not sure that the algorithm is even correct.

Let's start with your method signature.

std::list<Node*> findShortestPath(int fromX, int fromY, int toX, int toY, const std::vector<std::vector<int>> map, int w, int h)

The w and h arguments are not really needed because you can calculate them yourself inside the function.

h = map.size(); // Do not use `map` as a variable name!
w = map[0].size(); // Assuming rectangular graph as input.

They could be used to limit how much of the graph is searched, but there are better ways of doing that.

In all honesty, instead of passing a const std::vector<std::vector<int>> map variable, it would be better if you had a class that represented a graph. Your algorithm, as it is currently written, only works on rectangular graphs.

You return a std::list<Node*>. Unless you have a good reason to use an std::list, your default data structure in C++ should be an std::vector.

Returning Node*'s would make sense if you had well designed class that represented your graph, but in your case, returning an std::vector<Node> should be good enough. I will discuss this a little more later.

Do not use map as a variable name in C++. There is a data structure called std::map and it just makes your code confusing to read.

Let's move on to your implementation.

You should create a separate function for your Manhattan distance calculation.

int manhattanDistance(int x1, int y1, int x2, int y2)
{
    return std::abs(x1 - x2) + std::abs(y1 - y2);
}

Every time you call new, you have memory leaks because you never call delete on these nodes. If you insist on using pointers, then take a look at std::unique_ptr and std::move or just std::shared_ptr.

To avoid all memory issues, just work with Nodes instead of Node*s (except keep parent as a pointer).

Your code for calculating the neighbors is very ugly. If your input was some kind of graph class, then you could use an adjacency list to obtain a node's neighbors very easily and in a generic fashion that would work on all types of graphs.

Your A-Star algorithm is wrong/incomplete!

You do no update your gs, fs, and parents when a better path is found.

You do not check if a neighbor is already in the frontier/open set.