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