airrpathfind.cpp

RPG3D演示 国外著名3D引擎 鬼火 这是一个开发团队做的演示 有完整源代码

    // Reseting open and closed lists
    core::list<PathNode*>::Iterator iter = nodes.begin();
    while (iter != nodes.end())
    {
        (*iter)->list = PIL_NONE;
        iter++;
    }

    core::list<PathNode*> openList;  // Must be kept ordered
    openList.clear();

    //Adding first node into open list
    startNode->Gdist = 0;
    startNode->Fdist = startNode->Hdist(endNode);
    startNode->parent = NULL;
    openList.push_back(startNode);
    if (log) soubor << "First node added" << ::endl;

    u32 passCount = 1;
    bool pathFound = false;

    if (startNode == endNode) pathFound = true;

    while(openList.getSize() != 0)
    {
        if (log) soubor << ::endl << "Iteration number " << passCount << ::endl;

        //Remove the node with the lowest F score from the open list
        //The list is sorted, therefore removing the first node will do...
        PathNode* lowestScoreNode = *openList.begin();
        core::list<PathNode*>::Iterator toErase = openList.begin();
        openList.erase(toErase);
        lowestScoreNode->list = PIL_CLOSED;

        if (log)
        {
            soubor << "Lowest F score node in open list is: " <<
            lowestScoreNode->position.X << "," << lowestScoreNode->position.Y <<
            "," << lowestScoreNode->position.Z << ", F score: " <<
            lowestScoreNode->Fdist << ", G distance: " << lowestScoreNode->Gdist <<
            ::endl << "Adding neighbours to open list" << ::endl;
        }

        //Iterate through the neighbours of the PathNode
        core::list<PathNode*>::Iterator iter2 = lowestScoreNode->neighbours.begin();
        u32 i = 1;
        for (;iter2 != lowestScoreNode->neighbours.end(); ++i)
        {
            if (log) soubor << "    Neighbour " << i << ": " <<
                (*iter2)->position.X << "," << (*iter2)->position.Y << "," <<
                (*iter2)->position.Z;

            //If the neighbour is the end node, set its parent and don't care
            //about anything anymore
            if (*iter2 == endNode){
                endNode->parent = lowestScoreNode;
                pathFound = true;
                if (log) soubor << ", is the end node, leaving..." << ::endl;
                break;
            }

            //If the neighbour is on the closed list, leave
            if ((*iter2)->list == PIL_CLOSED)
            {
                if (log) soubor << ", is on the closed list, DISCARDING..." <<
                ::endl;
                ++iter2;
                continue;
            }

            //If the neighbour isn't on any of the lists, calculate its F and G,
            //set its parent to the current node, add it to open list and leave
            if ((*iter2)->list == PIL_NONE)
            {
                if (log) soubor << ", isn't on any of the lists, adding to open list..." <<
                ::endl;
                (*iter2)->Gdist = lowestScoreNode->distance(*iter2) + lowestScoreNode->Gdist;
                (*iter2)->Fdist = (*iter2)->Gdist + (*iter2)->Hdist(endNode);
                (*iter2)->parent = lowestScoreNode;
                (*iter2)->list = PIL_OPEN;
                //Add node to open list
                if (openList.getSize() == 0) {openList.push_back(*iter2);}
                else {
                    core::list<PathNode*>::Iterator addBefore = openList.begin();
                    while (addBefore != openList.end() && (*addBefore)->Fdist < (*iter2)->Fdist) addBefore++;
                    if (addBefore == openList.end()) openList.push_back(*iter2);
                    else openList.insert_before(addBefore, *iter2);
                }
                ++iter2;
                continue;
            }

            //Now the node can only be in the open list, so we just check if our
            //current path isn't better than the older path of the node. If it
            //is, we just change the parent of the node and its position in the
            //open list
            if (log) soubor << ", is already on the open list";
            u32 Gtemp = lowestScoreNode->distance(*iter2) + lowestScoreNode->Gdist;

            if (Gtemp < (*iter2)->Gdist)    //If the current path is better...
            {
                if (log) soubor << ", and is better, changing parent..." << ::endl;
                PathNode* currentNode = *iter2;
                currentNode->Gdist = Gtemp;
                currentNode->Fdist = Gtemp + currentNode->Hdist(endNode);
                currentNode->parent = lowestScoreNode;
                core::list<PathNode*>::Iterator toErase = openList.begin();
                while (*toErase != currentNode) toErase++;
                openList.erase(toErase);

                //Add node to open list
                if (openList.getSize() == 0) {openList.push_back(currentNode);}
                else {
                    core::list<PathNode*>::Iterator addBefore = openList.begin();
                    while (addBefore != openList.end() && (*addBefore)->Fdist < currentNode->Fdist) addBefore++;
                    if (addBefore == openList.end()) openList.push_back(currentNode);
                    else openList.insert_before(addBefore, currentNode);
                }
            }
            else {
                if (log) soubor << ", and is not better, going on..." << ::endl;
            }

            ++iter2;
        }
        if (pathFound) break;

        passCount++;
    }

    pathSegments.clear();
    if (pathFound)
    {
        if (log) soubor << ::endl << "Path exists and FOUND." << ::endl;
        PathNode* prevPathNode = endNode;
        while (prevPathNode != NULL)
        {
            pathSegments.push_front(prevPathNode);
            prevPathNode = prevPathNode->parent;
        }
    } else if (log) soubor << ::endl << "Path NOT FOUND." << ::endl;

    if (log)
    {
        core::list<PathNode*>::Iterator iter = pathSegments.begin();
        u32 nn = 1;
        while (iter != pathSegments.end())
        {
            soubor << "PathNode " << nn << ": " << (*iter)->position.X << "," <<
            (*iter)->position.Y << "," << (*iter)->position.Z << ::endl;
            iter++;
            nn++;
        }
        timeStats = timer->getTime() - timeStats;
        soubor << "Pathfinding took " << timeStats << " timer ticks (milliseconds).";
    }

    state = 0;
    soubor.close();
    return pathFound;
}

// Saves the pathfinding net to a file
void Network::saveNetToFile(c8* filename)
{
    ofstream soubor(filename);

    PathNode* helper;
    core::list<PathNode*>::Iterator iter = nodes.begin();

    soubor << nodes.getSize() << ::endl;

    while (iter != nodes.end())
    {
        helper = *iter;
        soubor << (helper->position.X) << ::endl;
        soubor << (helper->position.Y) << ::endl;
        soubor << (helper->position.Z) << ::endl;
        soubor << (helper->neighbours.getSize()) << ::endl;

        core::list<PathNode*>::Iterator iterNeigh = helper->neighbours.begin();

        while (iterNeigh != helper->neighbours.end())
        {
            soubor << ((*iterNeigh)->position.X) << ::endl;
            soubor << ((*iterNeigh)->position.Y) << ::endl;
            soubor << ((*iterNeigh)->position.Z) << ::endl;
            iterNeigh++;
        }

        iter++;
    }

    soubor.close();
}

// Loads pathfinding network from a file
void Network::loadNetFromFile(c8* filename)
{
    reinitializeNet();

    ifstream soubor;
    soubor.open(filename);
    if (!soubor) return;

    u32 ns;
    soubor >> ns;

    for (u32 i = 0; i != ns; i++)
    {
        core::vector3df newpos;
        soubor >> newpos.X;
        soubor >> newpos.Y;
        soubor >> newpos.Z;
        addNode(newpos);

        u32 n;
        soubor >> n;
        float unused;

        for (u32 j = 0; j != n; j++)
        {
            soubor >> unused; soubor >> unused; soubor >> unused;
        }
    }

    soubor.close();

    soubor.open(filename);

    soubor >> ns;

    for (i = 0; i != ns; i++)
    {
        core::vector3df thispos;
        soubor >> thispos.X;
        soubor >> thispos.Y;
        soubor >> thispos.Z;
        PathNode* thispoint = nearestNodePointer(thispos);

        u32 n;
        soubor >> n;

        for (u32 j = 0; j != n; j++)
        {
            core::vector3df otherpos;
            soubor >> otherpos.X;
            soubor >> otherpos.Y;
            soubor >> otherpos.Z;

            thispoint->setNeighbour(nearestNodePointer(otherpos));
        }
    }

    soubor.close();
}

// Reinitializes the Network, deleting all pointers and lists
void Network::reinitializeNet()
{
    core::list<PathNode*>::Iterator iter = nodes.begin();
    while (iter != nodes.end())
    {
        delete *iter;
        iter++;
    }

    nodes.clear();
    pathSegments.clear();

    startNode = NULL;
    endNode = NULL;
    selectedNode = NULL;

    return;
}
irr::core::vector3df Network::FindRandNode()
{
	irr::core::vector3df p;
	int r = rand()%(nodes.getSize()-1); //l蕐 v