/**
* Connects the node. This will connect the node to the previous node along the path to @a target
* and update the pathfinding information.
* @param tuCost The total cost of the path so far.
* @param prevNode The previous node along the path.
* @param prevDir The direction FROM the previous node.
* @param target The target position (used to update our guess cost).
*/
void PathfindingNode::connect(int tuCost, PathfindingNode* prevNode, int prevDir, const Position &target)
{
_tuCost = tuCost;
_prevNode = prevNode;
_prevDir = prevDir;
if (!inOpenSet()) // Otherwise we have this already.
{
Position d = target - _pos;
d *= d;
_tuGuess = 4 * sqrt((double)d.x + d.y + d.z);
}
}
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>();
}
