图搜索之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;

}

打印路径的方式是沿着父节点一步步往前回溯；

[cpp] view
plaincopy





/*

*本文实现了深度优先搜索、广度优先搜索和人工智能中常用的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");

}

}