A* C++ implementation is too slow 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&lt;Position*&gt; 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-&gt;getMapField()) == *targetField){
        Logger::getInstance()-&gt;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&lt;PathNode*&gt; children = currentNode-&gt;getChildren();
std::vector&lt;PathNode*&gt;::iterator child;

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

    Utils::PointerValuesEquals&lt;PathNode&gt; childToFind = {*(child)};

    //check if Node is alreay in closedList
    if(std::find_if(closedList.begin(), closedList.end(), childToFind) == closedList.end()) {
        double actualCost = (*child)-&gt;getCostOfField(currentNode)+currentNode-&gt;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()) &amp;&amp; actualCost &gt;= (*std::find_if(openList.begin(), openList.end(), childToFind))-&gt;getActualCost())){
            double estimatedCosts = actualCost+(*child)-&gt;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))-&gt;setParent(currentNode);
                (*std::find_if(openList.begin(), openList.end(), childToFind))-&gt;setActualCost(actualCost);
                (*std::find_if(openList.begin(), openList.end(), childToFind))-&gt;setTotalCost(estimatedCosts);

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

            } else {
                (*child)-&gt;setParent(currentNode);
                (*child)-&gt;setActualCost(actualCost);
                (*child)-&gt;setTotalCost(estimatedCosts);
                this-&gt;openList.push_back(*child);
                push_heap(openList.begin(), openList.end(), ComparePathNodePointer());
                make_heap(openList.begin(), openList.end(), ComparePathNodePointer());
            }
        } 
    }
}
};
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:


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.
Don't use find_if to test for set inclusion. Use an std::unordered_set for the closed or open nodes.