Code Review Stack Exchange is a question and answer site for peer programmer code reviews. Join them; it only takes a minute:

Sign up
Here's how it works:
  1. Anybody can ask a question
  2. Anybody can answer
  3. The best answers are voted up and rise to the top

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);

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


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

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

    make_heap(openList.begin(), openList.end(), ComparePathNodePointer());
    if(*(currentNode->getMapField()) == *targetField){
        Logger::getInstance()->log("Target found");
        return buildpath(currentNode);
    expandNode(currentNode, targetField);
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 {
                push_heap(openList.begin(), openList.end(), ComparePathNodePointer());
                make_heap(openList.begin(), openList.end(), ComparePathNodePointer());
share|improve this question

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.
share|improve this answer

Your Answer


By posting your answer, you agree to the privacy policy and terms of service.

Not the answer you're looking for? Browse other questions tagged or ask your own question.