void AStarSearcher::computeBestPath(std::vector<AbstractNode*>& out_path) { std::priority_queue<AbstractNode*, std::vector<AbstractNode*>, AStarPriorityQueueComparer> edgeList; AbstractNode* currentNode = m_rootNode; while(!currentNode->isEnd()) { currentNode->spawnChildren(); addExpandedNode(); const std::vector<AbstractNode*>& children = currentNode->getChildren(); for(int i = 0; i < children.size(); i++) { edgeList.push(children[i]); } if (edgeList.size() == 0) break; AbstractNode* nextBest = edgeList.top(); edgeList.pop(); //if we did not select a child node then we have to go back down and up through the tree if(nextBest->getParent() != currentNode) goToChild(currentNode, nextBest->getParent()); currentNode = nextBest; currentNode->onEnter(); } if (currentNode->isGoal()) { // Award 100 points setFinalScore(-currentNode->getTotalCost()); } else { setFinalScore(-currentNode->getTotalCost()); } while(currentNode) { out_path.insert(out_path.begin(), currentNode); currentNode = currentNode->getParent(); } }
void goToChild(AbstractNode* currentNode, AbstractNode* endNode) { AbstractNode* root = goBackToRoot(currentNode); std::stack<AbstractNode*> path; AbstractNode* node = endNode; while(node != NULL) { path.push(node); node = node->getParent(); } while(!path.empty()) { node = path.top(); path.pop(); node->onEnter(); } }