A* C++ implementation is too slow

Question

My A* Algorithm in C++ is pretty slow. I'm not sure if it is because of a bad implementation or just because I have way too many Nodes (I have a field of 256x256 Nodes). It takes the algorithm about 5 seconds to complete.

I'm using a vector for the open and closed List. For the openList I use std::make_heap to sort the Nodes.

/************
* Method to calculate the shortes path
*
**/
std::vector<Position*> Pathfinding::getShortestPath(MapField* startField, MapField* targetField) {

PathNode* startNode = new PathNode(startField, argosMap);
openList.push_back(startNode);

push_heap(openList.begin(), openList.end(), ComparePathNodePointer());


while(!(openList.empty())){

    make_heap(openList.begin(), openList.end(), ComparePathNodePointer());
    PathNode* currentNode = openList.front();

    pop_heap(openList.begin(), openList.end(), ComparePathNodePointer());

    openList.pop_back();
    make_heap(openList.begin(), openList.end(), ComparePathNodePointer());
    if(*(currentNode->getMapField()) == *targetField){
        Logger::getInstance()->log("Target found");
        return buildpath(currentNode);
    }
    expandNode(currentNode, targetField);
    closedList.push_back(currentNode);
}
return buildpath(startNode);
};

/************
* expands a PathNode and adds it to the openList if it meets the conditions of A*
*
**/
void Pathfinding::expandNode(PathNode* currentNode, MapField* targetField) {

std::vector<PathNode*> children = currentNode->getChildren();
std::vector<PathNode*>::iterator child;

for (child = children.begin(); child != children.end(); child++) {

    Utils::PointerValuesEquals<PathNode> childToFind = {*(child)};

    //check if Node is alreay in closedList
    if(std::find_if(closedList.begin(), closedList.end(), childToFind) == closedList.end()) {
        double actualCost = (*child)->getCostOfField(currentNode)+currentNode->getActualCost();

        //check if Node is already in openList, if so, check if the value is better if it is better, expand the new Node otherwise use the existing Node
        if(!((std::find_if(openList.begin(), openList.end(), childToFind) != openList.end()) && actualCost >= (*std::find_if(openList.begin(), openList.end(), childToFind))->getActualCost())){
            double estimatedCosts = actualCost+(*child)->getCostToTarget(targetField);

            //when Node is already in Openlist update it, if not add the new Node
            if(std::find_if(openList.begin(), openList.end(), childToFind) != openList.end()) {
                (*std::find_if(openList.begin(), openList.end(), childToFind))->setParent(currentNode);
                (*std::find_if(openList.begin(), openList.end(), childToFind))->setActualCost(actualCost);
                (*std::find_if(openList.begin(), openList.end(), childToFind))->setTotalCost(estimatedCosts);

                make_heap(openList.begin(), openList.end(), ComparePathNodePointer());

            } else {
                (*child)->setParent(currentNode);
                (*child)->setActualCost(actualCost);
                (*child)->setTotalCost(estimatedCosts);
                this->openList.push_back(*child);
                push_heap(openList.begin(), openList.end(), ComparePathNodePointer());
                make_heap(openList.begin(), openList.end(), ComparePathNodePointer());
            }
        } 
    }
}
};

Answer 1

As a guideline: your inner loop should contain no operation more expensive than O(log(N)). This means no iterating over the entire closed/open list, and no making heaps over and over again! Here's how:

1. Use a std::set sorted by weight instead of a heap. begin() is your next node, and to update the weight of a node first remove it from the set, change the weight, and put it back again.
2. Don't use find_if to test for set inclusion. Use an std::unordered_set for the closed or open nodes.