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