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;

}