附加题：设计地图求驾车路径

备注：本篇此题的结果将用图描绘的形式展示(有如下参考)

启发式搜索算法——A*算法_码农谷阿莫的博客-CSDN博客_启发式搜索算法

A*(Astar)搜索算法的实现(C语言)xlhdsj的博客-CSDN博客_astar算法c语言

(36条消息) A*算法简单实现（C语言）_脆弱的abc的博客-CSDN博客_a*算法代码

解决问题

全局路径规划，根据给定的目标位置和全局地图进行总体路径规划。

导航中使用A*算法计算到目标位置的最优路线，作为全局路线。

寻路算法

1.将地图虚拟化，将其划分为一个一个小方块，可以用二维数组来表示地图。

如图，黄色代表起点，绿色代表终点，黑色代表障碍物，红色代表待搜索的。

“open”列表，灰色代表已经搜索过的，”close”列表，蓝色代表当前搜索过的路径。

2.不停找周围的点，选出一个心的点作为起点再循环找，直到找到终点。

算法原理

定义

1.G：G表示从起点A移动到网络上指定方格的移动耗费（上下左右，斜方向）；

2.H：H表示从指定的方格移动到终点B的预计耗费（设定只可以上下左右移动）；

3.F：F=G+H，表示该点的总耗费；

4.open列表：一个记录下所有被考虑来寻找最短路径的格子

5.Close列表：一个记录下不会再被考虑的格子；

6.Point：（属性：是否为障碍物，父亲节点）；

初始设定

1.有一张一定宽的地图（定义好point）。

2.设定开始点和目标点。

寻路原理

开始寻路FindPath：

①开列表，关列表初始化。

②添加开始点到开列表，然后获得周压点集合并加入开列表，接着又把开始点从开列表中移除，并添加到关列表。

③判断这些周围点集合是否已经在开列表中。

不在则更新这些点的F和父亲点，并添加到开列表，

在则重新计算G值，G较小则更新G，F 和父亲点。

④从周围点集合中找到F最小的点，然后获得周围点集合，接着又把找到F最小的点从开列表中移除，并添加到关列表。

⑤接着执行第③步骤。

结束条件

1.目标格已经在“开启列表”，这个时候路径被找到。

2.如果开启列表已经空了，说明路径不存在。

寻路详细步骤

1.从起点s开始，把s作为一个等待检查的方格，放入到“开启列表”中

(“开启列表’就是一个存放等待检查方格的列表)

2.寻找起点s周围可以到达的方格(最多八个)，将它们放入到“开启列表”,并设置它们的父方格为S

3.从“开启列表”中删除起点s,并将放入到“关闭列表”中

(“关闭列表存放的是不再需要检查的方格)

4.计算每个周围方格的F值

F=G+H:

G表示从起点A移动到指定方格的移动消耗，假设横向移动-一个格子

消耗10，斜向移动-个格子消耗14 (具体值可以根据情况修改)

H表示从指定的方格移动到目标E点的预计消耗，我们假设H的计算方法，

忽略障碍物，只可以纵横向计算

5. 从“开启列表”中选择值最低的方格a ,将其从“开启列表”中删除，放入到“关闭列表”中

6.检查a所有临近并且可达的方格

a) 障碍物和“关闭列表”中的方格不考虑

b)如果这些方格还不在“开启列表”中的话，将它们加入到“开启列表”,并且计算这些方格的F值，并设置父方格为a

c) 如果某相邻的方格c已经在“开启列表”，计算新的路径从S到达方格c (即经过a的路径)

判断是否需要更新: G值是否更低一点

如果新的值更低，则修改父方格为方格a，重新计算值，H值不需要改变，因为方格到达目标点的预计消耗是固定的

如果新的值比较高，则说明新的路径消耗更高，则值不做改变(G值不变也不更新)

7.继续从“开启列表”中找出值最小的，从“开启列表”中删除，添加到"关闭列表”，再继续找出周围可以到达的方块，如此循环

8. 结束判断:

当“开启列表”中出现目标方块时，说明路径已经找到

当“开启列表”中没有了数据，则说明没有合适路径

Astar算法流程图

代码实现

（一）坐标搜索-简单Astar实现

```c#include <stdio.h>
#include <stdlib.h>
#include <math.h>   //为了使用sqrt()函数#define END_X 0
#define END_Y 4
#define START_X 3
#define START_Y 1
#define MAP_LENTH 5
#define MAP_WIDTH 5#define ERROR -1//---------------------------------------------------------------------------////定义open list和close list(里面有节点，节点还有自己的属性)
//链表数据区，是个结构体，不是链表。也就是节点的数据F、G、H还有坐标等等typedef struct Open_list Open_list, * pOpen_list;
typedef struct Node
{//父节点pOpen_list  pFather;float G;float H;//F值float F;//x,y坐标int x;int y;
}Node, * pNode;typedef struct Open_list
{struct Open_list * next;struct Node node;
}Open_list, * pOpen_list;//---------------------------------------------------------------------------//
//相关函数
float my_abs(int x);
//距离函数（给两个坐标(x1,y1)和（x2，y2））
float my_distance(int x1, int y1, int x2, int y2);
//添加链表
void list_add_tail(pOpen_list my_list, pOpen_list add_node);
//遍历链表,返回与tmpY和tmpY匹配的节点指针
pOpen_list list_browse(pOpen_list my_list, int tmpX, int tmpY);
//判断链表中是不是有某个节点（通过坐标来确定），有的话返回0，没有返回1
int judge_node_exist(pOpen_list mylist, int x, int y);
//删除链表中某个节点，通过坐标删除,并且返回这个删除的节点指针，方便加入到close list
pOpen_list list_delete_point(pOpen_list my_list, int tmpX, int tmpY);
//找到链表中最小的f值的函数,输入链表，返回最小f的节点
pOpen_list find_min_f(pOpen_list my_list);
//打印open list中的各个节点坐标以及F值
void msg_open_list(pOpen_list my_list);
//打印父节点坐标函数
void printf_father_node(pOpen_list my_list);//---------------------------------------------------------------------------//int main(int argc, char ** argv)
{//************************************************************************////1、构建地图 //障碍点为1，start为2，end为3   //地图太大了么// int map[20][20] = // {//     {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},//     {0, 0, 0, 0, 0, 0, 0, 3, 0, 0},//     {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},//     {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},//     {0, 0, 0, 1, 1, 0, 0, 0, 0, 0},//     {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},//     {0, 0, 0, 0, 1, 0, 0, 0, 0, 0},//     {0, 0, 0, 0, 0, 0, 1, 0, 0, 0},//     {0, 0, 2, 0, 0, 0, 0, 0, 0, 0},//     {1, 0, 0, 0, 0, 0, 0, 0, 0, 0}// };//障碍点为1，start为2，end为3int map[MAP_LENTH][MAP_WIDTH] = {{0,0,0,0,3},{0,0,0,1,1},{1,1,1,0,0},{0,2,0,1,0},{0,0,0,0,0}};//************************************************************************////2.把起点放入open list//创建一个指针pO指向open list和一个指针pC指向close list，注意现在open list是pO指向的那块内存 pOpen_list pO = (pOpen_list)malloc(sizeof(Open_list));pO->next = NULL;//pO = NULL;  千万不可以，NULL不可以访问的，除非pO赋值 pOpen_list pC = (pOpen_list)malloc(sizeof(Open_list));pC->next = NULL;//创建起始节点并初始化，创建一个目标节点 pOpen_list start = (pOpen_list)malloc(sizeof(Open_list));start->next = NULL;pOpen_list end = (pOpen_list)malloc(sizeof(Open_list));end->next = NULL;start->node.pFather = NULL;start->node.x = START_X;start->node.y = START_Y;start->node.G = 0;start->node.H = (my_abs(END_X - START_X) + my_abs(END_Y - START_Y));start->node.F = start->node.G + start->node.H;end->node.pFather = NULL;end->node.x = END_X;end->node.y = END_Y;//起始节点加入到open listlist_add_tail(pO, start);int i,j;    //计数来遍历 //************************************************************************//int cir = 1;while(cir){//printf("-------------------begin while--------------\n");//寻找最小的F值节点，记为pCurrent（第一次循环也就是起点）//pOpen_list pCurrent = (pOpen_list)malloc(sizeof(Open_list));msg_open_list(pO);    //现在open list中有的节点 pOpen_list pCurrent = find_min_f(pO);  //有可能找不到 //*********************************************************************////把当前点从open list中移除（通过坐标），加入到close list，记为ppOpen_list p = list_delete_point(pO, pCurrent->node.x, pCurrent->node.y);list_add_tail(pC, p);printf("core is (%d, %d)\n", p->node.x, p->node.y);printf("now ,the open list is as follow\n");msg_open_list(pO);//printf("------------------------begin for  for------------------\n");//*********************************************************************////还要考虑是不是障碍物，有没有在close list或者open list,考虑是不是边界//这块应该是遍历当前节点(p->node.x, p->node.y)的四周，并且都加入open listfor(i = -1; i < 2; i++){for(j = -1; j < 2; j++){if((p->node.x + i < 0) || (p->node.x + i > 4) || (p->node.y + j < 0) || (p->node.y + j > 4))   //超过边界了，跳过这次循环continue;if(judge_node_exist(pO, (p->node.x + i), (p->node.y + j)))    //不是open list里面的节点{if(1 == map[p->node.x + i][p->node.y + j])   //不可到达{//printf("(%d, %d) is not-reach\n", (p->node.x + i), (p->node.y + j));continue;}                     else if(!(judge_node_exist(pC, (p->node.x + i), (p->node.y + j))))   //在close list中{//printf("(%d, %d) is close list\n", (p->node.x + i), (p->node.y + j));continue;}   else if(((p->node.x + i)==END_X) && ((p->node.y+j)==END_Y))  //是目标节点，初始化{printf("找到目标\n");end->node.pFather = p;     //当前节点设为end 的父节点cir = 0;   //跳出循环标志，注意跳出的是forbreak;}else    //不在open list中的普通节点，加入进去初始化并设好父节点{//printf("(%d, %d) is normal node\n", (p->node.x + i), (p->node.y + j));pOpen_list pTmp = (pOpen_list)malloc(sizeof(Open_list));pTmp->next =NULL;pTmp->node.pFather = p;  //父节点为当前节点//节点坐标pTmp->node.x = p->node.x + i;pTmp->node.y = p->node.y + j;//节点G.H.F值       G值怎么算好(找到父节点和现在节点的坐标了)pTmp->node.G = my_distance(pTmp->node.x, pTmp->node.y, START_X, START_Y);   //初始节点到其实际距离pTmp->node.H = (my_abs(END_X - pTmp->node.x) + my_abs(END_Y - pTmp->node.y));pTmp->node.F = pTmp->node.G + pTmp->node.H;//加入到open listlist_add_tail(pO, pTmp);msg_open_list(pO);}                }else    //在open list中了{//printf("---------------------in open list-------------------\n");//printf("(%d, %d) in open list\n", (p->node.x + i), (p->node.y + j));//首先根据坐标找到他的指针 //pOpen_list pTmp = (pOpen_list)malloc(sizeof(Open_list));//pOpen_list pTmp = (pOpen_list)malloc(sizeof(Open_list));pOpen_list pTmp = list_browse(pO, (p->node.x + i), (p->node.y + j));//定义好两个的G值(一个是原本的G值，一个是通过当前节点到的G值)//所以用核心节点的G值加上当前节点到核心节点的值 float currentG = p->node.G + my_distance(pTmp->node.x, pTmp->node.y, p->node.x, p->node.y);float pastG = pTmp->node.G;//printf("currentG: %f      pastG: %f\n", currentG, pastG);if(currentG < pastG)    //当前更优{pTmp->node.pFather = p; //更换父节点//注意，更改F值和G值一定pTmp->node.G = currentG;pTmp->node.F = pTmp->node.G + pTmp->node.H;}   }}if(cir == 0)  //跳出外层的for循环break;} }//这里不能用！！！！msg_open_list啊，这个是打印所有的坐标卧槽了// msg_open_list(end->node.pFather);      //0,3// msg_open_list((end->node.pFather)->node.pFather);  //1,2// msg_open_list(end->node.pFather->node.pFather->node.pFather);printf_father_node(end);return 0;
}
//找到了，然后跳出来了循环//尾部插入链表
void list_add_tail(pOpen_list my_list, pOpen_list add_node)
{pOpen_list tmp = my_list;while (tmp->next != NULL){tmp = tmp->next;}tmp->next = add_node;add_node->next = NULL;
}//删除链表中某个节点，通过坐标删除,并且返回这个删除的节点指针，方便free
pOpen_list list_delete_point(pOpen_list my_list, int x, int y)
{while (my_list->next != NULL){if((my_list->next->node.x == x) && (my_list->next->node.y == y))  //找到删除节点{pOpen_list tmp = my_list->next;//这个节点不是最后一个节点if(my_list->next->next != NULL){my_list->next = my_list->next->next;tmp->next = NULL;}else //最后一个节点{my_list->next = NULL;tmp->next = NULL;}return tmp;}my_list = my_list->next;  }return NULL;
}//遍历链表,返回与p->node.y和p->node.y匹配的节点指针
pOpen_list list_browse(pOpen_list my_list, int x, int y)
{while (my_list->next != NULL){if((my_list->next->node.x == x) && (my_list->next->node.y == y)){return my_list->next;}my_list = my_list->next;  }return NULL;
}//判断链表中是不是有某个节点（通过坐标来确定），有的话返回0，没有返回1
int judge_node_exist(pOpen_list mylist, int x, int y)
{while(mylist->next != NULL){if((mylist->next->node.x == x) && (mylist->next->node.y == y)) //在open list{return 0;}mylist = mylist->next;}return 1;
}//找到链表中最小的f值的函数,输入链表，返回最小f的节点
pOpen_list find_min_f(pOpen_list my_list)
{//定义一个临时变量tmpf为第二个节点的F值，挨个比下去int tmpf = my_list->next->node.F;pOpen_list tmpp = my_list->next;while(my_list->next != NULL){if(tmpf > my_list->next->node.F){tmpf = my_list->next->node.F;tmpp = my_list->next;       //用一个循环就可以找到，注意！！！}my_list = my_list->next;}//找到了F值即为tmpf，怎么找到对应的节点，为什么不跟着定义一个临时节点呢return tmpp;
}//打印open list中的各个节点坐标以及F值
void msg_open_list(pOpen_list my_list)
{while(my_list->next != NULL){int x = my_list->next->node.x;int y = my_list->next->node.y;float f = my_list->next->node.F;printf("is (%d, %d).   F = %f\n", x, y, f);my_list = my_list->next;}
}//打印父节点坐标函数
void printf_father_node(pOpen_list my_list)
{while(my_list->node.pFather != NULL){printf("is(%d, %d)\n", my_list->node.pFather->node.x, my_list->node.pFather->node.y);my_list = my_list->node.pFather;}printf("track end\n");
}//绝对值函数（我的）
float my_abs(int x)
{if(x < 0){return (float)(-x);}else{return (float)(x);}
}//距离函数（给两个坐标(x1,y1)和（x2，y2））
float my_distance(int x1, int y1, int x2, int y2)
{return sqrt(  (my_abs(x1-x2)*my_abs(x1-x2)) + (my_abs(y1-y2)*my_abs(y1-y2)) );
}

（二）地图搜索路线-Atar

（1）Astar.c

```c
/** author: Atom
* date:     2012/12/03
* file:     Astar.c
*/
#include "Astar.h"#define SPEED   10long euclidean_distance(int, int, int, int);      /* 欧氏距离 */
long manhattan_distance(int, int, int, int);        /* 曼哈顿距离 */
long chebyshew_distance(int, int, int, int);        /* 切比雪夫距离 */int main(int argc, char* argv[])
{struct tile_map tmap;tmap.row = 35;tmap.column = 35;printf("euclidean distance:\n");init_map(&tmap);gen_wall(&tmap);astar(&tmap, 2 ,1 , 30, 30, euclidean_distance);destory_map(&tmap);printf("manhattan distance:\n");init_map(&tmap);gen_wall(&tmap);astar(&tmap, 3 ,3 , 30, 30, manhattan_distance);destory_map(&tmap);printf("chebyshew distance:\n");init_map(&tmap);gen_wall(&tmap);astar(&tmap, 3 ,3 , 30, 30, chebyshew_distance);destory_map(&tmap);return (0);
}/* 搜索路径 */
void astar(struct tile_map* tmap, int st_x, int st_y, int end_x,int end_y, distance_t distance)
{struct Bheap *o_heap = NULL, *c_heap = NULL;struct map_node *fnode = NULL;struct Bheap_node *inode = NULL, *onode = NULL;struct map_node *omnode = NULL;int fx = 0, fy = 0;if ((NULL == tmap) || (st_x <= 0) || (st_y <= 0) || (end_x <= 0) || (end_y <= 0))return;if (!is_reachable(tmap, st_x, st_y) || !is_reachable(tmap, end_x, end_y)){printf("开始节点或结束节点错误，无法到达!\n");return;}o_heap = Bheap_create(128, BHEAP_TYPE_SMALL);c_heap = Bheap_create(128, BHEAP_TYPE_SMALL);Bheap_init(o_heap);Bheap_init(c_heap);tmap->map[st_x][st_y] = START;tmap->map[end_x][end_y] = END;if (NULL == (fnode = MALLOC(struct map_node, 1))){fprintf(stderr, "malloc fnode error!\n");return;}if (NULL == (inode = MALLOC(struct Bheap_node, 1))){fprintf(stderr, "malloc inode error!\n");return;}memset(fnode, 0x00, sizeof(struct map_node));memset(fnode, 0x00, sizeof(struct Bheap_node));fnode->x = st_x;fnode->y = st_y;fnode->g = 0;fnode->h = distance(st_x, st_y, end_x, end_y);fnode->f = fnode->g + fnode->h;fnode->parent = NULL;inode->value = fnode;Bheap_push(o_heap, inode, _comp);#if 0print_map(tmap);
#endiffor ( ; ; ){omnode = NULL;if (NULL == (onode = Bheap_pop(o_heap, _comp))){break;}else{omnode = (struct map_node*)onode->value;if (is_arrived(tmap, omnode))break;Bheap_push(c_heap, onode, _comp);/*上*/fx = omnode->x;fy = omnode->y - 1;if (is_reachable(tmap, fx, fy)){if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*右上*/fx = omnode->x + 1;fy = omnode->y - 1;if (is_reachable(tmap, fx, fy)){    if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*右*/fx = omnode->x + 1;fy = omnode->y;if (is_reachable(tmap, fx, fy)){if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*右下*/fx = omnode->x + 1;fy = omnode->y + 1;if (is_reachable(tmap, fx, fy)){if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*下*/fx = omnode->x;fy = omnode->y + 1;if (is_reachable(tmap, fx, fy)){if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*左下*/fx = omnode->x - 1;fy = omnode->y + 1;if (is_reachable(tmap, fx, fy)){ if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*左*/fx = omnode->x - 1;fy = omnode->y;if (is_reachable(tmap, fx, fy)){ if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}/*左上*/fx = omnode->x - 1;fy = omnode->y - 1;if (is_reachable(tmap, fx, fy)){if(1 == deal_child(tmap, o_heap, c_heap, fx, fy,omnode, distance, end_x, end_y))continue;}}}if (NULL == omnode){printf("没有找到可行的路径!\n");}else{while(NULL != omnode){if ((START!= tmap->map[omnode->x][omnode->y])&& (END != tmap->map[omnode->x][omnode->y]))tmap->map[omnode->x][omnode->y] = ROAD;omnode = omnode->parent;}print_map(tmap);}Bheap_destory(&o_heap, 1, free_map_node);Bheap_destory(&c_heap, 1, free_map_node);}/* 处理↑、↗、→、↘、↓、↙、←、↖方向上的子节点 */
int deal_child(struct tile_map* tmap, struct Bheap *o_heap, struct Bheap *c_heap,int fx, int fy, struct map_node *omnode, distance_t distance, int end_x, int end_y)
{struct map_node *fnode = NULL;struct Bheap_node *inode = NULL;struct Bheap_node *exist_node = NULL;size_t idx = 0;if (NULL == (fnode = MALLOC(struct map_node, 1))){fprintf(stderr, "malloc map_node error!\n");return (-1);}if (NULL == (inode = MALLOC(struct Bheap_node, 1))){fprintf(stderr, "malloc map_node error!\n");return (-1);}memset(fnode, 0x00, sizeof(struct map_node));memset(inode, 0x00, sizeof(struct Bheap_node));fnode->x = fx;fnode->y = fy;inode->value = fnode;fnode->g = omnode->g + point_distance(omnode->x, omnode->y, fnode->x, fnode->y);fnode->h = distance(fnode->x, fnode->y, end_x, end_y);fnode->f = fnode->g + fnode->h;fnode->parent = omnode;/* 即不在open heap 也不在closed head */if (-1 == is_Bheap_contain(o_heap, inode, _eq) && -1 == is_Bheap_contain(c_heap, inode, _eq)){Bheap_push(o_heap, inode, _comp);if (is_arrived(tmap, fnode))return (1);}/* 在open heap*/else if (-1 != (idx = is_Bheap_contain(o_heap, inode, _eq))){if (NULL != (exist_node = Bheap_get(o_heap, idx))){if (fnode->f < ((struct map_node*)(exist_node->value))->f){((struct map_node*)(exist_node->value))->f = fnode->f;((struct map_node*)(exist_node->value))->parent = fnode->parent;}}free(fnode);free(inode);}/* 在closed heap */else{free(fnode);free(inode);}return (0);
}void free_map_node(struct Bheap_node* bn)
{free(bn->value);free(bn);
}/* 欧氏距离 */
long euclidean_distance(int x1, int y1, int x2, int y2)
{long distance = 0;distance = (long)sqrt((long)(pow((x1 - x2) * (SPEED) , 2)+ pow((y1 - y2) * (SPEED), 2)));return distance;
}/* 曼哈顿距离 */
long manhattan_distance(int x1, int y1, int x2, int y2)
{long distance = 0;distance = (abs(x1 - x2) + abs(y1 - y2)) * (SPEED);return distance;
}/* 切比雪夫距离 */
long chebyshew_distance(int x1, int y1, int x2, int y2)
{long distance = 0;distance = MAX(abs(x1 - x2) * (SPEED),abs(y1 - y2)* (SPEED));return distance;
}/* 实际两点距离(使用欧氏距离计算) */
long point_distance(int x1, int y1, int x2, int y2)
{return euclidean_distance(x1, y1, x2, y2);
}/* 判断点是否可达 */
int is_reachable(struct tile_map* tmap, int x, int y)
{if ((x >= (tmap->row - 1)) || (y >= (tmap->column - 1)) || (x < 1) || (y < 1) || (WALL == tmap->map[x][y]))return (0);return (1);}/* 判断是否到达终点 */
int is_arrived(struct tile_map* tmap, struct map_node* map_node)
{if (is_reachable(tmap, map_node->x, map_node->y) && (END == tmap->map[map_node->x][map_node->y]))return (1);elsereturn (0);
}/* Bheap_compare_t 函数实现 */
int _comp(struct Bheap_node* n1, struct Bheap_node* n2)
{struct map_node *mn1 = NULL, *mn2 = NULL;if ((NULL != n1) && (NULL != n2)){mn1 = (struct map_node*)n1->value;mn2 = (struct map_node*)n2->value;if (mn1->f > mn2->f)return (1);else if(mn1->f == mn2->f)return (0);elsereturn (-1);}elsereturn (0);
}/* Bheap_equal_t 函数实现 */
int _eq(struct Bheap_node* n1, struct Bheap_node* n2)
{struct map_node *mn1 = NULL, *mn2 = NULL;if ((NULL != n1) && (NULL != n2)){mn1 = (struct map_node*)n1->value;mn2 = (struct map_node*)n2->value;return ((mn1->x == mn2->x) && (mn1->y ==mn2->y));}elsereturn (0);
}/* 初始化map */
int init_map(struct tile_map* tmap)
{int o_idx;int i ,j;if (NULL == tmap)return (-1);tmap->map = MALLOC(int*, tmap->row);memset(tmap->map, 0x00, sizeof(int*) * tmap->row);for (o_idx = 0; o_idx < tmap->row; o_idx++){tmap->map[o_idx] = MALLOC(int, tmap->column);memset(tmap->map[o_idx], 0x00, sizeof(int) * tmap->column);}
}/*  */
void gen_wall(struct tile_map* tmap)
{if (NULL == tmap)return;
#if 1tmap->map[2][2] = WALL;tmap->map[2][4] = WALL;tmap->map[3][4] = WALL;tmap->map[4][4] = WALL;tmap->map[4][3] = WALL;tmap->map[3][2] = WALL;tmap->map[29][29] = WALL;tmap->map[29][30] = WALL;tmap->map[29][31] = WALL;tmap->map[30][31] = WALL;tmap->map[31][30] = WALL;tmap->map[31][29] = WALL;tmap->map[30][29] = WALL;
#endif
}/* 销毁map */
void destory_map(struct tile_map* tmap)
{int o_idx;if (NULL == tmap)return;for (o_idx = 0; o_idx < tmap->row; o_idx++)free(tmap->map[o_idx]);free(tmap->map);tmap->map = NULL;
}/* 打印map */
static void print_map(struct tile_map* tmap)
{int o_idx, i_idx;if (NULL == tmap)return;for (o_idx = 0; o_idx < tmap->row; o_idx++){for (i_idx = 0; i_idx < tmap->column; i_idx++){if (0 == o_idx || (tmap->row - 1 == o_idx))printf("--");else if (0 == i_idx || (tmap->column - 1 == i_idx))printf("| ");else if(START == tmap->map[o_idx][i_idx])printf("S ");else if (END == tmap->map[o_idx][i_idx])printf("E ");else if (ROAD == tmap->map[o_idx][i_idx])printf("0 ");else if (WALL == tmap->map[o_idx][i_idx])printf("W ");elseprintf("  ");}printf("\n");}
}

（2）Astar.h

```c
/*
* author:   Atom
* date:     2012/12/03
* file:     Astar.h
*/
#ifndef ASTAR_H
#define ASTAR_H#include <stdio.h>
#include <math.h>
#include "bheap.h"#define MALLOC(type,n)  (type *)malloc((n)*sizeof(type))#define MAX(a,b) ((a)>(b))?(a):(b)#define START      1
#define END         -1
#define EMPTY       0
#define WALL        9
#define ROAD        8struct tile_map
{int** map;int row;int column;
};struct map_node
{int x;int y;long f;                                            /*最终路径长度*/long g;                                           /*起点到该点的已知长度*/long h;                                           /*该点到终点的估计长度*/struct map_node* parent;
};typedef long (* distance_t)(int, int, int, int);int init_map(struct tile_map*);
void gen_wall(struct tile_map*);
void destory_map(struct tile_map*);
void astar(struct tile_map*, int, int, int, int, distance_t);
int _comp(struct Bheap_node*, struct Bheap_node*);
int _eq(struct Bheap_node*, struct Bheap_node*);
int is_reachable(struct tile_map*, int, int);
int is_arrived(struct tile_map*, struct map_node*);
void free_map_node(struct Bheap_node*);
int deal_child(struct tile_map*, struct Bheap*, struct Bheap*, int, int,struct map_node*, distance_t, int, int);
long point_distance(int, int, int, int);static void print_map(struct tile_map* tmap);
static void print_point(struct map_node*, char );
static void print_heap(struct Bheap*);#endif        /*ASTAR_H*/

（三）数据结构：最短路径_Dijkstra

```c
#include "stdio.h"
#include "stdlib.h"
#include "io.h"
#include "math.h"
#include "time.h"#define OK 1
#define ERROR 0
#define TRUE 1
#define FALSE 0#define MAXEDGE 20
#define MAXVEX 20
#define GRAPH_INFINITY 65535typedef int Status; /* Status是函数的类型,其值是函数结果状态代码，如OK等 */ typedef struct
{int vexs[MAXVEX];int arc[MAXVEX][MAXVEX];int numVertexes, numEdges;
}MGraph;typedef int Patharc[MAXVEX];    /* 用于存储最短路径下标的数组 */
typedef int ShortPathTable[MAXVEX];/* 用于存储到各点最短路径的权值和 *//* 构件图 */
void CreateMGraph(MGraph *G)
{int i, j;/* printf("请输入边数和顶点数:"); */G->numEdges=16;G->numVertexes=9;for (i = 0; i < G->numVertexes; i++)/* 初始化图 */{G->vexs[i]=i;}for (i = 0; i < G->numVertexes; i++)/* 初始化图 */{for ( j = 0; j < G->numVertexes; j++){if (i==j)G->arc[i][j]=0;elseG->arc[i][j] = G->arc[j][i] = GRAPH_INFINITY;}}G->arc[0][1]=1;G->arc[0][2]=5; G->arc[1][2]=3; G->arc[1][3]=7; G->arc[1][4]=5; G->arc[2][4]=1; G->arc[2][5]=7; G->arc[3][4]=2; G->arc[3][6]=3; G->arc[4][5]=3;G->arc[4][6]=6;G->arc[4][7]=9; G->arc[5][7]=5; G->arc[6][7]=2; G->arc[6][8]=7;G->arc[7][8]=4;for(i = 0; i < G->numVertexes; i++){for(j = i; j < G->numVertexes; j++){G->arc[j][i] =G->arc[i][j];}}}/*  Dijkstra算法，求有向网G的v0顶点到其余顶点v的最短路径P[v]及带权长度D[v] */
/*  P[v]的值为前驱顶点下标,D[v]表示v0到v的最短路径长度和 */
void ShortestPath_Dijkstra(MGraph G, int v0, Patharc *P, ShortPathTable *D)
{    int v,w,k,min;    int final[MAXVEX];/* final[w]=1表示求得顶点v0至vw的最短路径 */for(v=0; v<G.numVertexes; v++)    /* 初始化数据 */{        final[v] = 0;            /* 全部顶点初始化为未知最短路径状态 */(*D)[v] = G.arc[v0][v];/* 将与v0点有连线的顶点加上权值 */(*P)[v] = -1;               /* 初始化路径数组P为-1  */       }(*D)[v0] = 0;  /* v0至v0路径为0 */  final[v0] = 1;    /* v0至v0不需要求路径 */        /* 开始主循环，每次求得v0到某个v顶点的最短路径 */   for(v=1; v<G.numVertexes; v++)   {min=GRAPH_INFINITY;    /* 当前所知离v0顶点的最近距离 */        for(w=0; w<G.numVertexes; w++) /* 寻找离v0最近的顶点 */    {            if(!final[w] && (*D)[w]<min)             {                   k=w;                    min = (*D)[w];    /* w顶点离v0顶点更近 */            }        }        final[k] = 1;    /* 将目前找到的最近的顶点置为1 */for(w=0; w<G.numVertexes; w++) /* 修正当前最短路径及距离 */{/* 如果经过v顶点的路径比现在这条路径的长度短的话 */if(!final[w] && (min+G.arc[k][w]<(*D)[w]))   { /*  说明找到了更短的路径，修改D[w]和P[w] */(*D)[w] = min + G.arc[k][w];  /* 修改当前路径长度 */               (*P)[w]=k;        }       }   }
}int main(void)
{   int i,j,v0;MGraph G;    Patharc P;    ShortPathTable D; /* 求某点到其余各点的最短路径 */   v0=0;CreateMGraph(&G);ShortestPath_Dijkstra(G, v0, &P, &D);  printf("最短路径倒序如下:\n");    for(i=1;i<G.numVertexes;++i)   {       printf("v%d - v%d : ",v0,i);j=i;while(P[j]!=-1){printf("%d ",P[j]);j=P[j];}printf("\n");}    printf("\n源点到各顶点的最短路径长度为:\n");  for(i=1;i<G.numVertexes;++i)        printf("v%d - v%d : %d \n",G.vexs[0],G.vexs[i],D[i]);     return 0;
}

（四）数据结构：最短路径_Floyd

```c
#include "stdio.h"
#include "stdlib.h"   #include "math.h"
#include "time.h"#define OK 1
#define ERROR 0
#define TRUE 1
#define FALSE 0
#define MAXEDGE 20
#define MAXVEX 20
#define GRAPH_INFINITY 65535typedef int Status; /* Status是函数的类型,其值是函数结果状态代码，如OK等 */typedef struct
{int vexs[MAXVEX];int arc[MAXVEX][MAXVEX];int numVertexes, numEdges;
}MGraph;typedef int Patharc[MAXVEX][MAXVEX];
typedef int ShortPathTable[MAXVEX][MAXVEX];/* 构件图 */
void CreateMGraph(MGraph *G)
{int i, j;/* printf("请输入边数和顶点数:"); */G->numEdges=16;G->numVertexes=9;for (i = 0; i < G->numVertexes; i++)/* 初始化图 */{G->vexs[i]=i;}for (i = 0; i < G->numVertexes; i++)/* 初始化图 */{for ( j = 0; j < G->numVertexes; j++){if (i==j)G->arc[i][j]=0;elseG->arc[i][j] = G->arc[j][i] = GRAPH_INFINITY;}}G->arc[0][1]=1;G->arc[0][2]=5; G->arc[1][2]=3; G->arc[1][3]=7; G->arc[1][4]=5; G->arc[2][4]=1; G->arc[2][5]=7; G->arc[3][4]=2; G->arc[3][6]=3; G->arc[4][5]=3;G->arc[4][6]=6;G->arc[4][7]=9; G->arc[5][7]=5; G->arc[6][7]=2; G->arc[6][8]=7;G->arc[7][8]=4;for(i = 0; i < G->numVertexes; i++){for(j = i; j < G->numVertexes; j++){G->arc[j][i] =G->arc[i][j];}}}/* Floyd算法，求网图G中各顶点v到其余顶点w的最短路径P[v][w]及带权长度D[v][w]。 */
void ShortestPath_Floyd(MGraph G, Patharc *P, ShortPathTable *D)
{    int v,w,k;    for(v=0; v<G.numVertexes; ++v) /* 初始化D与P */  {        for(w=0; w<G.numVertexes; ++w)  {(*D)[v][w]=G.arc[v][w];  /* D[v][w]值即为对应点间的权值 */(*P)[v][w]=w;               /* 初始化P */}}for(k=0; k<G.numVertexes; ++k)   {for(v=0; v<G.numVertexes; ++v)  {        for(w=0; w<G.numVertexes; ++w)    {if ((*D)[v][w]>(*D)[v][k]+(*D)[k][w]){/* 如果经过下标为k顶点路径比原两点间路径更短 */(*D)[v][w]=(*D)[v][k]+(*D)[k][w];/* 将当前两点间权值设为更小的一个 */(*P)[v][w]=(*P)[v][k];/* 路径设置为经过下标为k的顶点 */}}}}
}int main(void)
{    int v,w,k;  MGraph G;    Patharc P;    ShortPathTable D; /* 求某点到其余各点的最短路径 */   CreateMGraph(&G);ShortestPath_Floyd(G,&P,&D);  printf("各顶点间最短路径如下:\n");    for(v=0; v<G.numVertexes; ++v)   {        for(w=v+1; w<G.numVertexes; w++)  {printf("v%d-v%d weight: %d ",v,w,D[v][w]);k=P[v][w];              /* 获得第一个路径顶点下标 */printf(" path: %d",v);   /* 打印源点 */while(k!=w)              /* 如果路径顶点下标不是终点 */{printf(" -> %d",k); /* 打印路径顶点 */k=P[k][w];         /* 获得下一个路径顶点下标 */}printf(" -> %d\n",w);    /* 打印终点 */}printf("\n");}printf("最短路径D\n");for(v=0; v<G.numVertexes; ++v)  {        for(w=0; w<G.numVertexes; ++w)    {printf("%d\t",D[v][w]);}printf("\n");}printf("最短路径P\n");for(v=0; v<G.numVertexes; ++v)  {        for(w=0; w<G.numVertexes; ++w)    {printf("%d ",P[v][w]);}printf("\n");}return 0;
}