図探索のA*アルゴリズム、深さ優先探索と広さ優先探索
12822 ワード
A*アルゴリズムの概要:F=G+H
ルートノードのG,H,Fを初期化する.
ルートノードはopenテーブルに追加されます.
while(openテーブルは空ではありません)
{
Openテーブルから評価関数の最小ノードを現在のノードPとして取り出す//ここでopenテーブルの要素数を1つ減らします.
if(Pがターゲットノード)
Pを返します.
Pはcloseテーブルに参加する.
for(Pの各子供ノードchild)
{
childがcloseテーブルにあるかどうかを判断する.
if(close)
continue;
else
{
childのG代価を計算する;//すなわち、発生した実際の代価
childがopenテーブルにあるかどうかを判断する.
if(openテーブル)
{
if(childのG代価 {
Openテーブルのchildの親ノードをPに設定します.
Openテーブルのこのchildの評価関数G,H,Fを更新する.
}
}else
{
childの親ノードをPに設定します.
childの評価関数G,H,Fを計算する.
childはopenテーブルに追加されます.
}
}
}
return false;
}
印刷パスは、親ノードに沿って一歩一歩遡る方法です.
ルートノードのG,H,Fを初期化する.
ルートノードはopenテーブルに追加されます.
while(openテーブルは空ではありません)
{
Openテーブルから評価関数の最小ノードを現在のノードPとして取り出す//ここでopenテーブルの要素数を1つ減らします.
if(Pがターゲットノード)
Pを返します.
Pはcloseテーブルに参加する.
for(Pの各子供ノードchild)
{
childがcloseテーブルにあるかどうかを判断する.
if(close)
continue;
else
{
childのG代価を計算する;//すなわち、発生した実際の代価
childがopenテーブルにあるかどうかを判断する.
if(openテーブル)
{
if(childのG代価
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("
");
}
printf("
");
}
void print_path(int maze[ROWS][COLS],node path[],int n)//
{
for(int i=0;i<n;i++)
{
printf("\t(%d,%d)\t%d
",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: !!!!!!!!!!!!!!!!!
");
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
",number);
print_maze(maze_copy);//
printf("
");
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
",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
",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("
A* Search:
");
printf("The path of building NO%d to NO%d is:
",NO1,NO2);
printf("\t(%d,%d),%d
",p.i,p.j,maze_copy[p.i][p.j]);//
length=printPath_A(p.pre);//
printf("\tThe path number is:%d
",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:
");
print_maze(maze);// （ ）
//
printf("*****************************************************************************
");
int length=DepthFirstSearch(12,0);// ，
if(length>0 && length<ROWS*COLS)
{
printf("Depth First Search:
");
print_path(maze,path,length);
printf("\tThe path number is:%d
",length);
}else{
printf("Can't find
");
}
memcpy(maze_copy,maze,ROWS*COLS*sizeof(int));
//
printf("*****************************************************************************
");
printf("Seting numbers to every buildings:
");
int number=BreadthFirstSearch();// ， 。
clear(open);
clear(close);
//
printf("*****************************************************************************
");
//ShortestPathForEachPairBuilding(number);// A* ,
//
printf("*****************************************************************************
");
while(1)
{
int NO1,NO2;
printf("
Please input the Number of the Start and End Building,The Number shold between 11 and %d
",number);
printf("If you input 0,it will close!!!
");
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("*****************************************************************************
");
}
}