这里原理就是根据一个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;
}