void Follower::resetStartNode()
{
mStartNode->setPosition(mPosition);
//find closest existing node and use the same links as it has
PathNode* close;
gCurrentLevel->getPaths()->getNodeNearPosition(close, mPosition);
mStartNode->setLinks(close->getLinks());
}
vector<D3DXVECTOR3> AStar::findPath(PathNode* _start, PathNode* _end)
{
//time it for debugging
clock_t clockStart = clock();
//empty the closed set vector
vector<PathNode*>().swap(mClosedSet);
//empty the open set and put in only the start node
vector<PathNode*>().swap(mOpenSet);
mOpenSet.push_back(_start);
//set all node gScores to max
for (PathNode* PN : mPathNodes)
PN->setGScore(INT_MAX);
//set start node properties
_start->setGScore(0);
_start->setFScore(findHeuristic(_start, _end));
//while the open set is not empty
while (mOpenSet.size() > 0)
{
//the node in open set having the lowest f score
PathNode* current;
findLowestFScore(current);
//if we found the goal, return the path
if (current == _end || ((clock() - clockStart) / (float)CLOCKS_PER_SEC) > MAX_PATH_TIME_LOW)
{
vector<D3DXVECTOR3> rePath;
reconstructPath(rePath, _start, current);
return rePath;
}
//save current to closed set
mClosedSet.push_back(current);
//remove current from open set
for (vector<PathNode*>::iterator iter = mOpenSet.begin(); iter != mOpenSet.end(); ++iter)
{
if (*iter == current)
{
mOpenSet.erase(iter);
break;
}
}
//for each linked node in the current node
for (PathNode* PN : current->getLinks())
{
//if it is already in the closed set, continue
if (inClosedSet(PN))
continue;
//find tentative gScore
int tempGScore = current->getGScore() + findHeuristic(current, _end);
//if link node is not in open set or tempGScore < present GScore
if (!inOpenSet(PN) || tempGScore < PN->getGScore())
{
//set the came from of the link to the current node
PN->setCameFrom(current);
//set g score
PN->setGScore(tempGScore);
PN->setFScore(PN->getGScore() + findHeuristic(PN, _end));
//if link is not in open set, add to it
if (!inOpenSet(PN))
mOpenSet.push_back(PN);
}
}
}
//finished loop without finding goal
OutputDebugString(L"ERROR: No path found.");
return vector<D3DXVECTOR3>();
}