图搜索之A*算法、深度优先搜索和广度优先搜索

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 3int 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-->RIGHTcontinue;}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-->DOWNcontinue;}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-->LEFTcontinue;}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--;//Backcontinue;}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;}}//检查是否在表中，是的话返回编号，否的话返回-1int 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");}}