图搜索之A*算法、深度优先搜索和广度优先搜索
博客转自:http://blog.csdn.net/wds555/article/details/24148245
A*算法概述:F=G+H
初始化根节点的G、H、F;
根节点加入open表;
while(open表不为空)
{
从open表中取出估价函数最小的节点作为当前节点P;//注意是取出,这里open表中元素数减一
if(P为目标节点)
返回P;
P加入close表中;
for(P的每个孩子节点child)
{
判断child是否在close表中;
if(在close中)
continue;
else
{
计算child的G代价;//即已产生的实际代价
判断child是否在open表中;
if(在open表中)
{
if(child的G代价<在open表中的代价)//即新路径更优
{
设置open表中的该child的父节点为P;
更新open表中的该child的估价函数G,H,F;
}
}else
{
设置child的父节点为P;
计算child的估价函数G,H,F;
child加入open表中;
}
}
}
return false;
}
打印路径的方式是沿着父节点一步步往前回溯;
- /*
- *本文实现了深度优先搜索、广度优先搜索和人工智能中常用的A*搜索。
- *本文中假设图由ROWS*COLS大小的矩阵,1代表不可通行,0代表可通行,矩阵中的2可看做图的起点和终点。
- *在迷宫求解中,深度优先是一种较常用的算法,类似贪心算法,只关注当前的信息。
- *本文利用广度优先搜索对建筑进行标号,图中值为1的节点可认为是建筑,相邻的1认为是同一栋建筑。
- *A*算法是一种启发式搜索,可看做广度优先搜索和迪杰斯特拉算法的发展。
- *估价函数F(x)=G(x)+H(x),G(x)为从起始节点到当前节点的实际代价,H(x)为从当前节点到目标节点的估计代价。
- *然后利用A*算法,计算并输出任意两建筑之间的最短路径
- */
- #include <stdio.h>
- #include <windows.h>
- #include <math.h>
- #define ROWS 22
- #define COLS 22
- #define UP 0
- #define RIGHT 1
- #define DOWN 2
- #define LEFT 3
- int maze[ROWS][COLS]=
- {
- { 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 2, 1, 1, 1,-1,-1,-1, 1, 1, 0, 0, 1},
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
- {-1, 0, 1, 0, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 1, 1, 0, 0, 0, 1},
- {-1, 0, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0,-1},
- {-1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0,-1},
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1},
- { 1, 0, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 1, 0, 0},
- {0,0,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
- {1,0,1,0,0,0,0,0,0,0,0,1,0,0,1,1,0,1,1,0,0,1},
- {0,0,0,0,1,1,0,1,0,1,0,1,0,0,1,0,0,0,0,0,0,1},
- {0,0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,0,1,0,1,0,0},
- {1,0,1,0,1,1,0,1,1,1,0,1,1,0,1,1,1,1,1,1,0,0},
- {2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2},
- {1,0,1,1,0,1,0,1,1,1,0,0,0,0,1,0,0,1,1,0,0,1},
- {0,0,1,1,0,1,0,1,0,1,0,0,1,0,1,1,0,1,1,0,0,1},
- {0,0,1,1,0,1,0,1,0,1,0,0,1,0,0,1,0,0,0,1,0,1},
- {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
- {1,0,0,1,0,1,0,1,0,1,0,1,0,0,1,1,0,1,1,0,0,1},
- {-1,0,1,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,1,0,0,-1},
- {-1,0,0,1,0,1,0,1,0,1,0,1,1,0,1,0,0,1,1,0,0,-1},
- {-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1},
- {1,1,1,0,0,1,1,0,0,1,1,1,1,2,0,1,1,0,1,1,0,1},
- };
- int maze_copy[ROWS][COLS]={0};
- typedef struct{
- int i;
- int j;
- int direction;
- int number;
- int G;
- int H;
- //int F;
- int pre;
- }node;
- node path[ROWS*COLS]={0};
- void print_maze(int temp[ROWS][COLS])//打印迷宫图
- {
- for(int i=0;i<ROWS;i++)
- {
- for(int j=0;j<COLS;j++)
- {
- printf("%3d",temp[i][j]);
- }
- printf("\n");
- }
- printf("\n");
- }
- void print_path(int maze[ROWS][COLS],node path[],int n)//打印路径
- {
- for(int i=0;i<n;i++)
- {
- printf("\t(%d,%d)\t%d\n",path[i].i,path[i].j,maze[path[i].i][path[i].j]);
- }
- }
- int DepthFirstSearch(int in_i,int in_j)//深度优先,迷宫求解
- {
- memcpy(maze_copy,maze,sizeof(maze));
- path[0].i=in_i;
- path[0].j=in_j;
- path[0].direction=UP;
- int p=0;
- while(p>=0 && p<ROWS*COLS)
- {
- if(maze_copy[path[p].i][path[p].j]==2 && p>0)
- {
- printf("Found the path: !!!!!!!!!!!!!!!!!\n");
- break;
- }
- maze_copy[path[p].i][path[p].j]=3;
- if(path[p].direction==UP)
- {
- if(path[p].i==0)
- {
- path[p].direction=RIGHT;//UP-->RIGHT
- continue;
- }
- if(maze_copy[path[p].i-1][path[p].j]==0 || maze_copy[path[p].i-1][path[p].j]==2)
- {
- p++;
- path[p].i=path[p-1].i-1;
- path[p].j=path[p-1].j;
- path[p].direction=UP;
- }
- else
- {
- path[p].direction=RIGHT;//UP-->RIGHT
- }
- }
- else if(path[p].direction==RIGHT)
- {
- if(path[p].j==COLS-1)
- {
- path[p].direction=DOWN;//RIGHT-->DOWN
- continue;
- }
- if(maze_copy[path[p].i][path[p].j+1]==0 || maze_copy[path[p].i][path[p].j+1]==2)
- {
- p++;
- path[p].i=path[p-1].i;
- path[p].j=path[p-1].j+1;
- path[p].direction=UP;
- }
- else
- {
- path[p].direction=DOWN;//RIGHT-->DOWN
- }
- }
- else if(path[p].direction==DOWN)
- {
- if(path[p].i==ROWS-1)
- {
- path[p].direction=LEFT;//DOWN-->LEFT
- continue;
- }
- if(maze_copy[path[p].i+1][path[p].j]==0 || maze_copy[path[p].i+1][path[p].j]==2)
- {
- p++;
- path[p].i=path[p-1].i+1;
- path[p].j=path[p-1].j;
- path[p].direction=UP;
- }
- else
- {
- path[p].direction=LEFT;//DOWN-->LEFT
- }
- }else //path[p].direction==LEFT
- {
- if(path[p].j==0)
- {
- p--; //Back
- continue;
- }
- if(maze_copy[path[p].i][path[p].j-1]==0 || maze_copy[path[p].i][path[p].j-1]==2)
- {
- p++;
- path[p].i=path[p-1].i;
- path[p].j=path[p-1].j-1;
- path[p].direction=UP;
- }
- else
- {
- p--; //Back
- }
- }
- }
- return p+1;
- }
- node open[ROWS*COLS];
- node close[ROWS*COLS];
- void clear(node temp[ROWS*COLS])
- {
- for(int i=0;i<ROWS*COLS;i++)
- {
- temp[i].i=0;
- temp[i].j=0;
- temp[i].direction=0;
- temp[i].G=0;
- temp[i].H=0;
- temp[i].number=0;
- temp[i].pre=0;
- }
- }
- //检查是否在表中,是的话返回编号,否的话返回-1
- int check1(node *close1,int length,int i,int j)
- {
- for(int k=length-1;k>=0;k--)
- {
- if(close1[k].i==i && close1[k].j==j)
- return close1[k].number;
- }
- return -1;
- }
- //广度优先搜索算法,对建筑编号
- int BreadthFirstSearch()
- {
- open[0].i=12;
- open[0].j=0;
- open[0].number=0;
- int length_open=0;
- int length_open2=1;
- int length_close=0;
- int check_in=-1;
- int number=10;
- while(length_open<ROWS*COLS)
- {
- node temp=open[length_open];//从open表中取出一节点最为活动节点
- length_open++;
- //处理活动节点的上方节点
- int temp_i=0;
- int temp_j=0;
- for(int k=0;k<4;k++)
- {
- temp_i=temp.i;
- temp_j=temp.j;
- if(k==0)//
- {
- if(temp.i<=0)continue;
- temp_i=temp.i-1;
- }
- else if(k==1)
- {
- if(temp.i>=ROWS-1)continue;
- temp_i=temp.i+1;
- }
- else if(k==2)
- {
- if(temp.j<=0)continue;
- temp_j=temp.j-1;
- }else
- {
- if(temp.j>=COLS-1)continue;
- temp_j=temp.j+1;
- }
- check_in=check1(close,length_close,temp_i,temp_j);//是否在close表中
- if(check_in>=0)//在close表中
- {
- if(maze[temp.i][temp.j]==maze[temp_i][temp_j])
- temp.number=check_in;//原图中值相同的话,则标号也相同
- }
- else//不在close表中
- {
- if(check1(open,length_open2,temp_i,temp_j)<0)//也不再open表中,加入到open表
- {
- open[length_open2].i=temp_i;
- open[length_open2++].j=temp_j;
- }
- }
- }
- if(temp.number==0 && maze[temp.i][temp.j]==1)
- temp.number=++number;
- close[length_close++]=temp;
- }
- for(int i=0;i<length_close;i++)
- {
- if(close[i].number>0)
- maze_copy[close[i].i][close[i].j]=close[i].number;
- }
- printf("\tThe numbers of building is:%d\n",number);
- print_maze(maze_copy);//打印建筑标号图
- printf("\n");
- return number;
- }
- //检查节点是否在表中,不在的话返回-1,否则返回节点位置。
- int check(node* temp,int length,int i,int j)
- {
- for(int k=length;k>=0;k--)
- if(temp[k].i==i && temp[k].j==j)
- return k;
- return -1;
- }
- //选择估价函数最小的节点,并从OPEN表中删除
- node choose_min(node *open,int length)
- {
- int f=open[0].G+open[0].H;
- int min=0;
- for(int i=0;i<length;i++)
- {
- if(f>open[i].G+open[i].H)
- {
- f=open[i].G+open[i].H;
- min=i;
- }
- }
- node temp=open[min];
- for(int i=min;i<length-1;i++)
- open[i]=open[i+1];
- return temp;
- }
- //打印最短路径
- int printPath_A(int p)
- {
- int length=0;
- while(p>=0)
- {
- printf("\t(%d,%d),%d\n",close[p].i,close[p].j,maze_copy[close[p].i][close[p].j]);
- p=close[p].pre;
- length++;
- }
- return length;
- }
- //A*算法,求任意位置间的最短路径
- node aStar(int i,int j,int goal_i,int goal_j,int goal)
- {
- open[0].i=i;
- open[0].j=j;
- open[0].G=0;
- open[0].H=abs(goal_i-i)+abs(goal_j-j);
- open[0].pre=-1;
- int length_open=1;
- int length_close=0;
- int check_in=-1;
- node temp;
- while(length_open>0 && length_open<ROWS*COLS-1)//只要OPEN表不为空
- {
- //从OPEN表中取出最小估价函数节点最为当前节点
- temp=choose_min(open,length_open);
- //printf("\t(%d,%d),\t%d\n",temp.i,temp.j,maze[temp.i][temp.j]);
- //if((temp.i==goal_i && temp.j==goal_j) || maze_copy[temp.i][temp.j]==goal)
- if(maze_copy[temp.i][temp.j]==goal)
- return temp;
- close[length_close++]=temp;//插入CLOSE表中
- length_open--;//调整open表长度
- //依次处理上下左右四个节点,并处理边界节点。
- int temp_i=0;
- int temp_j=0;
- for(int k=0;k<4;k++)
- {
- temp_i=temp.i;
- temp_j=temp.j;
- if(k==0)
- {
- if(temp.i<=0)continue;
- temp_i=temp.i-1;
- }
- else if(k==1)
- {
- if(temp.i>=ROWS-1)continue;
- temp_i=temp.i+1;
- }
- else if(k==2)
- {
- if(temp.j<=0)continue;
- temp_j=temp.j-1;
- }else
- {
- if(temp.j>=COLS-1)continue;
- temp_j=temp.j+1;
- }
- if(maze[temp_i][temp_j]==0 || maze[temp_i][temp_j]==2 ||maze_copy[temp_i][temp_j]==goal)
- {//处理当前节点上面的节点
- //检查该接点是否在CLOSE表中
- check_in=check(close,length_close,temp_i,temp_j);
- if(check_in<0)//不在CLOSE表中
- {
- int g=temp.G+1;
- //检查相邻界点是否在OPEN表中
- check_in=check(open,length_open,temp_i,temp_j);
- if(check_in==-1)//加入OPEN表
- {
- open[length_open].i=temp_i;
- open[length_open].j=temp_j;
- open[length_open].pre=length_close-1;
- open[length_open].G=g;
- open[length_open++].H=abs(temp_i-goal_i)+abs(temp_j-goal_j);
- }else if(g<open[check_in].G)//新路径代价更小,则更新路径信息。
- {
- open[check_in].pre=length_close-1;
- open[check_in].G=g;
- open[check_in].H=abs(temp_i-goal_i)+abs(temp_j-goal_j);
- }
- }
- }
- }
- }
- temp.pre=-1;
- return temp;
- }
- int LocateBuilding(int number,int *loc_i,int *loc_j)//定位建筑的坐标
- {
- for(int i=0;i<ROWS;i++)
- {
- for(int j=0;j<COLS;j++)
- {
- if(maze_copy[i][j]==number)
- {
- *loc_i=i;
- *loc_j=j;
- return 1;
- }
- }
- }
- return 0;
- }
- void ShortestPathBetweenTwoBuildings(int NO1,int NO2)//求NO1到NO2的最小路径
- {
- //int p=0;
- int length=0;
- int loc1_i,loc1_j,loc2_i,loc2_j;
- LocateBuilding(NO1,&loc1_i,&loc1_j);
- LocateBuilding(NO2,&loc2_i,&loc2_j);
- node p=aStar(loc1_i,loc1_j,loc2_i,loc2_j,maze_copy[loc2_i][loc2_j]);//A*算法求最小路径
- if(p.pre!=-1)
- {
- clear(open);
- clear(close);
- p=aStar(p.i,p.j,loc1_i,loc1_j,maze_copy[loc1_i][loc1_j]);//反向回溯寻找最优路径,防止路径重叠。
- printf("\nA* Search:\n");
- printf("The path of building NO%d to NO%d is:\n",NO1,NO2);
- printf("\t(%d,%d),%d\n",p.i,p.j,maze_copy[p.i][p.j]);//先输出起始点
- length=printPath_A(p.pre);//反向打印得到的最小路径
- printf("\tThe path number is:%d\n",length+1);
- }
- clear(open);
- clear(close);
- }
- void ShortestPathForEachPairBuilding(int number)//打印所有的建筑之间的最小路径
- {
- for(int i=11;i<number;i++)
- {
- for(int j=i+1;j<=number;j++)
- {
- ShortestPathBetweenTwoBuildings(j,i);//求i到j的最小路径
- }
- }
- }
- void main(int argc,char *argv[])
- {
- printf("Maze Map:\n");
- print_maze(maze);//打印迷宫图(交通图)
- //迷宫求解
- printf("*****************************************************************************\n\n");
- int length=DepthFirstSearch(12,0);//深度优先算法,迷宫求解
- if(length>0 && length<ROWS*COLS)
- {
- printf("Depth First Search:\n");
- print_path(maze,path,length);
- printf("\tThe path number is:%d\n\n",length);
- }else{
- printf("Can't find\n");
- }
- memcpy(maze_copy,maze,ROWS*COLS*sizeof(int));
- //为建筑编号
- printf("*****************************************************************************\n\n");
- printf("Seting numbers to every buildings:\n");
- int number=BreadthFirstSearch();//利用广度优先搜索算法,对建筑编号。
- clear(open);
- clear(close);
- //计算所有建筑之间的最优路径
- printf("*****************************************************************************\n\n");
- //ShortestPathForEachPairBuilding(number);//利用A*算法,打印所有建筑之间的最小路径
- //计算任意建筑之间的最优路径
- printf("*****************************************************************************\n\n");
- while(1)
- {
- int NO1,NO2;
- printf("\nPlease input the Number of the Start and End Building,The Number shold between 11 and %d\n",number);
- printf("If you input 0,it will close!!!\n\n");
- printf("The Start Building NO is:");
- scanf_s("%d",&NO1);
- while(NO1<11 || NO1>number)
- {
- if(NO1==0)
- return;
- printf("The Start Building NO is:");
- scanf_s("%d",&NO1);
- }
- printf("The End Building NO is:");
- scanf_s("%d",&NO2);
- while(NO2<11 || NO2>number)
- {
- if(NO1==0)
- return;
- printf("The End Building NO is:");
- scanf_s("%d",&NO2);
- }
- ShortestPathBetweenTwoBuildings(NO1,NO2);//求NO1到NO2之间的最优路径
- printf("*****************************************************************************\n\n");
- }
- }