A*寻路算法源码

---

Astar.h源码

#pragma once#include "stdafx.h"#include "MinHeap.h" //最小堆源码地址http://blog.csdn.net/heyaolongsanhao/article/details/53437325class 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 oncestruct 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;    }};