备注：本篇此题的结果将用图描绘的形式展示(有如下参考)
启发式搜索算法——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为3
int 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 list
list_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，记为p
pOpen_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 list
for(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; //跳出循环标志，注意跳出的是for
break;
}
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 list
list_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 10
long 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 0
print_map(tmap);
#endif
for ( ; ; )
{
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);
else
return (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);
else
return (-1);
}
else
return (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));
}
else
return (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 1
tmap->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 ");
else
printf(" ");
}
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 8
struct 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 65535
typedef 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;
else
G->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 65535
typedef 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;
else
G->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;
}
2053
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
