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