zoukankan      html  css  js  c++  java

  • 图搜索之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在CODE上查看代码片派生到我的代码片
    1. /* 
    2.     *本文实现了深度优先搜索、广度优先搜索和人工智能中常用的A*搜索。 
    3.     *本文中假设图由ROWS*COLS大小的矩阵，1代表不可通行，0代表可通行，矩阵中的2可看做图的起点和终点。 
    4.     *在迷宫求解中，深度优先是一种较常用的算法，类似贪心算法，只关注当前的信息。 
    5.     *本文利用广度优先搜索对建筑进行标号，图中值为1的节点可认为是建筑，相邻的1认为是同一栋建筑。 
    6.     *A*算法是一种启发式搜索，可看做广度优先搜索和迪杰斯特拉算法的发展。 
    7.     *估价函数F(x)=G(x)+H(x),G(x)为从起始节点到当前节点的实际代价，H(x)为从当前节点到目标节点的估计代价。 
    8.     *然后利用A*算法，计算并输出任意两建筑之间的最短路径 
    9.     */  
    10. #include <stdio.h>  
    11. #include <windows.h>  
    12. #include <math.h>  
    13.   
    14. #define ROWS 22  
    15. #define COLS 22  
    16. #define UP 0  
    17. #define RIGHT 1  
    18. #define DOWN 2  
    19. #define LEFT 3  
    20.   
    21. int maze[ROWS][COLS]=  
    22.     {  
    23.         { 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 2, 1, 1, 1,-1,-1,-1, 1, 1, 0, 0, 1},  
    24.         { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},  
    25.         {-1, 0, 1, 0, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 1, 1, 0, 0, 0, 1},  
    26.         {-1, 0, 1, 0, 1, 1, 0, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0,-1},  
    27.         {-1, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0,-1},  
    28.         { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1},  
    29.         { 1, 0, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 1, 0, 0},  
    30.         {0,0,1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},  
    31.         {1,0,1,0,0,0,0,0,0,0,0,1,0,0,1,1,0,1,1,0,0,1},  
    32.         {0,0,0,0,1,1,0,1,0,1,0,1,0,0,1,0,0,0,0,0,0,1},  
    33.         {0,0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,0,1,0,1,0,0},  
    34.         {1,0,1,0,1,1,0,1,1,1,0,1,1,0,1,1,1,1,1,1,0,0},  
    35.         {2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2},  
    36.         {1,0,1,1,0,1,0,1,1,1,0,0,0,0,1,0,0,1,1,0,0,1},  
    37.         {0,0,1,1,0,1,0,1,0,1,0,0,1,0,1,1,0,1,1,0,0,1},  
    38.         {0,0,1,1,0,1,0,1,0,1,0,0,1,0,0,1,0,0,0,1,0,1},  
    39.         {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},  
    40.         {1,0,0,1,0,1,0,1,0,1,0,1,0,0,1,1,0,1,1,0,0,1},  
    41.         {-1,0,1,1,0,1,0,1,0,1,0,1,0,0,1,0,0,1,1,0,0,-1},  
    42.         {-1,0,0,1,0,1,0,1,0,1,0,1,1,0,1,0,0,1,1,0,0,-1},  
    43.         {-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1},  
    44.         {1,1,1,0,0,1,1,0,0,1,1,1,1,2,0,1,1,0,1,1,0,1},  
    45.     };  
    46. int maze_copy[ROWS][COLS]={0};  
    47.   
    48. typedef struct{  
    49.     int i;  
    50.     int j;  
    51.     int direction;  
    52.     int number;  
    53.     int G;  
    54.     int H;  
    55.     //int F;  
    56.     int pre;  
    57. }node;  
    58. node path[ROWS*COLS]={0};  
    59.   
    60. void print_maze(int temp[ROWS][COLS])//打印迷宫图  
    61. {  
    62.     for(int i=0;i<ROWS;i++)  
    63.     {  
    64.         for(int j=0;j<COLS;j++)  
    65.         {  
    66.             printf("%3d",temp[i][j]);  
    67.         }  
    68.         printf(" ");  
    69.     }  
    70.     printf(" ");  
    71. }  
    72. void print_path(int maze[ROWS][COLS],node path[],int n)//打印路径  
    73. {  
    74.     for(int i=0;i<n;i++)  
    75.     {  
    76.         printf(" (%d,%d) %d ",path[i].i,path[i].j,maze[path[i].i][path[i].j]);  
    77.     }  
    78. }  
    79.   
    80. int DepthFirstSearch(int in_i,int in_j)//深度优先,迷宫求解  
    81. {  
    82.     memcpy(maze_copy,maze,sizeof(maze));  
    83.     path[0].i=in_i;  
    84.     path[0].j=in_j;  
    85.     path[0].direction=UP;  
    86.     int p=0;  
    87.     while(p>=0 && p<ROWS*COLS)  
    88.     {  
    89.         if(maze_copy[path[p].i][path[p].j]==2 && p>0)  
    90.         {  
    91.             printf("Found the path: !!!!!!!!!!!!!!!!! ");  
    92.             break;  
    93.         }  
    94.   
    95.         maze_copy[path[p].i][path[p].j]=3;  
    96.   
    97.         if(path[p].direction==UP)  
    98.         {  
    99.             if(path[p].i==0)  
    100.             {  
    101.                 path[p].direction=RIGHT;//UP-->RIGHT  
    102.                 continue;  
    103.             }  
    104.             if(maze_copy[path[p].i-1][path[p].j]==0 || maze_copy[path[p].i-1][path[p].j]==2)  
    105.             {  
    106.                 p++;  
    107.                 path[p].i=path[p-1].i-1;  
    108.                 path[p].j=path[p-1].j;  
    109.                 path[p].direction=UP;  
    110.             }  
    111.             else  
    112.             {  
    113.                 path[p].direction=RIGHT;//UP-->RIGHT  
    114.             }  
    115.         }  
    116.         else if(path[p].direction==RIGHT)  
    117.         {  
    118.             if(path[p].j==COLS-1)  
    119.             {  
    120.                 path[p].direction=DOWN;//RIGHT-->DOWN  
    121.                 continue;  
    122.             }  
    123.             if(maze_copy[path[p].i][path[p].j+1]==0 || maze_copy[path[p].i][path[p].j+1]==2)  
    124.             {  
    125.                 p++;  
    126.                 path[p].i=path[p-1].i;  
    127.                 path[p].j=path[p-1].j+1;  
    128.                 path[p].direction=UP;  
    129.             }  
    130.             else  
    131.             {  
    132.                 path[p].direction=DOWN;//RIGHT-->DOWN  
    133.             }  
    134.         }  
    135.         else if(path[p].direction==DOWN)  
    136.         {  
    137.             if(path[p].i==ROWS-1)  
    138.             {  
    139.                 path[p].direction=LEFT;//DOWN-->LEFT  
    140.                 continue;  
    141.             }  
    142.             if(maze_copy[path[p].i+1][path[p].j]==0 || maze_copy[path[p].i+1][path[p].j]==2)  
    143.             {  
    144.                 p++;  
    145.                 path[p].i=path[p-1].i+1;  
    146.                 path[p].j=path[p-1].j;  
    147.                 path[p].direction=UP;  
    148.             }  
    149.             else  
    150.             {  
    151.                 path[p].direction=LEFT;//DOWN-->LEFT  
    152.             }  
    153.         }else   //path[p].direction==LEFT  
    154.         {  
    155.             if(path[p].j==0)  
    156.             {  
    157.                 p--;    //Back  
    158.                 continue;  
    159.             }  
    160.             if(maze_copy[path[p].i][path[p].j-1]==0 || maze_copy[path[p].i][path[p].j-1]==2)  
    161.             {  
    162.                 p++;  
    163.                 path[p].i=path[p-1].i;  
    164.                 path[p].j=path[p-1].j-1;  
    165.                 path[p].direction=UP;  
    166.             }  
    167.             else  
    168.             {  
    169.                 p--;    //Back  
    170.             }  
    171.         }  
    172.     }  
    173.     return p+1;  
    174. }  
    175.   
    176. node open[ROWS*COLS];  
    177. node close[ROWS*COLS];  
    178.   
    179. void clear(node temp[ROWS*COLS])  
    180. {  
    181.     for(int i=0;i<ROWS*COLS;i++)  
    182.     {  
    183.         temp[i].i=0;  
    184.         temp[i].j=0;  
    185.         temp[i].direction=0;  
    186.         temp[i].G=0;  
    187.         temp[i].H=0;  
    188.         temp[i].number=0;  
    189.         temp[i].pre=0;  
    190.     }  
    191. }  
    192.   
    193. //检查是否在表中，是的话返回编号，否的话返回-1  
    194. int check1(node *close1,int length,int i,int j)  
    195. {  
    196.     for(int k=length-1;k>=0;k--)  
    197.     {  
    198.         if(close1[k].i==i && close1[k].j==j)  
    199.             return close1[k].number;  
    200.     }  
    201.     return -1;  
    202. }  
    203. //广度优先搜索算法，对建筑编号  
    204. int BreadthFirstSearch()  
    205. {  
    206.     open[0].i=12;  
    207.     open[0].j=0;  
    208.     open[0].number=0;  
    209.     int length_open=0;  
    210.     int length_open2=1;  
    211.     int length_close=0;  
    212.     int check_in=-1;  
    213.     int number=10;  
    214.     while(length_open<ROWS*COLS)  
    215.     {  
    216.         node temp=open[length_open];//从open表中取出一节点最为活动节点  
    217.         length_open++;  
    218.         //处理活动节点的上方节点  
    219.         int temp_i=0;  
    220.         int temp_j=0;  
    221.         for(int k=0;k<4;k++)  
    222.         {  
    223.             temp_i=temp.i;  
    224.             temp_j=temp.j;  
    225.             if(k==0)//  
    226.             {  
    227.                 if(temp.i<=0)continue;  
    228.                 temp_i=temp.i-1;  
    229.             }  
    230.             else if(k==1)  
    231.             {  
    232.                 if(temp.i>=ROWS-1)continue;  
    233.                 temp_i=temp.i+1;  
    234.             }  
    235.             else if(k==2)  
    236.             {  
    237.                 if(temp.j<=0)continue;  
    238.                 temp_j=temp.j-1;  
    239.             }else  
    240.             {  
    241.                 if(temp.j>=COLS-1)continue;  
    242.                 temp_j=temp.j+1;  
    243.             }  
    244.   
    245.             check_in=check1(close,length_close,temp_i,temp_j);//是否在close表中  
    246.             if(check_in>=0)//在close表中  
    247.             {  
    248.                 if(maze[temp.i][temp.j]==maze[temp_i][temp_j])  
    249.                     temp.number=check_in;//原图中值相同的话，则标号也相同  
    250.             }  
    251.             else//不在close表中  
    252.             {  
    253.                 if(check1(open,length_open2,temp_i,temp_j)<0)//也不再open表中，加入到open表  
    254.                 {  
    255.                     open[length_open2].i=temp_i;  
    256.                     open[length_open2++].j=temp_j;  
    257.                 }  
    258.             }  
    259.         }  
    260.         if(temp.number==0 && maze[temp.i][temp.j]==1)  
    261.             temp.number=++number;  
    262.         close[length_close++]=temp;  
    263.     }  
    264.   
    265.     for(int i=0;i<length_close;i++)  
    266.     {  
    267.         if(close[i].number>0)  
    268.             maze_copy[close[i].i][close[i].j]=close[i].number;  
    269.     }  
    270.     printf(" The numbers of building is:%d ",number);  
    271.     print_maze(maze_copy);//打印建筑标号图  
    272.     printf(" ");  
    273.   
    274.     return number;  
    275. }  
    276.   
    277. //检查节点是否在表中，不在的话返回-1，否则返回节点位置。  
    278. int check(node* temp,int length,int i,int j)  
    279. {  
    280.     for(int k=length;k>=0;k--)  
    281.         if(temp[k].i==i && temp[k].j==j)  
    282.             return k;  
    283.     return -1;  
    284. }  
    285. //选择估价函数最小的节点，并从OPEN表中删除  
    286. node choose_min(node *open,int length)  
    287. {  
    288.     int f=open[0].G+open[0].H;  
    289.     int min=0;  
    290.     for(int i=0;i<length;i++)  
    291.     {  
    292.         if(f>open[i].G+open[i].H)  
    293.         {  
    294.             f=open[i].G+open[i].H;  
    295.             min=i;  
    296.         }  
    297.     }  
    298.     node temp=open[min];  
    299.     for(int i=min;i<length-1;i++)  
    300.         open[i]=open[i+1];  
    301.     return temp;  
    302. }  
    303. //打印最短路径  
    304. int printPath_A(int p)  
    305. {  
    306.     int length=0;  
    307.     while(p>=0)  
    308.     {  
    309.         printf(" (%d,%d),%d ",close[p].i,close[p].j,maze_copy[close[p].i][close[p].j]);  
    310.         p=close[p].pre;  
    311.         length++;  
    312.     }  
    313.     return length;  
    314. }  
    315. //A*算法，求任意位置间的最短路径  
    316. node aStar(int i,int j,int goal_i,int goal_j,int goal)  
    317. {  
    318.     open[0].i=i;  
    319.     open[0].j=j;  
    320.     open[0].G=0;  
    321.     open[0].H=abs(goal_i-i)+abs(goal_j-j);  
    322.     open[0].pre=-1;  
    323.     int length_open=1;  
    324.     int length_close=0;  
    325.     int check_in=-1;  
    326.     node temp;  
    327.     while(length_open>0 && length_open<ROWS*COLS-1)//只要OPEN表不为空  
    328.     {  
    329.         //从OPEN表中取出最小估价函数节点最为当前节点  
    330.         temp=choose_min(open,length_open);  
    331.         //printf(" (%d,%d), %d ",temp.i,temp.j,maze[temp.i][temp.j]);  
    332.         //if((temp.i==goal_i && temp.j==goal_j) || maze_copy[temp.i][temp.j]==goal)  
    333.         if(maze_copy[temp.i][temp.j]==goal)  
    334.             return temp;  
    335.         close[length_close++]=temp;//插入CLOSE表中  
    336.         length_open--;//调整open表长度  
    337.         //依次处理上下左右四个节点，并处理边界节点。  
    338.         int temp_i=0;  
    339.         int temp_j=0;  
    340.         for(int k=0;k<4;k++)  
    341.         {  
    342.             temp_i=temp.i;  
    343.             temp_j=temp.j;  
    344.             if(k==0)  
    345.             {  
    346.                 if(temp.i<=0)continue;  
    347.                 temp_i=temp.i-1;  
    348.             }  
    349.             else if(k==1)  
    350.             {  
    351.                 if(temp.i>=ROWS-1)continue;  
    352.                 temp_i=temp.i+1;  
    353.             }  
    354.             else if(k==2)  
    355.             {  
    356.                 if(temp.j<=0)continue;  
    357.                 temp_j=temp.j-1;  
    358.             }else  
    359.             {  
    360.                 if(temp.j>=COLS-1)continue;  
    361.                 temp_j=temp.j+1;  
    362.             }  
    363.   
    364.             if(maze[temp_i][temp_j]==0 || maze[temp_i][temp_j]==2 ||maze_copy[temp_i][temp_j]==goal)  
    365.             {//处理当前节点上面的节点  
    366.                 //检查该接点是否在CLOSE表中  
    367.                 check_in=check(close,length_close,temp_i,temp_j);  
    368.                 if(check_in<0)//不在CLOSE表中  
    369.                 {  
    370.                     int g=temp.G+1;  
    371.                     //检查相邻界点是否在OPEN表中  
    372.                     check_in=check(open,length_open,temp_i,temp_j);  
    373.                     if(check_in==-1)//加入OPEN表  
    374.                     {  
    375.                         open[length_open].i=temp_i;  
    376.                         open[length_open].j=temp_j;  
    377.                         open[length_open].pre=length_close-1;  
    378.                         open[length_open].G=g;  
    379.                         open[length_open++].H=abs(temp_i-goal_i)+abs(temp_j-goal_j);  
    380.                     }else if(g<open[check_in].G)//新路径代价更小，则更新路径信息。  
    381.                     {  
    382.                         open[check_in].pre=length_close-1;  
    383.                         open[check_in].G=g;  
    384.                         open[check_in].H=abs(temp_i-goal_i)+abs(temp_j-goal_j);  
    385.                     }  
    386.                 }  
    387.             }  
    388.         }  
    389.     }  
    390.     temp.pre=-1;  
    391.     return temp;  
    392. }  
    393.   
    394. int LocateBuilding(int number,int *loc_i,int *loc_j)//定位建筑的坐标  
    395. {  
    396.     for(int i=0;i<ROWS;i++)  
    397.     {  
    398.         for(int j=0;j<COLS;j++)  
    399.         {  
    400.             if(maze_copy[i][j]==number)  
    401.             {  
    402.                 *loc_i=i;  
    403.                 *loc_j=j;  
    404.                 return 1;  
    405.             }  
    406.         }  
    407.     }  
    408.     return 0;  
    409. }  
    410.   
    411. void ShortestPathBetweenTwoBuildings(int NO1,int NO2)//求NO1到NO2的最小路径  
    412. {  
    413.     //int p=0;  
    414.     int length=0;  
    415.     int loc1_i,loc1_j,loc2_i,loc2_j;  
    416.     LocateBuilding(NO1,&loc1_i,&loc1_j);  
    417.     LocateBuilding(NO2,&loc2_i,&loc2_j);  
    418.     node p=aStar(loc1_i,loc1_j,loc2_i,loc2_j,maze_copy[loc2_i][loc2_j]);//A*算法求最小路径  
    419.     if(p.pre!=-1)  
    420.     {  
    421.         clear(open);  
    422.         clear(close);  
    423.         p=aStar(p.i,p.j,loc1_i,loc1_j,maze_copy[loc1_i][loc1_j]);//反向回溯寻找最优路径，防止路径重叠。  
    424.         printf(" A* Search: ");  
    425.         printf("The path of building NO%d to NO%d is: ",NO1,NO2);  
    426.         printf(" (%d,%d),%d ",p.i,p.j,maze_copy[p.i][p.j]);//先输出起始点  
    427.         length=printPath_A(p.pre);//反向打印得到的最小路径  
    428.         printf(" The path number is:%d ",length+1);  
    429.     }  
    430.     clear(open);  
    431.     clear(close);  
    432. }  
    433.   
    434. void ShortestPathForEachPairBuilding(int number)//打印所有的建筑之间的最小路径  
    435. {  
    436.     for(int i=11;i<number;i++)  
    437.     {  
    438.         for(int j=i+1;j<=number;j++)  
    439.         {  
    440.             ShortestPathBetweenTwoBuildings(j,i);//求i到j的最小路径  
    441.         }  
    442.     }  
    443. }  
    444.   
    445. void main(int argc,char *argv[])  
    446. {  
    447.     printf("Maze Map: ");  
    448.     print_maze(maze);//打印迷宫图（交通图）  
    449.       
    450.     //迷宫求解  
    451.     printf("***************************************************************************** ");  
    452.     int length=DepthFirstSearch(12,0);//深度优先算法，迷宫求解  
    453.     if(length>0 && length<ROWS*COLS)  
    454.     {  
    455.         printf("Depth First Search: ");  
    456.         print_path(maze,path,length);  
    457.         printf(" The path number is:%d ",length);  
    458.     }else{  
    459.         printf("Can't find ");  
    460.     }  
    461.     memcpy(maze_copy,maze,ROWS*COLS*sizeof(int));  
    462.   
    463.     //为建筑编号  
    464.     printf("***************************************************************************** ");  
    465.     printf("Seting numbers to every buildings: ");  
    466.     int number=BreadthFirstSearch();//利用广度优先搜索算法，对建筑编号。  
    467.     clear(open);  
    468.     clear(close);  
    469.   
    470.     //计算所有建筑之间的最优路径  
    471.     printf("***************************************************************************** ");  
    472.     //ShortestPathForEachPairBuilding(number);//利用A*算法,打印所有建筑之间的最小路径  
    473.   
    474.     //计算任意建筑之间的最优路径  
    475.     printf("***************************************************************************** ");  
    476.     while(1)  
    477.     {         
    478.         int NO1,NO2;  
    479.         printf(" Please input the Number of the Start and End Building,The Number shold between 11 and %d ",number);  
    480.         printf("If you input 0,it will close!!! ");  
    481.   
    482.         printf("The Start Building NO is:");  
    483.         scanf_s("%d",&NO1);  
    484.         while(NO1<11 || NO1>number)  
    485.         {  
    486.             if(NO1==0)  
    487.                 return;  
    488.             printf("The Start Building NO is:");  
    489.             scanf_s("%d",&NO1);  
    490.         }  
    491.   
    492.         printf("The End Building NO is:");  
    493.         scanf_s("%d",&NO2);  
    494.         while(NO2<11 || NO2>number)  
    495.         {  
    496.             if(NO1==0)  
    497.                 return;  
    498.             printf("The End Building NO is:");  
    499.             scanf_s("%d",&NO2);  
    500.         }  
    501.         ShortestPathBetweenTwoBuildings(NO1,NO2);//求NO1到NO2之间的最优路径  
    502.         printf("***************************************************************************** ");  
    503.     }  
    504. }  
  • 相关阅读:
    stl学习笔记—set/multimap
    2013 杭州站 hdoj4778 Gems Fight!
    矩阵加速数列递推
    暑假写的两个数据结构
    AC自动机模板 hdoj2222 UVA-11468
    树莓派安装3.5英寸触摸屏幕
    树莓派设置NTP同步
    在树莓派上设置无线静态IP
    运行tomcat7w.exe未安装指定的服务
    LoadRunner 11安装Micosoft Visual C++ 2005 SP1时提示命令行选项语法错误
  • 原文地址：https://www.cnblogs.com/walccott/p/4957041.html
Copyright © 2011-2022 走看看