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 Node
s (I have a field of 256x256 Node
s). 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 Node
s.
/************
* 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());
}
}
}
}
};