A-star pathfinding algorithm

Question

I'm working on a school project where I need to implement the pathfinding algorithm. The requirements are that it needs to find the path in at most a few seconds and ideally in less than 1 second. I've been trying to optimize my code to make the algorithm faster but it keeps taking a few minutes to find the path and I can't find what the cause it is..

I'll provide my code below and I hope someone will be able to help and/or point to things that can be done more efficiently which would be greatly appreciated.

Header of Pathfinder:

#ifndef PATHFINDER_H
#define PATHFINDER_H

//#include "world.h"
#include "lib/world.h"
#include "node.h"
#include <iostream>
#include <QSet>
#include <QStack>
#include <QHash>
#include <cmath>
#include <queue>

class Pathfinder
{
public:
    Pathfinder(std::vector<std::shared_ptr<Tile>> tiles, int columns, int rows);
    struct compareDistance {
        bool operator()(const std::shared_ptr<Node> a , const std::shared_ptr<Node> b){
            return (b->getF() < a->getF());
        }
    };

    std::vector<std::shared_ptr<Node>> nodeList;
    std::priority_queue<std::shared_ptr<Node>, std::vector<std::shared_ptr<Node>>,
                     compareDistance> openList;

    std::vector<std::shared_ptr<Node>> closedList;
    QStack<std::shared_ptr<Tile>> path;
    QStack<std::shared_ptr<Tile>> findPath(int xDestination, int yDestination);

    std::vector<std::shared_ptr<Enemy>> enemyList;
    std::vector<std::shared_ptr<PEnemy>> pEnemyList;
    //std::vector<std::shared_ptr<HealthPack>> healthpacks;

    float getWeight() const;
    void setWeight(float value);

    float getHeuristicCost(int current_x, int current_y, int destination_x, int destination_y);

    int getHeight() const;
    void setHeight(int value);

    int getWidth() const;
    void setWidth(int value);

    float getMoveCost() const;
    void setMoveCost(float value);

    bool solveAlgorithm(int xDestination, int yDestination);
    void startingPoint(int xStart, int yStart);

private:
    std::vector<std::shared_ptr<Tile>> tiles;
    float weight = 1;
    int width = 0;
    int height = 0;

    bool isValid(int x, int y);
    void addNeighbors(int index, std::shared_ptr<Node> pre, std::shared_ptr<Node> finalNode);
    std::shared_ptr<Tile> getTile(int x,int y);
    std::vector<int> specialTiles;
};

#endif // PATHFINDER_H

Function to iterate over all the nodes and update their distance:

bool Pathfinder::solveAlgorithm(int xDestination, int yDestination)
{
    std::cout<<"Solve Algorithm starting"<<std::endl;

    auto destinationIndex = xDestination + yDestination*width;
    auto destinationTile = std::make_shared<Tile>(xDestination,yDestination,tiles[(uint)destinationIndex]->getValue());
    std::shared_ptr<Node> destinationNode = std::make_shared<Node>(destinationTile,nullptr);
    nodeList[destinationIndex] = destinationNode; //nodeList is std::vector<std::shared_ptr<Node>>

    std::cout<<"Width: "<<width<<std::endl;
    std::cout<<"Height: "<<height<<std::endl;

    //openList is priorityQueue
    while(!openList.empty()){
        std::shared_ptr<Node> currentNode = openList.top();
        openList.pop(); //remove currentNode from openList
        closedList.push_back(currentNode); //closedList is std::vector<shared_ptr<Node>>
        //int index = currentNode->getTile()->getXPos()+currentNode->getTile()->getYPos()*width;
        //closed.insert(index,currentNode);

        auto currentTile = currentNode->getTile();//Tile the current node points to
        int currentX = currentTile->getXPos();
        int currentY = currentTile->getYPos();
        std::cout<<"Start position: "<<currentTile->getXPos()<<", "<<currentTile->getYPos()<<std::endl;

        if(currentNode == destinationNode){
            return true;
        }

        for (int newX = -1; newX <= 1; newX++) {
            for (int newY = -1; newY <= 1; newY++) {
                if(isValid(currentX+newX,currentY+newY) && !(newX == 0 && newY == 0)){
                    int newIndex = (currentX+newX) + (currentY+newY)*width;
                    addNeighbors(newIndex,currentNode,destinationNode);
                }
            }
        }
    }
    return false;
}

Function to check or add neighbours:

void Pathfinder::addNeighbors(int index, std::shared_ptr<Node> parent, std::shared_ptr<Node> final)
{
    //tiles is std::vector<std::shared_ptr<Tile>>
    auto tilePointer = tiles[(uint)index];//convert tile this node is associated with from unique ptr to shared ptr
    std::shared_ptr<Node> neighbor;
    //Neighbor not yet discovered
    if(nodeList[index] == nullptr){
        neighbor = std::make_shared<Node>(tilePointer, nullptr);
        nodeList[index] = neighbor;
    }
    else{
        neighbor = nodeList[index];
    }

    if(neighbor->getTile()->getValue() <= 1){
        //if neighbor is not in closed list
        if(std::find(closedList.begin(), closedList.end(), neighbor) == closedList.end()){
            auto tempG = parent->getG() + sqrt(pow(neighbor->getTile()->getXPos()-parent->getTile()->getXPos(),2)+pow(neighbor->getTile()->getYPos()-parent->getTile()->getYPos(),2));
            tempG = (tempG + (1-neighbor->getTile()->getValue()));//*weight;
            auto tempH = getHeuristicCost(neighbor->getTile()->getXPos(),neighbor->getTile()->getYPos(),final->getTile()->getXPos(),final->getTile()->getYPos());
            tempH *= weight;
            double tempF = tempG + tempH;

            //The neighbor was newly made node - also possible to check the G cost of neighbor
            //If G = 0 ==> not yet updated and hence new node
            if(neighbor->getParent() == nullptr){
                neighbor->setParent(parent);
                neighbor->setG(tempG);
                neighbor->setH(tempH);
                openList.push(neighbor);
            }
            else {
                //We found better path ==> update cost and parent
                if(neighbor->getF() > tempF){
                    neighbor->setParent(parent);
                    neighbor->setG(tempG);
                    neighbor->setH(tempH);
                }
            }
        }
    }
}

The header of the Node Class:

#ifndef NODE_H
#define NODE_H

#include "world.h"
#include "lib/world.h"
#include <memory>
#include <limits>


class Node
{
public:
    Node(std::shared_ptr<Tile> tile,std::shared_ptr<Node> previousNode);
    Node();

    std::shared_ptr<Tile> getTile() const;
    void setTile(const std::shared_ptr<Tile> &value);

    std::shared_ptr<Node> getParent() const;
    void setParent(std::shared_ptr<Node> value);

    double getG() const;
    void setG(double value);
    double getF() const;

    double getH() const;
    void setH(double value);

private:
    std::shared_ptr<Node> parent;
    std::shared_ptr<Tile> tile;
    double g;
    double h;
};

My heuristic function is the Manhattan distance: return abs(destination_x - current_x)+abs(destination_y - current_y);

For the backtracing I use a Stack so I can simply pop() as long as it is not empty to draw the tiles being part of the path found:

QStack<std::shared_ptr<Tile>> Pathfinder::findPath(int xDestination, int yDestination)
{
    if(solveAlgorithm(xDestination,yDestination) == true){
        std::shared_ptr<Node> endNode = closedList.back(); //endNode or destination is the last added node to the closedList
        while(endNode->getParent() != nullptr){
            path.push(endNode->getTile());//push the tile of parent to top of stack
            endNode = endNode->getParent();
        }
        return path;
    }
    else{
       return path;
    }
}

I could not compile it, various types do not match. For example there is void addNeighbors(int index, Node* pre, Node* finalNode); in the header, but the implementation uses shared_ptr<Node>. — user555045
– user555045, Commented Dec 10, 2020 at 19:22
@harold My apologies, I copy pasted my actual version of the Pathfinder header where I changed everything to Node* instead of std::shared_ptr<Node>. I edited the header, it should be fine now — Zyo2606
– Zyo2606, Commented Dec 10, 2020 at 20:07

user555045 · Accepted Answer · 2020-12-10 22:04:50Z

An easy performance win

I had to make up some code to "fill the gaps", and my own map, but with that I managed to compile and profile the code. The result is not a big surprise though. On a 200x200 map with some annoying obstacles (to prevent the search being too trivial), there was one line in the whole program that took 95% of the time:

if (std::find(closedList.begin(), closedList.end(), neighbor.get()) == closedList.end())

That does not surprise me: the closed list can easily get quite large (depending on the obstacles, about as large as the whole map), and here is a linear search over the whole thing.

There are other ways it could work, for examble a boolean isClosed on the Node object (this is the simplest way to fix the problem), or a hashmap of coordinates, or even a 2D array of booleans. std::vector is not a good choice to represent the Closed set with.

Keep in mind that this line will stop working:

 Node* endNode = closedList.back();

But there are other ways to get that node.

A sneaky bug

Consider this piece of the code:

//We found better path ==> update cost and parent
if (neighbor->getF() > tempF) {
    neighbor->setParent(parent);
    neighbor->setG(tempG);
    neighbor->setH(tempH);
}

Often I see this code omitted, which in a sense is even more incorrect, but this implementation of the "change parent if a shortcut to an open node is found" has a problem: the priority queue is not informed of the new F score. As a result, the node may not come out of the queue at the right moment, and more insidiously this violates the internal structure of the priority queue.

std::priority_queue does not have the capability of responding to a changed key. You could make your own data structure that can do that, by implementing your own binary heap on top of an std::vector and a hashmap from coordinate to index-in-the-heap. The hashmap is to be able to efficiently find where in the heap a given node currently is, which is necessary to be able to restore the heap property for that node. Any time something in the heap moves, that hashmap would need to be updated with the new indexes.

Since that slightly odd mechanism is needed, the std::push_heap and std::pop_heap functions from the algorithm header are not much use. It can be done without the hashmap mechanism, but then there will sometimes be a linear search over the entire Open List, which would create a similar situation as there was with the slow search through the Closed List.

Overuse of `std::shared_ptr`

For large maps, destroying the Pathfinder object can cause a stack overflow due to too many chained shared_ptr<Node> destructions. In my opinion, the pointer to parent should not be a shared_ptr: nodes don't partially own their parent, the Pathfinder owns all nodes. It should be possible to arrange that with unique_ptr, or by keeping the nodes in the nodeList by value. The node itself can refer to its parent by raw pointer or node-index or coordinate or even by an integer 0..7 that tells the direction to go in. A node does not need access to its parent, it merely needs to record which way to go when constructing the path.

The openList does not need to redundantly double-own the nodes which the pathfinder already owns through the nodeList. That applies to closeList as well, but I recommended its removal to begin with.

Similarly, in my view nodes don't really have shared ownership of tiles, the map owns tiles and nodes merely refer to existing tiles: shared_ptr is not really appropriate.

The resulting path does not need to own the tiles either and I would suggest a vector of coordinates (by value) to ensure that it only contains the data that really matters and doesn't entangle itself with ownership.

Thanks for this detailed answer! Indeed I changed this line you pointed out and it is much faster now! Also I had already changed all these std::shared_ptr and I'm using raw pointer now for parent etc. I don't quite understand this The openList does not need to redundantly double-own the nodes which the pathfinder already owns through the nodeList — Zyo2606
– Zyo2606, Commented Dec 11, 2020 at 20:10
@Zyo2606 I mean since the nodes are present in the nodeList and kept alive that way, the openList does not need to keep alive those nodes also: it could refer to them to by raw pointer or index or coordinate etc. — user555045
– user555045, Commented Dec 12, 2020 at 0:28
Yes ok I understand, that is indeed changed to raw pointer in the meantime :) — Zyo2606
– Zyo2606, Commented Dec 12, 2020 at 11:49

Stack Exchange Network

A-star pathfinding algorithm

1 Answer 1

An easy performance win

A sneaky bug

Overuse of `std::shared_ptr`

You must log in to answer this question.

Hot Network Questions

A-star pathfinding algorithm

1 Answer 1

An easy performance win

A sneaky bug

Overuse of std::shared_ptr

You must log in to answer this question.

Related

Hot Network Questions

Overuse of `std::shared_ptr`