std::list<Vect3f> CWayPointManager::GetPath(CWayPoint* startPos, CWayPoint* destPos)
{
std::list<Vect3f> vecPath;
GraphNodeList openList;
GraphNodeList closeList;
GraphNode* gn = new GraphNode();
gn->SetWP(startPos);
openList.PushBack(gn);
GraphNode* endNode = new GraphNode();
endNode->SetWP(destPos);
while (!openList.IsEmpty())
{
GraphNode* current = openList.PopFront();
if(endNode->CompareNode(current))
{
endNode->SetParentNode(current->GetParentNode());
CHECKED_DELETE(current);
break;
}
//Add To close List
closeList.PushBack(current);
//Iterate around brothers
WayPointList& brothers = current->GetWP()->GetBrothers();
WayPointListIt it = brothers.begin();
WayPointListIt itEnd = brothers.end();
for (; it != itEnd; ++it)
{
CWayPoint* wp = *it;
//Set the cost of node_successor to be the cost of node_current plus
//the cost to get to node_successor from node_current
//Set h to be the estimated distance to node_goal (Using heuristic function)
GraphNode* successor = new GraphNode();
successor->SetWP(wp);
successor->SetG(current->GetG() + (current->GetWP()->GetPosition().Distance(wp->GetPosition())));
successor->SetH(wp->GetPosition().Distance(destPos->GetPosition()));
//find node_successor on the OPEN list
int32 ofound = openList.IndexOf(successor);
//if node_successor is on the OPEN list but the existing one is as good
//or better then discard this successor and continue
if (ofound>=0)
{
GraphNode* temp = openList.At(ofound);
if(temp->GetF() < current->GetF())
{
CHECKED_DELETE(successor);
continue;
}
}
//find node_successor on the CLOSED list
int32 cFound = closeList.IndexOf(successor);
//if node_successor is on the CLOSED list but the existing one is as good
//or better then discard this successor and continue;
if (cFound>=0)
{
GraphNode* temp = closeList.At(cFound);
if(temp->GetF() < current->GetF())
{
CHECKED_DELETE(successor);
continue;
}
}
//Remove occurences of node_successor from OPEN and CLOSED
if (ofound != -1)
{
openList.RemoveAt(ofound);
}
if (cFound!=-1)
{
closeList.RemoveAt(cFound);
}
//Set the parent of node_successor to node_current;
successor->SetParentNode(current);
//Add node_successor to the OPEN list
openList.PushBack(successor);
}
openList.Sort();
}
if(endNode->GetParentNode() == NULL)
{
return vecPath;
}
GraphNode* backtrace = endNode;
while(backtrace != NULL)
{
vecPath.push_front(backtrace->GetWP()->GetPosition());
backtrace = backtrace->GetParentNode();
}
CHECKED_DELETE(endNode);
return vecPath;
}
