A*寻路算法源码

A*寻路算法源码

---

Astar.h源码

#pragma once
#include "stdafx.h"
#include "MinHeap.h" //最小堆源码地址http://blog.csdn.net/heyaolongsanhao/article/details/53437325
class AstarNodeRun;
class Astar;
class AstarRunData;
class AstarNode
{
public:
    int nodeIndex;
    int connectNum;
    AstarNode** connects;
    AstarNode(int nodeIndex):connectNum(0),connects(NULL){this->nodeIndex = nodeIndex;}
    virtual double getH(AstarNode* target) = 0;
    virtual double getCost(int i) = 0; //调用runData的openConnNode

};
class AstarNodeRun : public MinHeapNode
{
public:
    double g;
    double h; 
    AstarNode* node;
    AstarNodeRun* parent;
    unsigned short pathID; //和AstarRunData的pathID相同表示该节点访问过
    bool isOpen; //pathID和AstarRunData的pathID相同的情况下，isOpen为true表示在open表，否则在close表
    AstarNodeRun():MinHeapNode(0),g(0),h(0),node(NULL),parent(NULL),pathID(0),isOpen(false)
    {
    }
    double f()
    {
        return g+h;
    }
};

class AstarRunData //这个是可以复用的，用pathID就不需要清理AstartNodeRun的状态
{
public:
    Astar* astar;
    //用pathID来区分close表里和未访问过的节点
    //如果AstarNodeRun的pathID和AstarRunData的pathID相同表示已经访问过
    //如果AstarNodeRun的isOpen为true，表示在close表里，如果isOpen为false表示在open表里
    //如果AstarNodeRun的pathID和AstarRunData的pathID不同，表示未访问过
    //用pathID的好处是，只在访问到的时候根据pathID来判定AstarNodeRun里的数据是否是脏数据
    //在AstarNode数量很多的时候，每次寻路需要清理所有AstarNodeRun的访问状态
    //或者采取每次访问到一个AstarNode，动态生成一个AstarRunData，消耗都比较大
    unsigned short pathID;
    AstarNodeRun** runNodes;
    int nodeCount;
    MinHeap* openNodesHeap; //open表
    AstarRunData(int nodeNum, AstarNode** nodes);
    void clearPathId();
    void init(Astar* p);
    ~AstarRunData();

};

class Astar
{
private:
    static unsigned short nextPathID;
public:
    unsigned short pathID;
    AstarRunData* runData;
    AstarNode* startNode;
    AstarNode* endNode;
    int searchedNodes;

    Astar();
    AstarNodeRun* getNodeRun(int nodeIndex);
    void init(AstarRunData* runData, AstarNode* start, AstarNode* end);
    vector<AstarNode*> findPath();
    ~Astar();
};

Astar.cpp源码

#include "stdafx.h"
#include "Astar.h"
#include "stdafx.h"
#include "MinHeap.h"

AstarRunData::AstarRunData(int nodeNum, AstarNode** nodes):astar(NULL),pathID(0)
{
    this->nodeCount = nodeNum;
    runNodes = new AstarNodeRun*[nodeCount];
    openNodesHeap = new MinHeap(512);
    for(int i=0;i<nodeCount;i++)
    {
        runNodes[i] = new AstarNodeRun();
        runNodes[i]->node = nodes[i];
    }
}
void AstarRunData::clearPathId()
{
    for(int i=0;i<nodeCount;i++)
    {
        runNodes[i]->pathID = 0;
    }
}
void AstarRunData::init(Astar* p)
{
    astar= p;
    pathID = p->pathID;
    openNodesHeap->clear();
}
AstarRunData::~AstarRunData()
{
    astar = NULL;
    if(runNodes != NULL)
    {
        delete runNodes;
        runNodes = NULL;
    }
}

unsigned short Astar::nextPathID = 0;
Astar::Astar():runData(NULL),startNode(NULL),endNode(NULL),searchedNodes(0)
{
    pathID = nextPathID++; 
    if(pathID == 0) //永远不用0
    {
        pathID = nextPathID++;
    }
}
AstarNodeRun* Astar::getNodeRun(int nodeIndex)
{
    if(nodeIndex >=0)
    {
        return runData->runNodes[nodeIndex];
    }
    return NULL;
}

void Astar::init(AstarRunData* runData, AstarNode* start, AstarNode* end)
{
    this->runData = runData;
    //AstarNodeRun的pathID必然小于runData的pathID，如果Astar.pathID<runData->pathID
    //就可能会导致Astar.pathID和AstarNodeRun的pathID相同，所以需要清理下
    if(pathID < runData->pathID) 
    {
        runData->clearPathId();
    }
    runData->init(this);
    this->startNode = start;
    this->endNode = end;



}
vector<AstarNode*> Astar::findPath()
{
    vector<AstarNode*> path;

    AstarNodeRun* startNodeRun = getNodeRun(startNode->nodeIndex);
    startNodeRun->pathID = pathID;
    startNodeRun->parent = NULL;
    startNodeRun->g = 0;
    startNodeRun->h = startNode->getH(endNode);
    startNodeRun->nodeValue = startNodeRun->f();

    AstarNodeRun* currentNodeRun = startNodeRun;
    while(true)
    {
        searchedNodes++;
        if(currentNodeRun->node == endNode)
        {
            AstarNodeRun* c = currentNodeRun;
            while(c != NULL)
            {
                path.push_back(c->node);
                c = c->parent;
            }
            std::reverse(path.begin(),path.end()); //反转下路径
            break;
        }
        int connectNum = currentNodeRun->node->connectNum;
        for(int i=0; i<connectNum; i++)
        {
            AstarNode* nextNode = currentNodeRun->node->connects[i];
            double cost = currentNodeRun->node->getCost(i);
            if(cost < 0)
            {
                continue;
            }
            AstarNodeRun* nextNodeRun = getNodeRun(nextNode->nodeIndex);
            if(nextNodeRun->pathID != runData->pathID) //还没访问过nextNodeRun,通过pathID不同表示没访问过，这样每次寻路的时候，不需要清NodeRun的状态（或新建NodeRun)
            {
                nextNodeRun->parent = currentNodeRun;
                nextNodeRun->pathID = runData->pathID;
                nextNodeRun->h = nextNodeRun->node->getH(endNode);
                nextNodeRun->g = currentNodeRun->g + cost;
                nextNodeRun->nodeValue = nextNodeRun->f();
                nextNodeRun->isOpen = true;
                runData->openNodesHeap->add(nextNodeRun);
            }
            else 
            {
                //如果nextNodeRun已经访问过，且nextNodeRun的新g更小的话，就加入到最小堆里
                if (currentNodeRun->g + cost < nextNodeRun->g)
                {
                    nextNodeRun->parent = currentNodeRun;
                    nextNodeRun->g = currentNodeRun->g + cost; //已经访问过的H不变
                    nextNodeRun->nodeValue = nextNodeRun->f();
                    if(nextNodeRun->isOpen)
                    {
                        runData->openNodesHeap->up(nextNodeRun->nodeIndex);
                    }
                    else
                    {
                        runData->openNodesHeap->add(nextNodeRun);
                    }
                }
            }
        }

        if(runData->openNodesHeap->size() ==0)
        {
            break;
        }
        currentNodeRun = (AstarNodeRun*) runData->openNodesHeap->removeMin();
        currentNodeRun->isOpen = false;
    }

    return path;
}

Astar::~Astar()
{
    startNode = NULL;
    endNode = NULL;
}

NaviGraph.h源码

#include "stdafx.h"
#include "Astar.h"

#pragma once
struct Int2
{
public:
    Int2(){x = 0; y = 0;}
    int x;
    int y;
};
class TriangleAstarNode : public AstarNode
{
public:
    Int2* vectices;
    TriangleAstarNode** triangleConnects;
    double* costs;
    Int2 center;
public:
    TriangleAstarNode(Int2* vectices, int nodeIndex):AstarNode(nodeIndex)
    {
        this->vectices = vectices;
        center.x= 0;
        center.y = 0;
        for(int i=0; i<3; i++)
        {
            Int2 targetCenter = vectices[i];
            center.x += targetCenter.x;
            center.y += targetCenter.y;
        }
        center.x /= 3;
        center.y /= 3;
        costs = new double[3];
    }
    void setConnects(TriangleAstarNode** connects, int connectNum)
    {
        this->triangleConnects =  connects;
        this->connects = (AstarNode**)(this->triangleConnects);
        this->connectNum = connectNum;
        for(int i=0; i<connectNum;i++)
        {
            if(connects[i] != NULL)
            {
                Int2 pos = connects[i]->center;
                costs[i] = sqrt(1.0*((pos.x-center.x)*(pos.x- center.x) + (pos.y-center.y)*(pos.y- center.y)));
            }
            else
            {
                costs[i] = -1;
            }
        }
    }
    virtual double getH(AstarNode* target)
    {
        Int2 targetPos = ((TriangleAstarNode*)target)->center;
        return sqrt(1.0*((targetPos.x-center.x)*(targetPos.x-center.x)+(targetPos.y-center.y)*(targetPos.y-center.y)));
    }
    virtual double getCost(int i)
    {
        return costs[i];
    }

};

class NaviGraph
{
public:
    AstarNode** astarNodes; //没有用AstarNode**，这样分配的数组是连片的内存
    int nodeCount;
    AstarRunData* runData;
    NaviGraph():astarNodes(NULL),nodeCount(0),runData(NULL){}
    void init(int nodeCount, AstarNode** astarNodes)
    {
        this->nodeCount = nodeCount;
        this->astarNodes = astarNodes;
        this->runData = new AstarRunData(nodeCount, astarNodes);
    }
};

PathFinder源码

#pragma once
#include "stdafx.h"
#include "NaviGraph.h"
class PathFinder
{
public:
    NaviGraph* graph;
    PathFinder()
    {
        graph = new NaviGraph();
    }
    vector<AstarNode*> findPath(AstarNode* startNode, AstarNode* endNode)
    {
        Astar* astar= new Astar();
        astar->init(graph->runData, startNode, endNode);
        return astar->findPath();
    }
};

AstarTest.h源码

#include "stdafx.h"
#include "Astar.h"
#include "NaviGraph.h"
#include "PathFinder.h"
#include <iostream>
using namespace std;

class AstarTest
{
public:
    void doTest()
    {
        Int2 v[7];
        v[0].x = 200;v[0].y = 0;
        v[1].x = 0;v[1].y = 200;
        v[2].x = 200;v[2].y = 200;
        v[3].x = 100;v[3].y = 500;
        v[4].x = 400;v[4].y = 400;
        v[5].x = 600;v[5].y = 700;
        v[6].x = 800;v[6].y = 100;

        int nodeVIndexs[][3] = {{0,1,2},{1,3,2},{2,3,4},{4,3,5},{4,5,6},{0,4,6},{0,2,4}};
        TriangleAstarNode** nodes = new TriangleAstarNode*[7];
        for(int i=0;i<7;i++)
        {
            Int2 nodeV[3];
            nodeV[0] = v[nodeVIndexs[i][0]];
            nodeV[1] = v[nodeVIndexs[i][1]];
            nodeV[2] = v[nodeVIndexs[i][2]];
            nodes[i] = new TriangleAstarNode(nodeV,i);
        }
        int nodeCIndexs[][3] = {{-1,1,6},{-1,2,0},{1,3,6},{2,-1,4},{3,-1,5},{6,4,-1},{0,2,5}};
        for(int i=0; i<7;i++)
        {
            TriangleAstarNode** connects = new TriangleAstarNode*[3];
            connects[0] = (nodeCIndexs[i][0]>=0?nodes[nodeCIndexs[i][0]]:NULL);
            connects[1] = (nodeCIndexs[i][1]>=0?nodes[nodeCIndexs[i][1]]:NULL);
            connects[2] = (nodeCIndexs[i][2]>=0?nodes[nodeCIndexs[i][2]]:NULL);
            nodes[i]->setConnects(connects,3);
        }
        PathFinder* pathFinder = new PathFinder();
        pathFinder->graph->init(7,(AstarNode**)nodes);
        vector<AstarNode*> nodePath = pathFinder->findPath(nodes[3], nodes[5]);
        for(int i=0; i<nodePath.size(); i++)
        {
            cout << nodePath[i]->nodeIndex << " ";
        }
        cout <<endl;
    }
};