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