Skip to main content
2 of 4
added 26 characters in body

SFML: A Star Pathfinding is incredibly slow?

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:

std::vector<Node*> AStar::GeneratePath(Node* start, Node* goal)
{
    int c = 0;
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;

    start->g = 0;

    openSet.push_back(start);

    while (openSet.size() > 0)
    {
        Node* currentNode = openSet.at(0);
        for (int i = 0; i < openSet.size(); ++i)
        {
            if (openSet.at(i)->f < currentNode->f || openSet.at(i)->f == currentNode->f && openSet.at(i)->h < currentNode->h)
            {
                currentNode = openSet.at(i);
            }
        }

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

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

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

                int newMovementCostToNeighbour = currentNode->g + GetDistance(currentNode, neighbour) + neighbour->penalty;
                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(&closedSet, neighbour)) openSet.push_back(neighbour);
                    else openSet;
                }
            }
        }
    }

    std::vector<Node*> nodes;
    Node* current = goal;
    while (current != nullptr)
    {
        nodes.push_back(current);
        current = current->parent;
    }

    reverse(nodes.begin(), nodes.end());

    return nodes;
}

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

    if (distX > distY) return distY * (distX - distY);
    return distX + (distY - distX);
}

std::vector<Node*> AStar::GetNeighbours(Node* _pNode)
{
    std::vector<Node*> neighbours;
    for (int x = -1; x <= 1; ++x)
    {
        if (_pNode == nullptr)
        {
            break;
        }
        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 < 512 && checkY >= 0 && checkY < 512)
            {
                neighbours.push_back(GetNodeAt(checkX, checkY));
            }
        }
    }
    return neighbours;
}

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

void AStar::Remove(std::vector<Node*>* _pVector, Node* _pNode)
{
    for (std::vector<Node*>::iterator iter = _pVector->begin(); iter != _pVector->end(); ++iter)
    {
        if (*iter == _pNode)
        {
            _pVector->erase(iter);
            break;
        }
    }
}

Node* AStar::GetNodeAt(int _x, int _y)
{
    for (Node* node : EntityManager::GetInstance()->GetAStar()->GetNodeMap())
    {
        if (node->x == _x && node->y == _y)
        {
            return node;
        }
    }
    return nullptr;
}

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?