AStar算法求解迷宫最短路径问题

 我这个仅针对7行7列的迷宫写的，写的比较急，要计算n行m列的迷宫稍微改下就行了

代码如下，若有不妥请高手指点：

#include <iostream>#include <set>#include <string>#include <cmath>#include <cstdlib>#define N 100using namespace std;struct node{double h,g,f;int x,y;bool operator<(node other)const{return f<other.f;}};int map[N][N];//迷宫，1表示可以走，0表示不可以走int row,col;//矩阵的行和列multiset<node> open;//可以拓展节点multiset<node>::iterator it;node close[N][N];//已经走过的节点int step[8][2] = {1,0,0,1,-1,0,0,-1,1,1,1,-1,-1,1,-1,-1};//走的方式int prev[N*N];//存放前一个节点void Init(){memset(close,0,sizeof(close));}double getg(node a,node b)//获取当前点到终点的路径值{return sqrt(1.0*(a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));}bool Is_Match(node a,node b){if(a.x == b.x && a.y == b.y){return true;}return false;}node begin,end;int main(){while(cin>>row>>col){open.clear();Init();for(int i=0;i<row;i++)//输入迷宫{for(int j=0;j<col;j++){cin>>map[i][j];}}cin>>begin.x>>begin.y>>end.x>>end.y;//起始点和终点begin.g = getg(begin,end);//初始化起始点的估值函数begin.f = begin.g;begin.h = 0;open.insert(begin);//open表插入起点bool IsSuccess = false;while(!open.empty())//但还有节点可以走{node son = *(open.begin());//取出路径消耗+估值函数最小的节点open.erase(open.begin());//从open表中删除上面的节点for(int i=0;i<8;i++)//探寻8个方向{int u = son.x+step[i][0];int v = son.y+step[i][1];if(u>=0&&v>=0&&u<row&&v<col&&map[u][v])//如果这个节点在矩阵中，且值为1，可以走{//更新这个节点的信息，包括路径消耗和估值函数node tmp;tmp.x = u;tmp.y = v;tmp.g = getg(tmp,end);tmp.h = son.h+1;tmp.f = tmp.h + tmp.g;bool IsInOpen = false;//表示是否在open表中if(u == end.x&&v == end.y)//如果是终点，就退出循环{prev[u*7+v] = son.x*7+son.y;//记录前一个节点IsSuccess = true;//表示成功找到了路径goto BreakPoint;}for(it = open.begin();it!=open.end();it++){//如果在open表中，比较tmp这个点与open表中的f的大小，如果前者f小就把tmp插入到open表中if(Is_Match(*it,tmp)){IsInOpen = true;//表示在open表中if((*it).f>tmp.f){node New = tmp;open.erase(it);open.insert(tmp);prev[tmp.x*7+tmp.y] = son.x*7+son.y;//记录前一个节点break;}}}bool IsInClose = false;//标记是否在close表中if(close[tmp.x][tmp.y].f)//如果在close中，表示已经走过，再次比较f的大小，如果前者f小，把这个点插入到open表中，并从close表中删除这个点。{IsInClose = true;//在close表中if(tmp.f < close[tmp.x][tmp.y].f){open.insert(tmp);close[tmp.x][tmp.y].f = 0;close[tmp.x][tmp.y].g = 0;close[tmp.x][tmp.y].h = 0;prev[tmp.x*7+tmp.y] = son.x*7+son.y;//记录前一个节点}}if(!IsInOpen&&!IsInClose)//如果既不在close表中，也不在open表中，表示没有走过，把这个节点插入open表中。{open.insert(tmp);prev[tmp.x*7+tmp.y] = son.x*7+son.y;//记录前一个节点}}}//把从open表中取出点得放到close表中，表示已经走过close[son.x][son.y].x = son.x;close[son.x][son.y].y = son.y;close[son.x][son.y].g = son.g;close[son.x][son.y].f = son.f;close[son.x][son.y].h = son.h;}BreakPoint:;if(IsSuccess)//如果成功找到了路径，输出路径，我没有去反序输出结果，所以点是倒着的。{cout<<"The Path Is:"<<endl;int x = end.x*7+end.y;int y = begin.x*7+begin.y;while(x!=y){cout<<"("<<x/7<<","<<x%7<<") -- > ";x = prev[x];}cout<<"("<<begin.x<<","<<begin.y<<")"<<endl;cout<<endl;cout<<"Reach Success!"<<endl;}else //不能到达终点{cout<<"Reach Failed!"<<endl;}}}