c – A *寻路保证找到最短路径？

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