int Graph::FindPath(Node *start, Node *finish, list< vec2f > &path)
{
list<NodeRecord*> open;
list<NodeRecord*> closed;
list<NodeRecord*>::iterator openIt;
list<NodeRecord*>::iterator closedIt;
// add the starting node to the open list
open.push_back( new NodeRecord(start, NULL, 0.0f, 0.0f + start->pos.DistanceSq(finish->pos) ) );
// NodeRecord(Node *node, Node *from, float cost, float totalCost)
while(!open.empty())
{
// find the node record with the lowest cost
NodeRecord *currentRecord = open.front();
openIt = ++open.begin();
while(openIt != open.end())
{
if((*openIt)->total < currentRecord->total)
currentRecord = (*openIt);
openIt++;
}
// get a pointer to the current node
Node *currentNode = currentRecord->node;
// if the current node is the finish point
if(currentNode == finish)
{
// add the finish node
path.push_front(currentNode->pos);
// add all the from nodes
Node *from = currentRecord->from;
while(!closed.empty())
{
// if this node record is where the path came from,
if(closed.back()->node == from) //&& closed.back()->from != NULL
{
// add it to the path
path.push_front( from->pos );
// get the next 'from' node
from = closed.back()->from;
}
// delete the node record
delete closed.back();
closed.pop_back();
}
while(! open.empty() )
{
delete open.back();
open.pop_back();
}
// a path was found
return 0;
}
// cycle through all neighbors of the current node
bool isClosed, isOpen;
for(int i = 0; i < (int)currentNode->neighbours.size(); i++)
{
// check if neigbour is on the closed list
isClosed = false;
closedIt = closed.begin();
while(closedIt != closed.end())
{
if(currentNode->neighbours[i] == (*closedIt)->node)
{
isClosed = true;
break;
}
closedIt++;
}
// skip if already on the closed list
if(isClosed == true)
continue;
float cost = currentRecord->cost + currentNode->distance[i];
float totalCost = cost + currentNode->neighbours[i]->pos.DistanceSq(finish->pos);
// check if this neighbour is already on the open list
isOpen = false;
openIt = open.begin();
while(openIt != open.end())
{
if(currentNode->neighbours[i] == (*openIt)->node)
{
// node was found on the open list
if(totalCost < (*openIt)->total)
{
// node on open list was updated
(*openIt)->cost = cost;
(*openIt)->total = totalCost;
(*openIt)->from = currentNode;
}
isOpen = true;
break;
}
openIt++;
}
// skip if already on the open list
if(isOpen == true)
continue;
// add to the open list
open.push_back( new NodeRecord(currentNode->neighbours[i], currentNode, cost, totalCost) );
}
// move the current node to the closed list after it has been evaluated
closed.push_back( currentRecord );
open.remove( currentRecord );
}
// free any nodes left on the closed list
while(! closed.empty() )
{
delete closed.back();
closed.pop_back();
}
// no path was found
return -1;
}
Back