A*算法的C++实现

算法的介绍在：http://www.vckbase.com/document/viewdoc/?id=1422

英文地址：http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003

本文的代码是完全按照上面的介绍文章写的，包括使用的数据也是文中6*8的矩阵。这只是一个最简单的实现，当数据规模变大后，“开启列表”的策略需要相应调整。除此外感觉算法的模块化和封装写的不好，请路过的读者指教。

---

#include<iostream>
#include<cmath>
#include<deque>
using namespace std;

const int ROW = 6;
const int COLUMN = 8;
enum status{open,closed,obstacle,unsorted};
struct node{
    int row;
    int col;
    node* father;
    int gvalue;
    int hvalue;
    int fvalue;
    status st;
};
node map[ROW][COLUMN];
deque<node*> openTable;
deque<node*> closeTable;

int init(node source,node dest)
{
    for(int i=0;i<ROW;i++)
    {
        for(int j=0;j<COLUMN;j++)
        {
            map[i][j].row = i;
            map[i][j].col = j;
            map[i][j].father = NULL;
            map[i][j].hvalue = (abs(i-dest.row)+abs(j-dest.col))*10;
            map[i][j].st = unsorted;
        }
    }
    //障碍物

　 map[1][3].st = obstacle;
    map[2][3].st = obstacle;
    map[3][3].st = obstacle;

    map[source.row][source.col].st = open;
    map[source.row][source.col].gvalue = 0;
    openTable.push_back(&map[source.row][source.col]);
}

//把当前扩展点周围没有进入“开启列表”的节点加入开启列表，并更新G值

void openNeighbor(int row,int col

{
    deque<node*>::iterator it;
    for(it=openTable.begin();it<openTable.end();it++)
    {
        if((*it)==(&map[row][col]))
        {
            map[row][col].st = closed;
            openTable.erase(it);
            closeTable.push_back(&map[row][col]);
            break;
        }
    }
    //上
    if(row-1>=0)
    {
        if(map[row-1][col].st == open)//检查该节点是否有更优的G值

        {
            if(map[row-1][col].gvalue>map[row][col].gvalue+10)
            {
                map[row-1][col].gvalue = map[row][col].gvalue+10;
                map[row-1][col].father = &map[row][col];
            }
        }
        else if(map[row-1][col].st == unsorted)
        {
            map[row-1][col].st = open;
            map[row-1][col].father = &map[row][col];
            map[row-1][col].gvalue = 10+map[row][col].gvalue;
            openTable.push_back(&map[row-1][col]);
        }
    }
    //右
    if(col+1<COLUMN)
    {
        if(map[row][col+1].st == open)
        {
            if(map[row][col+1].gvalue>map[row][col].gvalue+10)
            {
                map[row][col+1].gvalue = map[row][col].gvalue+10;
                map[row][col+1].father = &map[row][col];
            }
        }
        else if(map[row][col+1].st == unsorted)
        {
            map[row][col+1].st = open;
            map[row][col+1].father = &map[row][col];
            map[row][col+1].gvalue = 10+map[row][col].gvalue;
            openTable.push_back(&map[row][col+1]);
        }
    }
    //下
    if(row+1<ROW)
    {
        if(map[row+1][col].st == open)
        {
            if(map[row+1][col].gvalue>map[row][col].gvalue+10)
            {
                map[row+1][col].gvalue = map[row][col].gvalue+10;
                map[row+1][col].father = &map[row][col];
            }
        }
        else if(map[row+1][col].st == unsorted)
        {
            map[row+1][col].st = open;
            map[row+1][col].father = &map[row][col];
            map[row+1][col].gvalue = 10+map[row][col].gvalue;
            openTable.push_back(&map[row+1][col]);
        }
    }
    //左
    if(col-1>=0)
    {
        if(map[row][col-1].st == open)
        {
            if(map[row][col-1].gvalue>map[row][col].gvalue+10)
            {
                map[row][col-1].gvalue = map[row][col].gvalue+10;
                map[row][col-1].father = &map[row][col];
            }
        }
        else if(map[row][col-1].st == unsorted)
        {
            map[row][col-1].st = open;
            map[row][col-1].father = &map[row][col];
            map[row][col-1].gvalue = 10+map[row][col].gvalue;
            openTable.push_back(&map[row][col-1]);
        }
    }
    //左上
    if(row-1>=0 && col-1>=0&& map[row-1][col].st != obstacle && map[row][col-1].st != obstacle)
    {
        if(map[row-1][col-1].st == open)
        {
            if(map[row-1][col-1].gvalue>map[row][col].gvalue+14)
            {
                map[row-1][col-1].gvalue = map[row][col].gvalue+14;
                map[row-1][col-1].father = &map[row][col];
            }
        }
        else if(map[row-1][col-1].st == unsorted)
        {
            map[row-1][col-1].st = open;
            map[row-1][col-1].father = &map[row][col];
            map[row-1][col-1].gvalue = 14+map[row][col].gvalue;
            openTable.push_back(&map[row-1][col-1]);
        }
    }
    //右上
    if(row-1>=0 && col+1<COLUMN && map[row-1][col].st != obstacle && map[row][col+1].st != obstacle)
    {
        if(map[row-1][col+1].st == open)
        {
            if(map[row-1][col+1].gvalue>map[row][col].gvalue+14)
            {
                map[row-1][col+1].gvalue = map[row][col].gvalue+14;
                map[row-1][col+1].father = &map[row][col];
            }
        }
        else if(map[row-1][col+1].st == unsorted)
        {
            map[row-1][col+1].st = open;
            map[row-1][col+1].father = &map[row][col];
            map[row-1][col+1].gvalue = 14+map[row][col].gvalue;
            openTable.push_back(&map[row-1][col+1]);
        }
    }
    //右下
    if(row+1<ROW && col+1<COLUMN && map[row+1][col].st != obstacle && map[row][col+1].st != obstacle)
    {
        if(map[row+1][col+1].st == open)
        {
            if(map[row+1][col+1].gvalue>map[row][col].gvalue+14)
            {
                map[row+1][col+1].gvalue = map[row][col].gvalue+14;
                map[row+1][col+1].father = &map[row][col];
            }
        }
        else if(map[row+1][col+1].st == unsorted)
        {
            map[row+1][col+1].st = open;
            map[row+1][col+1].father = &map[row][col];
            map[row+1][col+1].gvalue = 14+map[row][col].gvalue;
            openTable.push_back(&map[row+1][col+1]);
        }
    }
    //左下
    if(row+1<ROW && col-1>=0&& map[row+1][col].st != obstacle && map[row][col-1].st != obstacle)
    {
        if(row+1<ROW && col-1>=0 && map[row+1][col-1].st == open && map[row+1][col].st != obstacle && map[row][col-1].st != obstacle)
        {
            if(map[row+1][col-1].gvalue>map[row][col].gvalue+14)
            {
                map[row+1][col-1].gvalue = map[row][col].gvalue+14;
                map[row+1][col-1].father = &map[row][col];
            }
        }
        else if(row+1<ROW && col-1>=0 && map[row+1][col-1].st)
        {
            map[row+1][col-1].st = open;
            map[row+1][col-1].father = &map[row][col];
            map[row+1][col-1].gvalue = 14+map[row][col].gvalue;
            openTable.push_back(&map[row+1][col-1]);
        }
    }
}

bool findPath(node source,node dest)
{
    int minDistence;
    int pos;
    int rowNum,colNum;
    deque<node*>::iterator i;
    while(openTable.size()>0)
    {
        //反复遍历“开启列表”并寻找最下F值的节点

        minDistence = numeric_limits<int> ::max();
        for(i=openTable.begin();i<openTable.end();i++)
        {
            if((((node*)*i)->gvalue + ((node*)*i)->hvalue)<minDistence)
            {
                minDistence = ((node*)*i)->gvalue + ((node*)*i)->hvalue;
                rowNum = ((node*)*i)->row;
                colNum = ((node*)*i)->col;
            }
        }
        if(rowNum==dest.row && colNum==dest.col)//达到目标节点
        {
            openNeighbor(rowNum,colNum);
            return true;
        }
        else
        {
            openNeighbor(rowNum,colNum);
        }
    }
    return false;
}

void printPath(node source,node dest)
{
    deque<node*> dq;
    node* p = &map[dest.row][dest.col];
    while(!(p->row==source.row&&p->col==source.col))
    {
        dq.push_back(p);
        p=p->father;
    }
    dq.push_back(p);
    for(int i=dq.size()-1;i>=0;i--)
    {
        cout<<"("<<dq[i]->row<<","<<dq[i]->col<<")"<<endl;
    }
}

int main()
{
    node source,dest;
    source.row = 2;
    source.col = 1;
    dest.row = 2;
    dest.col = 5;

    init(source,dest);
    if(findPath(source,dest))
    {
        printPath(source,dest);
    }
    else
    {
        cout<<"不存在从起点到终点的路径！"<<endl;
    }

    system("pause");
    return 0;
}