Why is my A* Pathfinding incredibly slow?

Question

I'm trying to use A* to make my enemies move about, but it takes up to 20 seconds to get a path to walk.

My grid is only 64*64.

This is my code:

 #include "AStar.h"
#include "Node.h"
#include "EntityManager.h"
#include <algorithm>
#include <iostream>
#include <unordered_set>
#include <queue>

using namespace Dotmk_AI;
using namespace Dotmk_Managers;

AStar::AStar()
{

}

std::vector<Node*> AStar::GeneratePath(Node* start, Node* goal)
{
    std::vector<std::vector<Node*>>* pNodeMap = EntityManager::GetAStar()->GetNodeMap();

    std::vector<Node*> openSet;
    std::unordered_set<Node*> closedSet;

    make_heap(openSet.begin(), openSet.end());

    start->g = 0;

    openSet.push_back(start);
    push_heap(openSet.begin(), openSet.end());
    while (openSet.size() > 0)
    {
        Node* currentNode = openSet[0];
        for (int i = 0; i < openSet.size(); ++i)
        {
            if (openSet[0]->f < currentNode->f || openSet[0]->f == currentNode->f && openSet[0]->h < currentNode->h)
            {
                currentNode = openSet[i];
            }
        }

        Remove(&openSet, currentNode);
        closedSet.insert(currentNode);

        if (currentNode->x == goal->x && currentNode->y == goal->y)
        {
            goal->parent = currentNode->parent;
            break;
        }

        for (Node* neighbour : GetNeighbours(currentNode, pNodeMap))
        {
            if (neighbour != nullptr)
            {
                if (Contains(closedSet, neighbour) || neighbour->e == 0) continue;

                int newMovementCostToNeighbour = currentNode->g + GetDistance(currentNode, neighbour);
                if (newMovementCostToNeighbour < neighbour->g || !Contains(openSet, neighbour))
                {
                    neighbour->g = newMovementCostToNeighbour;
                    neighbour->h = GetDistance(neighbour, goal);
                    neighbour->f = neighbour->g + neighbour->h;
                    neighbour->parent = currentNode;

                    if (!Contains(openSet, neighbour)) openSet.push_back(neighbour);
                }
            }
        }
    }
    std::vector<Node*> nodes;
    Node* current = goal;
    while (current != nullptr)
    {
        nodes.push_back(current);
        current = current->parent;
    }
    reverse(nodes.begin(), nodes.end());
    nodes.pop_back();
    std::cout << nodes.size() << std::endl;

    return nodes;
}

int AStar::GetDistance(Node* nodeA, Node* nodeB)  
{
    int distX = abs(nodeA->x - nodeB->x);
    int distY = abs(nodeA->y - nodeB->y);

    return 1 * (distX * distX + distY * distY);
}

std::vector<Node*> AStar::GetNeighbours(Node* _pNode, std::vector<std::vector<Node*>>* _pNodeMap)
{
    std::vector<Node*> neighbours;
    neighbours.reserve(8);
    for (int x = -1; x <= 1; ++x)
    {
        for (int y = -1; y <= 1; ++y)
        {
            if (x == 0 && y == 0) continue;

            int checkX = _pNode->x + x;
            int checkY = _pNode->y + y;

            if (checkX >= 0 && checkX < 64 && checkY >= 0 && checkY < 64)
            {
                neighbours.push_back((*_pNodeMap)[checkX][checkY]);
            }
        }
    }
    return neighbours;
}

bool AStar::Contains(std::vector<Node*> _pVector, Node* _pNode)
{
    return find(_pVector.begin(), _pVector.end(), _pNode) != _pVector.end();
}

bool AStar::Contains(std::unordered_set<Node*> _pVector, Node* _pNode)
{
    return find(_pVector.begin(), _pVector.end(), _pNode) != _pVector.end();
}

void AStar::Remove(std::vector<Node*>* _pVector, Node* _pNode)
{
    int pos = find(_pVector->begin(), _pVector->end(), _pNode) - _pVector->begin();
    auto it = _pVector->begin() + pos;
    rotate(it, it + 1, _pVector->end());
    _pVector->pop_back();
}

and this is my node class

class Node
{
public:
    Node(int _x, int _y, int _e);
    ~Node();

    std::vector<Node*> neighbors;
    Node* parent;
    int f, g, h, x, y, e, penalty;
};

Is there anything I'm doing wrong?

EDIT: Setting it on Release mode will make the pathfinding ALMOST instant, but still noticable. I've taken much of your guys advice and improved the code.

Answer 1

I see a few issues.

1. you are doing a lot of linear scans over vectors (at least 2 for each neighbour for each node you examine). This will be pretty slow. You don't even need the closed set if you just have a bool isClosed in the Node.

2. You should sort the openset or make the openSet a min heap of some description. This will make the search for the next node a lot faster.

3. erase will move all nodes in the vector up one which is again a linear scan. Instead it's cheaper to just swap the node with the last one and pop_back (at least when not keeping it sorted). When using a proper min heap that operation will become a O(log n) operation.

Finally if all enemies are going to the same goal (like the player) then you can simply flood fill the entire arena once (making the parent field point to the next node) and use that for pathing.

Answer 2

Since performance is the most important thing - I would rewrite this to use arrays rather than standard library containers. The problem with using data structures designed and developed by someone else is that if you don't understand how they're implemented you'll very easily end up performing millions of unnecessary operations - including memory allocation and deallocation - without noticing anything except the performance hit when running the code with a large number of nodes.

There's only one quick win I can see which would be to make the following change to your GetNeighbours function:

std::vector<Node*> AStar::GetNeighbours(Node* _pNode, std::vector<std::vector<Node*>>* _pNodeMap)
{
    std::vector<Node*> neighbours;
    neighbours.reserve(8);
    // Keep a count of the number of Neighbours found
    int neighboursFound = 0;
    for (int x = -1; x <= 1; ++x)
    {
        for (int y = -1; y <= 1; ++y)
        {
            if (x == 0 && y == 0) continue;

            int checkX = _pNode->x + x;
            int checkY = _pNode->y + y;

            if (checkX >= 0 && checkX < 64 && checkY >= 0 && checkY < 64)
            {
                neighbours.push_back((*_pNodeMap)[checkX][checkY]);
                // Return early from the function once all the neighbours are found
                if(++neighboursFound == 8) {return neighbours;}
            }
        }
    }
    return neighbours;
}

Now the other thing I vaguely remembered (then a quick bit of reading confirmed) about standard library containers is that using the push_heap and make_heap functions will call the less than '<' operator for your node class. I'm not sure if you have that defined somewhere else and haven't included it, and I also don't know what happens if you use it but haven't defined it, but I have an inkling that all of your code relating to using a heap may actually not be doing anything useful without it.

If it doesn't sort your open set the way you think it does, then that would mean that at every step of the way each node would be compared to all of the nodes around it - if they were closer than the node being considered they'd eventually be processed in the order they were added. So if your nodes where on opposite sides of the grid it would consider most of the nodes rather than only the potential shortest path.

The test to see if I'm right would be to add an overloaded less than operator which I think is something like the following (I haven't done OO C++ for a while) to your Node class.

bool Node::operator<(const Node& a, const Node& b) {
    return a.h < b.h;
}

Answer 3

The problem was this:

bool AStar::Contains(std::vector<Node*> _pVector, Node* _pNode)
{
    return find(_pVector.begin(), _pVector.end(), _pNode) != _pVector.end();
}

It made a copy of the vector every single time it was called, wich made it incredibly slow. Making it a pointer to the vector increased the time from about 30 Seconds to only about 6 Ms.

After implementing a custom heap like structure, it is now only 1 Ms or even 0 Ms at certain times.

Like Trevor Powell also said, the problem was also rooted in running the game in Debug mode, wich made it way worse.

Answer 4

EDIT: Setting it on Release mode will make the pathfinding ALMOST instant

This is probably the key piece of information.

Although you don't say so, your problem is presumably that you're using the STL in a debug build in something being compiled within Visual Studio.

Visual Studio's implementation of the STL is notoriously (and catastrophically) slow in debug builds, and double-particularly for the creation and copying of templated container classes.

One thing you can do to vastly improve STL performance in debug builds in Visual Studio is to turn off your program's usage of the debug heap, which you can do by setting _NO_DEBUG_HEAP=1 in your project's environment variables. (more details on the Microsoft MSDN blog)

Some folks report speed improvements from disabling STL iterator sanity checking (by adding #define _SECURE_SCL 0 to a prefix file), but I've never found this to make a notable difference in my projects, whereas disabling use of the debug heap makes all the difference in the world.

Alternately, you can always switch to using an alternate implementation of STL, instead of the one provided by Microsoft. EASTL is apparently popular, and doesn't suffer from the same performance problems as Microsoft's default implementation. I've not used EASTL myself, but I've heard other people say nice things about it, so it might well be another option.