A*算法源码

这几天大体完成了服务器框架的代码，闲下来看了下A*算法，也在断断续续中把自己的源码写完，这里记录下，以后可以回顾：

基本的原理我这里也不在阐述了，这篇翻译还是不错，只是其中有些遗漏罢了：
http://www.cppblog.com/mythit/archive/2009/04/19/80492.aspx
这篇对几个算法有更详细的介绍：
http://blog.csdn.net/niteip/article/details/7519609

下面直接贴代码吧，代码里面有比较详细的注释，三个文件：main.cpp, AstarTest.h, AstarTest.cpp,欢迎拍砖指正，毕竟有批评才会进步嘛，^-^!.最后一个疑问就是不明白加权曼哈顿距离的好处在哪里，希望知道的告知一下，谢谢~

AstarTest.h

/* A*搜寻算法，俗称A星算法。这是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。常用于游戏中的NPC（Non-Player-ControlledCharacter）的移动计算，或线上游戏的BOT（ROBOT）的移动计算上。该算法像Dijkstra算法一样，可以找到一条最短路径；也像BFS一样，进行启发式的搜索。A*算法是一种启发式搜索算法，启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。A*算法的公式为：f(n)=g(n)+h(n)，g(n)表示从起点到任意顶点n的实际距离，h(n)表示任意顶点n到目标顶点的估算距离。 这个公式遵循以下特性：如果h(n)为0，只需求出g(n)，即求出起点到任意顶点n的最短路径，则转化为单源最短路径问题，即Dijkstra算法如果h(n)<=“n到目标的实际距离”，则一定可以求出最优解。而且h(n)越小，需要计算的节点越多，算法效率越低。对于函数h(n)，估算距离常用的方法有：曼哈顿距离：定义曼哈顿距离的正式意义为L1-距离或城市区块距离，也就是在欧几里德空间的固定直角坐标系上两点所形成的线段对轴产生的投影的距离总和。例如在平面上，坐标（x1,y1）的点P1与坐标（x2, y2）的点P2的曼哈顿距离为：|x1 - x2| + |y1 - y2|。欧氏距离：是一个通常采用的距离定义，它是在m维空间中两个点之间的真实距离。在二维和三维空间中的欧氏距离的就是两点之间的距离。例如在平面上，坐标（x1,y1）的点P1与坐标（x2, y2）的点P2的欧氏距离为： sqrt((x1-x2)^2+(y1-y2)^2 )。切比雪夫距离：是两个向量之间各分量差值的最大值。例如在平面上，坐标（x1, y1）的点P1与坐标（x2, y2）的点P2的切比雪夫距离为：max(|x1 - x2| , |y1 - y2|)。注：关于是否能穿过对角线的规则，是自己定义的该工程分两种方式寻路：直线寻路：只能 直上直下直左直右对角线寻路：包括直线寻路，额外加上走对角线路线参考网页地址：http://blog.csdn.net/niteip/article/details/7519609感谢作者*/#pragma once#include <iostream>#include <queue>#include <math.h>using namespace std;//定义是否可以走对角线的宏#define WALK_DIAGONAL#ifdef WALK_DIAGONAL#define NeighborNum 8       //邻居个数static int offset[NeighborNum][2]={{1,0},{1,1},{1,-1},{-1,0},{-1,-1},{-1,1},{0,-1},{0,1}};//邻居的坐标选择#else#define NeighborNum 4   //邻居个数static int offset[NeighborNum][2]={{1,0},{-1,0},{0,-1},{0,1}};//邻居的坐标选择#endif//定义二维数组的长度#define RowNumber 6#define ColumnNumber 8//定义曼哈顿距离的扩大比例#define ManhattanSacle 10 //10倍enum NodeType{    Node_Start = 0,     //起始点    Node_Barrier,       //障碍物，把所有不能通过的点都认为是障碍物    Node_Way,           //可通行的路    Node_FindWay,       //经过寻路后，会经过的点    Node_End,           //终点    Node_Null,          //未定义的点};//定义找到的路径的符号表示#define FindWay '*' //最终找到的路径的符号表示#define HaveFindPlace 'o'//找过的地方的符号表示//定义一个节点的数据结构struct myNode{    myNode()    {        m_ParentNode = NULL;        m_Type = Node_Null;        m_X = m_Y = 0;        m_Gvalue = 0;        m_Hvalue = 0;        m_ParentNode = NULL;        m_IsFind = false;    }    char m_cSymbol;     //该节点的符号    int m_Type;         //该节点的类型    int m_X;            //节点的X坐标    int m_Y;            //节点的Y坐标    float m_Gvalue;     //g(x)  G值    float m_Hvalue;     //h(x)  H值    myNode* m_ParentNode;   //该节点指向的父节点    bool m_IsFind;      //该节点是否被找过    //按F值（= G + H） 从小到大排序，F值小的优先级高    friend bool operator<(myNode nNodeA,myNode nNodeB)    {        return (nNodeA.m_Gvalue+nNodeA.m_Hvalue > nNodeB.m_Gvalue+nNodeB.m_Hvalue);    }    //重载操作符 ==     friend bool operator==(myNode nNodeA,myNode nNodeB)    {        return (nNodeA.m_X == nNodeB.m_X &&  nNodeA.m_Y == nNodeB.m_Y);    }};class A_Star{public:    A_Star(const char pImportMap[RowNumber][ColumnNumber]);    ~A_Star();protected:    //初始化地图    bool initMap(const char pImportMap[RowNumber][ColumnNumber]);    //曼哈顿距离为：|x1 - x2| + |y1 - y2|    float manhattanDis(int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y);    //加权曼哈顿距离 better  不大清楚为什么用这个    float jiaquan_manhattanDis(int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y);    //欧氏距离为： sqrt((x1-x2)^2+(y1-y2)^2 )    float euclideanDis(int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y);    //切比雪夫距离为：max(|x1 - x2| , |y1 - y2|)    float chebyshevDis(int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y);    //大小比较函数    int max(int a,int b)    {        return a>b?a:b;    }//对外接口public:    //判断节点是否超过边界    //return true 超过边界，false：未超过    bool isOverBorder(int nX,int nY);    //A*核心算法    void AstarAlgorithm();    //找到路线路线    bool findRoute();    //打印出路线    void printRoute();private:    myNode  m_StarNode[RowNumber][ColumnNumber];    //存储所有节点数    myNode  m_StarStart;        //开始节点    myNode  m_StartEnd;         //终点节点};

AstarTest.cpp

#include "AstarTest.h"A_Star::A_Star(const char pImportMap[RowNumber][ColumnNumber]){    if (!initMap(pImportMap))    {        exit(0);    }}A_Star::~A_Star(){}bool A_Star::initMap( const char pImportMap[RowNumber][ColumnNumber] ){    bool isHaveStart,isHaveEnd;    isHaveStart = isHaveEnd = false;//检测是否有起点和终点    for(int i=0;i<RowNumber;i++)    {        for(int j=0;j<ColumnNumber;j++)        {            m_StarNode[i][j].m_cSymbol = pImportMap[i][j];            m_StarNode[i][j].m_X=i;            m_StarNode[i][j].m_Y=j;            if(m_StarNode[i][j].m_cSymbol=='s')            {                m_StarNode[i][j].m_Type = Node_Start;                m_StarNode[i][j].m_Gvalue=0;                m_StarNode[i][j].m_IsFind=true;                isHaveStart = true;                                 m_StarStart=m_StarNode[i][j];            }            else if(m_StarNode[i][j].m_cSymbol=='e')            {                m_StarNode[i][j].m_Type = Node_End;                m_StartEnd=m_StarNode[i][j];                isHaveEnd = true;                   }            else if(m_StarNode[i][j].m_cSymbol=='x')            {                m_StarNode[i][j].m_Type = Node_Barrier;            }            else            {                m_StarNode[i][j].m_Type = Node_Way;            }        }    }    if (!isHaveStart)    {        cout<<"Where will We Start"<<endl;    }    else if(!isHaveEnd)    {        cout<<"Where will We Go ?"<<endl;    }    return (isHaveStart&&isHaveEnd);}bool A_Star::isOverBorder(int nX,int nY){    if (nX < 0 || nX >= RowNumber || nY < 0 || nY >= ColumnNumber)    {        return true;    }    return false;}//曼哈顿距离为：|x1 - x2| + |y1 - y2|float A_Star::manhattanDis(int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y){    float hVal;    hVal=(float)abs(nDisStart_x-nDisEnd_x)+abs(nDisStart_y-nDisEnd_y);    hVal*=ManhattanSacle;    return hVal;}//加权曼哈顿 betterfloat A_Star::jiaquan_manhattanDis( int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y ){    float hVal,dx,dy;    dx=(float)abs(nDisStart_x-nDisEnd_x);    dy=(float)abs(nDisStart_y-nDisEnd_y);    if(dx>dy)        hVal=10*dx+6*dy;    else        hVal=6*dx+10*dy;    return hVal;}//欧氏距离为： sqrt((x1-x2)^2+(y1-y2)^2 )float A_Star::euclideanDis( int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y ){    float hVal;    hVal=(float)sqrt(pow((nDisStart_x-nDisEnd_x),2)+pow((nDisStart_y-nDisEnd_y),2));    return hVal;}//切比雪夫距离为：max(|x1 - x2| , |y1 - y2|)float A_Star::chebyshevDis( int nDisStart_x,int nDisStart_y,int nDisEnd_x,int nDisEnd_y ){    float hVal;    hVal=(float)max(abs(nDisStart_x-nDisEnd_x),abs(nDisStart_y-nDisEnd_y));    return hVal;}void A_Star::AstarAlgorithm(){    priority_queue<myNode> openQueue;//open表    openQueue.push(m_StarStart);//将起始点压入open表    myNode currNode,*neighborNode;    int nNeighborX,nNeighborY;  //邻居的x，y坐标, 邻居相对于选定点的G值偏移    float offsetG;    while(!openQueue.empty())//oepn表不为空一直循环    {        currNode = openQueue.top();//每次有元素插入都会进行排序，myNode 重载了操作运算符 < ，每次队列头的元素的F值最小        if (currNode == m_StartEnd)//当前节点为终点，退出        {            break;        }        openQueue.pop();//出队列    //  printRoute();        for (int i=0;i<NeighborNum;++i)//遍历该节点的邻居        {            //根据所有偏移量找到邻居            nNeighborX = currNode.m_X + offset[i][0];            nNeighborY = currNode.m_Y + offset[i][1];            //邻居节点超出边界，障碍物，查找过了都跳过            if (isOverBorder(nNeighborX,nNeighborY))            {                continue;            }            neighborNode = &m_StarNode[nNeighborX][nNeighborY];//找到邻居节点的信息            if (neighborNode->m_IsFind || neighborNode->m_Type == Node_Barrier)            {                continue;            }            //这里 可以替换不同的算法，选择想用的h(x)            neighborNode->m_Hvalue = jiaquan_manhattanDis(nNeighborX,nNeighborY,m_StartEnd.m_X,m_StartEnd.m_Y);            offsetG = 1.0f;//不是对角线的邻居，默认 1 个距离，相当于一个格子            if (offset[i][0]!=0 && offset[i][1]!=0)//说明是对对角线上的邻居            {                offsetG = 1.4f;//取近似值,利于计算,1.4个格子            }            //还没压入open表 或者            //如果该邻居之前的G值(G值是重新选择父亲节点前计算得到的)小于从新计算的G值(以当前点为父节点计算的)            //该邻居节点的父节点要做更改，指向当前节点作为父节点, 因为这个估值更小，代价更低            if(neighborNode->m_Gvalue == 0 || neighborNode->m_Gvalue>currNode.m_Gvalue+offsetG)                             {                neighborNode->m_ParentNode = &m_StarNode[currNode.m_X][currNode.m_Y];                neighborNode->m_Gvalue = currNode.m_Gvalue+offsetG;            }            neighborNode->m_IsFind = true;//被查找过了            openQueue.push(*neighborNode);        }    }}bool A_Star::findRoute(){    myNode* pNode = m_StarNode[m_StartEnd.m_X][m_StartEnd.m_Y].m_ParentNode;//最后节点，非终点    if (pNode == NULL)    {        cout<<"坑爹啊？根本没有路径可以到达终点"<<endl;        return false;    }    //根据终点，一个个往回找    while (pNode)    {        if (pNode->m_ParentNode)        {            pNode->m_Type = Node_FindWay;        }        pNode = pNode->m_ParentNode;    }    return true;}void A_Star::printRoute(){    int nFindNum = 0;//统计遍历的节点数    for(int i=0;i<RowNumber;i++)    {        for(int j=0;j<ColumnNumber;j++)        {            if (m_StarNode[i][j].m_IsFind)//统计遍历的节点数            {                nFindNum++;            }            if(m_StarNode[i][j].m_Type==Node_FindWay)//找到的路                cout<<FindWay;//<<"F:"<<m_StarNode[i][j].m_Gvalue+m_StarNode[i][j].m_Hvalue;            else if(m_StarNode[i][j].m_IsFind)//找过的地方            {                if (m_StarNode[i][j].m_Type == Node_Way)                {                    cout<<HaveFindPlace;//<<"F:"<<m_StarNode[i][j].m_Gvalue+m_StarNode[i][j].m_Hvalue;                }                else                {                    cout<<m_StarNode[i][j].m_cSymbol;                }            }            else                cout<<m_StarNode[i][j].m_cSymbol;        }        cout<<endl;    }    cout<<"共找了 "<<nFindNum<<" 个节点，终于找到了路"<<endl;}

Main.cpp

#include "AstarTest.h"int main(){    //自定义一个地图 （RowNumber * ColumnNumber）大小    //'s':起始点字符    //'e':终点字符    //'x':障碍物字符    //'.':可通过点    char chMapDraw[RowNumber][ColumnNumber] =    {             { '.','.','.','.','.','.','.','.'},          { '.','.','.','.','x','.','.','.'},          { '.','.','s','.','x','.','e','.'},          { '.','.','.','.','x','.','.','.'},          { '.','.','.','.','.','.','.','.'},          { '.','.','.','.','.','.','.','.'},      };    //初始化地图    A_Star Astar(chMapDraw);    //A*算法    Astar.AstarAlgorithm();    //找到路径    if (Astar.findRoute())    {        Astar.printRoute();//打印路径    }    getchar();}