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



【推广】 免费学中医，健康全家人
原文地址：https://www.cnblogs.com/walccott/p/4957041.html