石器时代LA官方

 找回密码
 立即注册
搜索
热搜: 活动 交友 discuz
查看: 55|回复: 0

[基础教材] 石器时代无挂实现自动寻路的相关代码

[复制链接]

1万

主题

1万

帖子

1万

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
17427
石币
7306
发表于 2018-9-15 20:57:12 | 显示全部楼层 |阅读模式

距离估计与实际值越接近,估价函数取得就越好
例如对于几何路网来说,可以取两节点间曼哈顿距离做为距离估计,即f=g(n) + (abs(dx - nx) + abs(dy - ny));这样估价函数f(n)在g(n)一定的情况下,会或多或少的受距离估计值h(n)的制约,节点距目标点近,h值小,f值相对就小,能保证最短路的搜索向终点的方向进行。明显优于Dijkstra算法的毫无方向的向四周搜索。
算法实现(路径搜索)
创建两个表,OPEN表保存所有已生成而未考察的节点,CLOSED表中记录已访问过的节点。
算起点的h(s);
将起点放入OPEN表;
  1. while(OPEN!=NULL)
  2. {
  3.     从OPEN表中取f(n)最小的节点n;
  4.     if(n节点==目标节点)
  5.         break;
  6.     for(当前节点n的每个子节点X)
  7.     {
  8.         计算f(X);
  9.         if(XinOPEN)
  10.             if(新的f(X)<OPEN中的f(X))
  11.             {
  12.                 把n设置为X的父亲;
  13.                 更新OPEN表中的f(n);
  14.             }
  15.         if(XinCLOSE)
  16.             continue;
  17.         if(Xnotinboth)
  18.         {
  19.             把n设置为X的父亲;
  20.             求f(X);
  21.             并将X插入OPEN表中;//还没有排序
  22.         }
  23.     }//endfor
  24.     将n节点插入CLOSE表中;
  25.     按照f(n)将OPEN表中的节点排序;//实际上是比较OPEN表内节点f的大小,从最小路径的节点向下进行。
  26. }//endwhile(OPEN!=NULL)
复制代码
保存路径,即从终点开始,每个节点沿着父节点移动直至起点,这就是你的路径;
用C语言实现A*最短路径搜索算法
  1. #include <stdio.h>
  2. #include <math.h>
  3.   
  4. #define MaxLength 100    //用于优先队列(Open表)的数组
  5. #define Height     15    //地图高度
  6. #define Width      20    //地图宽度
  7.   
  8. #define Reachable   0    //可以到达的结点
  9. #define Bar         1    //障碍物
  10. #define Pass        2    //需要走的步数
  11. #define Source      3    //起点
  12. #define Destination 4    //终点
  13.   
  14. #define Sequential  0    //顺序遍历
  15. #define NoSolution  2    //无解决方案
  16. #define Infinity    0xfffffff
  17.   
  18. #define East       (1 << 0)
  19. #define South_East (1 << 1)
  20. #define South      (1 << 2)
  21. #define South_West (1 << 3)
  22. #define West       (1 << 4)
  23. #define North_West (1 << 5)
  24. #define North      (1 << 6)
  25. #define North_East (1 << 7)
  26.   
  27. typedef struct
  28. {
  29.     signed char x, y;
  30. } Point;
  31.   
  32. const Point dir[8] =
  33. {
  34.     {0, 1},   // East
  35.     {1, 1},   // South_East
  36.     {1, 0},   // South
  37.     {1, -1},  // South_West
  38.     {0, -1},  // West
  39.     {-1, -1}, // North_West
  40.     {-1, 0},  // North
  41.     {-1, 1}   // North_East
  42. };
  43.   
  44. unsigned char within(int x, int y)
  45. {
  46.     return (x >= 0 && y >= 0
  47.         && x < Height && y < Width);
  48. }
  49.   
  50. typedef struct
  51. {
  52.     int x, y;
  53.     unsigned char reachable, sur, value;
  54. } MapNode;
  55.   
  56. typedef struct Close
  57. {
  58.     MapNode *cur;
  59.     char vis;
  60.     struct Close *from;
  61.     float F, G;
  62.     int H;
  63. } Close;
  64.   
  65. typedef struct //优先队列(Open表)
  66. {
  67.     int length;        //当前队列的长度
  68.     Close* Array[MaxLength];    //评价结点的指针
  69. } Open;
  70.   
  71. static MapNode graph[Height][Width];
  72. static int srcX, srcY, dstX, dstY;    //起始点、终点
  73. static Close close[Height][Width];
  74.   
  75. // 优先队列基本操作
  76. void initOpen(Open *q)    //优先队列初始化
  77. {
  78.     q->length = 0;        // 队内元素数初始为0
  79. }
  80.   
  81. void push(Open *q, Close cls[Height][Width], int x, int y, float g)
  82. {    //向优先队列(Open表)中添加元素
  83.     Close *t;
  84.     int i, mintag;
  85.     cls[x][y].G = g;    //所添加节点的坐标
  86.     cls[x][y].F = cls[x][y].G + cls[x][y].H;
  87.     q->Array[q->length++] = &(cls[x][y]);
  88.     mintag = q->length - 1;
  89.     for (i = 0; i < q->length - 1; i++)
  90.     {
  91.         if (q->Array[i]->F < q->Array[mintag]->F)
  92.         {
  93.             mintag = i;
  94.         }
  95.     }
  96.     t = q->Array[q->length - 1];
  97.     q->Array[q->length - 1] = q->Array[mintag];
  98.     q->Array[mintag] = t;    //将评价函数值最小节点置于队头
  99. }
  100.   
  101. Close* shift(Open *q)
  102. {
  103.     return q->Array[--q->length];
  104. }
  105.   
  106. // 地图初始化操作
  107. void initClose(Close cls[Height][Width], int sx, int sy, int dx, int dy)
  108. {    // 地图Close表初始化配置
  109.     int i, j;
  110.     for (i = 0; i < Height; i++)
  111.     {
  112.         for (j = 0; j < Width; j++)
  113.         {
  114.             cls[i][j].cur = &graph[i][j];        // Close表所指节点
  115.             cls[i][j].vis = !graph[i][j].reachable;        // 是否被访问
  116.             cls[i][j].from = NULL;                // 所来节点
  117.             cls[i][j].G = cls[i][j].F = 0;
  118.             cls[i][j].H = abs(dx - i) + abs(dy - j);    // 评价函数值
  119.         }
  120.     }
  121.     cls[sx][sy].F = cls[sx][sy].H;            //起始点评价初始值
  122.     //    cls[sy][sy].G = 0;                        //移步花费代价值
  123.     cls[dx][dy].G = Infinity;
  124. }
  125.   
  126. void initGraph(const int map[Height][Width], int sx, int sy, int dx, int dy)
  127. {    //地图发生变化时重新构造地
  128.     int i, j;
  129.     srcX = sx;    //起点X坐标
  130.     srcY = sy;    //起点Y坐标
  131.     dstX = dx;    //终点X坐标
  132.     dstY = dy;    //终点Y坐标
  133.     for (i = 0; i < Height; i++)
  134.     {
  135.         for (j = 0; j < Width; j++)
  136.         {
  137.             graph[i][j].x = i; //地图坐标X
  138.             graph[i][j].y = j; //地图坐标Y
  139.             graph[i][j].value = map[i][j];
  140.             graph[i][j].reachable = (graph[i][j].value == Reachable);    // 节点可到达性
  141.             graph[i][j].sur = 0; //邻接节点个数
  142.             if (!graph[i][j].reachable)
  143.             {
  144.                 continue;
  145.             }
  146.             if (j > 0)
  147.             {
  148.                 if (graph[i][j - 1].reachable)    // left节点可以到达
  149.                 {
  150.                     graph[i][j].sur |= West;
  151.                     graph[i][j - 1].sur |= East;
  152.                 }
  153.                 if (i > 0)
  154.                 {
  155.                     if (graph[i - 1][j - 1].reachable
  156.                         && graph[i - 1][j].reachable
  157.                         && graph[i][j - 1].reachable)    // up-left节点可以到达
  158.                     {
  159.                         graph[i][j].sur |= North_West;
  160.                         graph[i - 1][j - 1].sur |= South_East;
  161.                     }
  162.                 }
  163.             }
  164.             if (i > 0)
  165.             {
  166.                 if (graph[i - 1][j].reachable)    // up节点可以到达
  167.                 {
  168.                     graph[i][j].sur |= North;
  169.                     graph[i - 1][j].sur |= South;
  170.                 }
  171.                 if (j < Width - 1)
  172.                 {
  173.                     if (graph[i - 1][j + 1].reachable
  174.                         && graph[i - 1][j].reachable
  175.                         && map[i][j + 1] == Reachable) // up-right节点可以到达
  176.                     {
  177.                         graph[i][j].sur |= North_East;
  178.                         graph[i - 1][j + 1].sur |= South_West;
  179.                     }
  180.                 }
  181.             }
  182.         }
  183.     }
  184. }
  185.   
  186. int bfs()
  187. {
  188.     int times = 0;
  189.     int i, curX, curY, surX, surY;
  190.     unsigned char f = 0, r = 1;
  191.     Close *p;
  192.     Close* q[MaxLength] = { &close[srcX][srcY] };
  193.   
  194.     initClose(close, srcX, srcY, dstX, dstY);
  195.     close[srcX][srcY].vis = 1;
  196.   
  197.     while (r != f)
  198.     {
  199.         p = q[f];
  200.         f = (f + 1) % MaxLength;
  201.         curX = p->cur->x;
  202.         curY = p->cur->y;
  203.         for (i = 0; i < 8; i++)
  204.         {
  205.             if (! (p->cur->sur & (1 << i)))
  206.             {
  207.                 continue;
  208.             }
  209.             surX = curX + dir[i].x;
  210.             surY = curY + dir[i].y;
  211.             if (! close[surX][surY].vis)
  212.             {
  213.                 close[surX][surY].from = p;
  214.                 close[surX][surY].vis = 1;
  215.                 close[surX][surY].G = p->G + 1;
  216.                 q[r] = &close[surX][surY];
  217.                 r = (r + 1) % MaxLength;
  218.             }
  219.         }
  220.         times++;
  221.     }
  222.     return times;
  223. }
  224.   
  225. int astar()
  226. {    // A*算法遍历
  227.     //int times = 0;
  228.     int i, curX, curY, surX, surY;
  229.     float surG;
  230.     Open q; //Open表
  231.     Close *p;
  232.   
  233.     initOpen(&q);
  234.     initClose(close, srcX, srcY, dstX, dstY);
  235.     close[srcX][srcY].vis = 1;
  236.     push(&q, close, srcX, srcY, 0);
  237.   
  238.     while (q.length)
  239.     {    //times++;
  240.         p = shift(&q);
  241.         curX = p->cur->x;
  242.         curY = p->cur->y;
  243.         if (!p->H)
  244.         {
  245.             return Sequential;
  246.         }
  247.         for (i = 0; i < 8; i++)
  248.         {
  249.             if (! (p->cur->sur & (1 << i)))
  250.             {
  251.                 continue;
  252.             }
  253.             surX = curX + dir[i].x;
  254.             surY = curY + dir[i].y;
  255.             if (!close[surX][surY].vis)
  256.             {
  257.                 close[surX][surY].vis = 1;
  258.                 close[surX][surY].from = p;
  259.                 surG = p->G + sqrt((curX - surX) * (curX - surX) + (curY - surY) * (curY - surY));
  260.                 push(&q, close, surX, surY, surG);
  261.             }
  262.         }
  263.     }
  264.     //printf("times: %d\n", times);
  265.     return NoSolution; //无结果
  266. }
  267.   
  268. const int map[Height][Width] = {
  269.     {0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,1,1},
  270.     {0,0,1,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1},
  271.     {0,0,0,0,0,0,1,0,0,0,0,0,0,1,1,0,0,0,0,1},
  272.     {0,0,0,0,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0},
  273.     {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,0,1},
  274.     {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0},
  275.     {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0},
  276.     {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
  277.     {0,0,0,1,0,0,0,0,0,1,1,0,0,0,0,0,0,0,0,0},
  278.     {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0},
  279.     {0,1,1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0},
  280.     {0,0,0,0,1,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0},
  281.     {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,0},
  282.     {0,1,0,0,0,0,1,0,0,0,0,0,0,1,0,1,0,0,0,1},
  283.     {0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0}
  284. };
  285.   
  286. const char Symbol[5][3] = { "□", "▓", "▽", "☆", "◎" };
  287.   
  288. void printMap()
  289. {
  290.     int i, j;
  291.     for (i = 0; i < Height; i++)
  292.     {
  293.         for (j = 0; j < Width; j++)
  294.         {
  295.             printf("%s", Symbol[graph[i][j].value]);
  296.         }
  297.         puts("");
  298.     }
  299.     puts("");
  300. }
  301.   
  302. Close* getShortest()
  303. {    // 获取最短路径
  304.     int result = astar();
  305.     Close *p, *t, *q = NULL;
  306.     switch(result)
  307.     {
  308.     case Sequential:    //顺序最近
  309.         p = &(close[dstX][dstY]);
  310.         while (p)    //转置路径
  311.         {
  312.             t = p->from;
  313.             p->from = q;
  314.             q = p;
  315.             p = t;
  316.         }
  317.         close[srcX][srcY].from = q->from;
  318.         return &(close[srcX][srcY]);
  319.     case NoSolution:
  320.         return NULL;
  321.     }
  322.     return NULL;
  323. }
  324.   
  325. static Close *start;
  326. static int shortestep;
  327. int printShortest()
  328. {
  329.     Close *p;
  330.     int step = 0;
  331.   
  332.     p = getShortest();
  333.     start = p;
  334.     if (!p)
  335.     {
  336.         return 0;
  337.     }
  338.     else
  339.     {
  340.         while (p->from)
  341.         {
  342.             graph[p->cur->x][p->cur->y].value = Pass;
  343.             printf("(%d,%d)→\n", p->cur->x, p->cur->y);
  344.             p = p->from;
  345.             step++;
  346.         }
  347.         printf("(%d,%d)\n", p->cur->x, p->cur->y);
  348.         graph[srcX][srcY].value = Source;
  349.         graph[dstX][dstY].value = Destination;
  350.         return step;
  351.     }
  352. }
  353.   
  354. void clearMap()
  355. {    // Clear Map Marks of Steps
  356.     Close *p = start;
  357.     while (p)
  358.     {
  359.         graph[p->cur->x][p->cur->y].value = Reachable;
  360.         p = p->from;
  361.     }
  362.     graph[srcX][srcY].value = map[srcX][srcY];
  363.     graph[dstX][dstY].value = map[dstX][dstY];
  364. }
  365.   
  366. void printDepth()
  367. {
  368.     int i, j;
  369.     for (i = 0; i < Height; i++)
  370.     {
  371.         for (j = 0; j < Width; j++)
  372.         {
  373.             if (map[i][j])
  374.             {
  375.                 printf("%s ", Symbol[graph[i][j].value]);
  376.             }
  377.             else
  378.             {
  379.                 printf("%2.0lf ", close[i][j].G);
  380.             }
  381.         }
  382.         puts("");
  383.     }
  384.     puts("");
  385. }
  386.   
  387. void printSur()
  388. {
  389.     int i, j;
  390.     for (i = 0; i < Height; i++)
  391.     {
  392.         for (j = 0; j < Width; j++)
  393.         {
  394.             printf("%02x ", graph[i][j].sur);
  395.         }
  396.         puts("");
  397.     }
  398.     puts("");
  399. }
  400.   
  401. void printH()
  402. {
  403.     int i, j;
  404.     for (i = 0; i < Height; i++)
  405.     {
  406.         for (j = 0; j < Width; j++)
  407.         {
  408.             printf("%02d ", close[i][j].H);
  409.         }
  410.         puts("");
  411.     }
  412.     puts("");
  413. }
  414.   
  415. int main(int argc, const char **argv)
  416. {
  417.     initGraph(map, 0, 0, 0, 0);
  418.     printMap();
  419.   
  420.     while (scanf("%d %d %d %d", &srcX, &srcY, &dstX, &dstY) != EOF)
  421.     {
  422.         if (within(srcX, srcY) && within(dstX, dstY))
  423.         {
  424.             if (shortestep = printShortest())
  425.             {
  426.                 printf("从(%d,%d)到(%d,%d)的最短步数是: %d\n",
  427.                     srcX, srcY, dstX, dstY, shortestep);
  428.                 printMap();
  429.                 clearMap();
  430.                 bfs();
  431.                 //printDepth();
  432.                 puts((shortestep == close[dstX][dstY].G) ? "正确" : "错误");
  433.                 clearMap();
  434.             }
  435.             else
  436.             {
  437.                 printf("从(%d,%d)不可到达(%d,%d)\n",
  438.                     srcX, srcY, dstX, dstY);
  439.             }
  440.         }
  441.         else
  442.         {
  443.             puts("输入错误!");
  444.         }
  445.     }
  446.     return (0);
  447. }
复制代码


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

小黑屋|手机版|Archiver|石器时代LA官方

GMT+8, 2018-10-15 19:19 , Processed in 0.223780 second(s), 21 queries .

快速回复 返回顶部 返回列表