Node* AStar::VisitNode()
{
Node* pCurrentNode = qOpenList.front();
qOpenList.pop_front();
pCurrentNode->eState = NodeState::Closed;
std::cout << "Visiting Node(" << pCurrentNode->X() << "," << pCurrentNode->Y() << ")...\n";
std::cout << " Adding Adj Node...\n";
std::list<Node*>::iterator iAdjEnd(pCurrentNode->lAdj.end());
for(std::list<Node*>::iterator iter = pCurrentNode->lAdj.begin(); iter != iAdjEnd; ++iter)
{
AddNodeToOpenList(pCurrentNode, *iter);
}
std::cout << " Adding Adj Node...DONE\n";
std::cout << " OPENLIST - START Elements: " << qOpenList.size() << "\n";
std::list<Node*>::iterator iEnd(qOpenList.end());
for(std::list<Node*>::iterator iter = qOpenList.begin(); iter != iEnd;++iter)
{
std::cout << " (" << (*iter)->X() << "," << (*iter)->Y() << ") F = " << (*iter)->iF << " = G(" << (*iter)->iG << ") + H(" << (*iter)->iH << ")\n";
}
std::cout << " OPENLIST - END \n";
std::cout << "Visiting Node(" << pCurrentNode->X() << "," << pCurrentNode->Y() << ")... DONE\n";
return pCurrentNode;
}
PathFinder::PathFinder(IntVector2 start, IntVector2 goal)
: m_start(start)
, m_goal(goal)
, m_isFinished(false)
{
PathNode* startNode = CreateNode(start, nullptr, 0.f, 0.f, 0.f);
startNode->h = MapProxy::ComputeHueristic(startNode->m_position, m_goal);
startNode->f = startNode->h;
AddNodeToOpenList(startNode);
}
void AStar::Setup()
{
CreateGraph();
CreateGraphAdjs();
ComputeGraphHeuristics();
AddNodeToOpenList(NULL, tRoot[iStartNode]);
/*Search();
Clean();*/
}
void AStar::Search()
{
AddNodeToOpenList(NULL, tRoot[iStartNode]);
Node* pCurrentNode = NULL;
while(!qOpenList.empty())
{
pCurrentNode = VisitNode();
if (pCurrentNode == tRoot[iEndNode])
{
PrintPath(pCurrentNode);
break;
}
}
}
//---------------------------------------------------------------------------------------------------------------------------
//PATHFIND
//---------------------------------------------------------------------------------------------------------------------------
std::vector<IntVector2> PathFinder::FindBestPathFromStartToGoal(IntVector2 start, IntVector2 goal, int movementProperties) {
if (start == goal)
return std::vector<IntVector2>();
m_start = start;
m_goal = goal;
CleanUpListsAndRestart();
PathNode* startNode = CreateNode(start, nullptr, 0.f, 0.f, 0.f);
startNode->h = MapProxy::ComputeHueristic(startNode->m_position, m_goal);
startNode->f = startNode->h;
AddNodeToOpenList(startNode);
while (!m_isFinished) {
if (!RunOneStep(movementProperties)) {
return std::vector<IntVector2>();
}
}
return m_currBestPath;
}
//---------------------------------------------------------------------------------------------------------------------------
bool PathFinder::RunOneStep(int movementProperties) {
if(m_openList.empty())
return false;
PathNode* activeNode = FindLowestFNodeOnOpenListAndRemoveIt();
AddToClosedList(activeNode);
if (activeNode->m_position == m_goal) {
WalkClosedListAndConstructPath();
m_isFinished = true;
return true;
}
std::vector<IntVector2> validPositions = MapProxy::FindValidAdjacentPositions(activeNode->m_position, movementProperties);
for (IntVector2& position : validPositions) {
if (IsPositionOnClosedList(position))
continue;
float gLocal = MapProxy::ComputeLogalG(activeNode->m_position, position, movementProperties);
float gParent = activeNode->gLocal + activeNode->gParent;
float h = MapProxy::ComputeHueristic(position, m_goal);
PathNode* existingNode = FindOpenNodeWithPos(position);
if (existingNode) {
if ((gLocal + gParent) < (existingNode->gLocal + existingNode->gParent)) {
UpdateNodeValues(existingNode, gLocal, gParent, activeNode);
}
continue;
}
PathNode* newNode = CreateNode(position, activeNode, gLocal, gParent, h);
AddNodeToOpenList(newNode);
}
return false;
}