最短路径的A*算法

这里原理就是根据一个g（到起始点的真实路径）、h（到目标点的真估计数值），用这个来衡量路径的好坏，这样找一个合适的h函数就是个问题，而且在运行时，套路是这样的，一个open表，一个close表，open用来存遍历了却还没有真正访问的点，close用来存访问过的点，每次都选open表中f函数最小的节点，拿出来访问，在遍历其周围的节点，把它再存到open表中，再循环直到找到终点为止。

我的A* 算法，跑一些复杂地图时，可能就会有bug，路径有可能会不是最短，也可能会有断点，也就是贴出来留个纪念，但是跑一般的图时，不会有事。

#include<iostream>
#include<vector>
#include<algorithm>
#include<cmath>
#define RED "\033[31m" /*red*/
#define YELLOW "\033[33m" /*Green*/
#define RESET "\033[0m"
using namespace std;

typedef struct node
{
    int x,y;//坐标
    int g;//g函数到起点的真实距离
    int h;//h函数到终点的估计距离
    int f;//f=h+g
    node* father;//父节点
    node(int x,int y)//struct的构造函数
    {
        this->x=x;
        this->y=y;
        this->g=0;
        this->f=0;
        this->father=NULL;
    }
    node(int x,int y,node* father)
    {
        this->x=x;
        this->y=y;
        this->g=0;
        this->f=0;
        this->father=father;
    }
}node;

int weightW=10;//直线代价
int weightWH=14;//对角线代价
int row=20;//行
int col=20;//列

class A_star
{
public: 
    node *start;
    node *end; 
    vector<node*> openlist;//open表，存遍历到的节点
    vector<node*> closelist;//close表，存访问过的节点
    A_star();
    ~A_star();

    void search(node* start,node* end);
    void check(int x,int y,node* father,int g);
    void nextstep(node* current);
    int finding(vector<node*>* nodelist,int x,int y);
    static bool compare(node* n1,node* n2);
    void print(node* current);
};
void printmap();
int map[100][100]= //地图（纯手打）
{
    {0,1,0,0,0,1,1,0,0,0,1,0,0,0,1,0,0,1,0,0,0},
    {0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,1,0,0,0},
    {0,1,0,0,0,1,1,0,0,0,0,0,1,0,0,0,1,0,0,0,0},
    {0,1,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,1,1,0,0,0,0,1,0,0,0,1,1,0,0,0,1,0,0,0},
    {0,0,1,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,1,0,0,0,0,0,1,0,0,0,0,1,1,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0},
    {1,1,1,1,0,0,0,1,1,1,1,0,0,0,0,0,1,0,1,0,0},
    {0,0,0,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0},
    {0,0,0,0,0,1,1,0,0,0,0,0,1,0,0,0,1,0,1,0,0},
    {0,1,0,0,0,1,1,0,0,0,1,0,0,0,1,0,0,1,0,0,0},
    {0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,0,0,1,0,0,0},
    {0,1,0,0,0,1,1,0,0,0,0,0,1,0,0,0,1,0,0,0,0},
    {0,1,0,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,1,1,0,0,0,0,1,0,0,0,1,1,0,0,0,1,0,0,0},
    {0,0,1,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0},
    {0,0,1,0,0,0,0,0,1,0,0,0,0,1,1,0,0,0,0,0,0},
    {0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0,0,0,0,0,0},
    {1,1,1,1,0,0,0,1,1,1,1,0,0,0,0,0,1,0,1,0,0},
    {0,0,0,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,1,0,0},
};
A_star::A_star()
{
}
A_star::~A_star()
{
}
void A_star::search(node* start,node* end)
{
    if(start->x<0||start->x>row||end->y<0||end->y>row||end->x<0||end->x>row||start->y<0||start->y>row)
        return;//如果数组越界直接返回
    node* current;
    this->start=start;
    this->end=end;
    openlist.push_back(start);//将起点放到open里
    while(openlist.size()>0)
    {
        current=openlist[0];
        if(current->x==end->x && current->y==end->y)//如果找到终点
        {
            cout<<"find the path"<<endl;
            print(current);
            openlist.clear();
            closelist.clear();
            break;
        }
        nextstep(current);//用于遍历函数
        closelist.push_back(current);
        openlist.erase(openlist.begin());//一旦访问完就丢弃到close中
        sort(openlist.begin(),openlist.end(),compare);//根据f值从小到大排序
    }
}
void A_star::nextstep(node* current)
{
    check(current->x-1,current->y,current,weightW);//下
    check(current->x+1,current->y,current,weightW);//上
    check(current->x,current->y+1,current,weightW);//右
    check(current->x,current->y-1,current,weightW);//左
    if(map[current->x+1][current->y]!=1||map[current->x][current->y+1]!=1)
        check(current->x+1,current->y+1,current,weightWH);//以及斜着的方向,这个if嘛。。打个比方如果他的左是障碍物 下也是障碍物，那么他的左下其实是不能通过的，这个if就是为了这个事
    if(map[current->x+1][current->y]!=1||map[current->x][current->y-1]!=1)
        check(current->x+1,current->y-1,current,weightWH);
    if(map[current->x-1][current->y]!=1||map[current->x][current->y+1]!=1)
        check(current->x-1,current->y+1,current,weightWH);
    if(map[current->x-1][current->y]!=1||map[current->x][current->y-1]!=1)
        check(current->x-1,current->y-1,current,weightWH);
}
void A_star::check(int x,int y,node* father,int g)//用来访问节点
{
    int j;
    if(x<0||x>row||y<0||y>row)//如果访问到的节点越界了，直接返回
        return ;
    if(map[x][y]==1)//如果是障碍物，也直接返回
        return ;
    if(j=finding(&closelist,x,y)!=0)//如果是close表中的，也直接返回
        return ;
    j=0;
    if(j=finding(&openlist,x,y)!=0)//如果在open表中的，则需比较f值，如果当前的f值小，就替换成当前的节点
    {
        node* p=openlist[j];
        if(father->f+g<p->f)//这个我用了估算，最精确的是把当前节点的f值算出来和在open表中的拿来比较，但是算f值比较麻烦，我这里就用father->f+g来代替当前节点的f
        {
            p->father=father;
            p->g=father->g+g;
            p->f=p->g+p->h;
        }
    }
    else//如果两个表都不在就计算一下f值，存到open表中
    {
        node* p=new node(x,y,father);
        p->h=abs(p->x-end->x)*weightW+abs(p->y-end->y)*weightW;
        p->g=p->father->g+g;
        p->f=p->g+p->h;
        openlist.push_back(p);
    }
}
int A_star::finding(vector<node*>* nodelist,int x,int y)
{
    int i,j=0;
    for(i=0;i<nodelist->size();i++)
    {
        if(nodelist->at(i)->x==x&&nodelist->at(i)->y==y)
        {
            j=i;
            break;
        }
    }
    return j;
}
bool A_star::compare(node* n1,node* n2)
{
    return n1->f<n2->f;
}
void A_star::print(node* current)//将路径制为7
{
    while(current!=NULL)
    {
        map[current->x][current->y]=7;
        current=current->father;
    }
}
void printmap()//输出地图
{
    int i,j=0;
    for(i=0;i<=row;i++)
    {
        if(i<10)
            cout<<i<<":  ";
        else
            cout<<i<<": ";
        for(j=0;j<col;j++)
        {
            if(map[i][j]==7)
                cout<<YELLOW<<map[i][j]<<" ";
            else if(map[i][j]==1)
                cout<<RED<<map[i][j]<<" ";
            else
                cout<<RESET<<map[i][j]<<" ";
        }
        cout<<endl;
    }
}
int main()
{
    printmap();
    cout<<"注意1为障碍物"<<"请输入两个点的坐标作为起点和终点"<<endl;
    int x1,x2,y1,y2;
    cin>>x1>>y1>>x2>>y2;
    A_star a;
    node *start=new node(x1,y1);
    node *end=new node(x2,y2);
    a.search(start,end);
    map[x1][y1]=2;
    map[x2][y2]=3;
    printmap();
    return 0;
}