A*(Astar)搜索算法的实现(C语言)
A*（A-Star)算法是一种静态路网中求解最短路最有效的方法，是启发搜索中的一种。
1.起源
名字创意来源于第一届百度之星比赛决赛中有题目是一道经典的8数码题目，解这道题，冠军ACRush使用了A*算法（Astar）。Astar又包含了“百度之星”的含义。
2.算法的描述
2.1 该算法可以用如下等式表示:
f(n) = g(n) + h(n)
其中
f(n) 是从初始点经由节点n到目标点的移动耗费。
g(n) 是在状态空间中从起点到n节点的实际耗费。
h(n) 从网格上那个方格移动到终点的预估移动耗费。这经常被称为启发式的，这样叫的原因是因为它只是个猜测。我们没办法事先知道路径的长度，因为路上可能存在各种障碍(墙，水，等等)。
g(n) 是在状态空间中从起点到n节点的实际耗费。
h(n) 从网格上那个方格移动到终点的预估移动耗费。这经常被称为启发式的，这样叫的原因是因为它只是个猜测。我们没办法事先知道路径的长度，因为路上可能存在各种障碍(墙，水，等等)。
2.2 该算法的伪代码描述如下:
创建两个表，OPEN表保存所有已生成而未考察的节点，CLOSED表中记录已访问过的节点;
算起点的估价值;
将起点放入OPEN表;
while(OPEN!=NULL)
{
从OPEN表中取估价值f最小的节点n;
if(n节点==目标节点)
{
break;
}
for(当前节点n 的每个子节点X)
{
算X的估价值;
if(X in OPEN)
{
if( X的估价值小于OPEN表的估价值 )
{
把n设置为X的父亲;
更新OPEN表中的估价值; /*取最小路径的估价值*/
}
}
if(X in CLOSE)
{
continue;
}
if(X not in both)
{
把n设置为X的父亲;
求X的估价值;
并将X插入OPEN表中;
}
}/* end for */
将n节点插入CLOSE表中;
按照估价值将OPEN表中的节点排序; /*实际上是比较OPEN表内节点f的大小，从最小路径的节点向下进行。*/
}/* end while(OPEN!=NULL) */
保存路径，即 从终点开始，每个节点沿着父节点移动直至起点，这就是你的路径；3.算法实现的几个关键点
3.1 维护open表
算法中从open表中需要选择估算值最小的节点进行，相当于open表是一个有序序列。对于open表的实现可以用完全排序链表。但是对于节点数量大的网络中有一个更优的选择，那就是二叉堆。
二叉堆是一种特殊的堆，二叉堆是完全二元树或者是近似完全二元树。二叉堆有两种:最大堆和最小堆。最大堆:父结点的键值总是大于或等于任何一个子节点的键值；最小堆:父结点的键值总是小于或等于任何一个子节点的键值。 在这里用到是最小二叉堆。二叉堆的介绍也可以参考: Using Binary Heaps in A* Pathfinding
二叉堆是一种特殊的堆，二叉堆是完全二元树或者是近似完全二元树。二叉堆有两种:最大堆和最小堆。最大堆:父结点的键值总是大于或等于任何一个子节点的键值；最小堆:父结点的键值总是小于或等于任何一个子节点的键值。 在这里用到是最小二叉堆。二叉堆的介绍也可以参考: Using Binary Heaps in A* Pathfinding
二叉堆实现代码如下:
bheap.h
/*
* author: Atom
* date: 2012/12/02
* file: bheap.h
*/
#ifndef _BHEAP_H
#define _BHEAP_H
#include
#include
#include
#define DEF_SIZE 16 /* 默认heap大小 */
#define INC_RATE 1.5 /* heap自动增长速度 */
enum Bheap_type
{
BHEAP_TYPE_BIG, /* 大根二叉堆 */
BHEAP_TYPE_SMALL /* 小根二叉堆 */
};
struct Bheap_node
{
void* value;
};
struct Bheap
{
struct Bheap_node** head;
size_t size; /* 当前元素个数 */
size_t max_size; /* 堆容量 */
enum Bheap_type Btype; /* 堆类型 */
};
/* condition result
* n1 > n2 1
* n1 == n2 0
* n1 < n2 -1
*/
typedef int (*Bheap_compare_t)(struct Bheap_node* n1, struct Bheap_node* n2);
/* condition result
* n1 == n2 1
* n1 != n2 0
*/
typedef int (*Bheap_equal_t)(struct Bheap_node* n1, struct Bheap_node* n2);
/* free 节点,包括bn->value */
typedef void (*Bheap_free_node_t)(struct Bheap_node* bn);
/*根据 Bheap_type 创建二叉堆,如果未指定堆大小或指定的堆大小 小于等于 1,则初始化堆容量为16*/
static struct Bheap* Bheap_create(size_t size, enum Bheap_type Btype)
{
struct Bheap* heap = NULL;
if (NULL == (heap = (struct Bheap*)malloc(sizeof(struct Bheap))))
{
fprintf(stderr, "Bheap malloc error!\n");
return (NULL);
}
memset(heap, 0x00, sizeof(struct Bheap));
if (size > 1)
{
heap->head = (struct Bheap_node**)malloc(sizeof(struct Bheap_node*) * (size + 1));
if (NULL == heap->head)
{
fprintf(stderr, "Bheap.head malloc error!\n");
return (NULL);
}
heap->max_size = size;
}
else
{
heap->head = (struct Bheap_node**)malloc(sizeof(struct Bheap_node*) * ((DEF_SIZE) + 1));
if (NULL == heap->head)
{
fprintf(stderr, "Bheap.head malloc error!\n");
return (NULL);
}
heap->max_size = DEF_SIZE;
}
heap->Btype = Btype;
return (heap);
}
/* 初始化二叉堆 */
static void Bheap_init(struct Bheap* heap)
{
if (NULL == heap)
return;
memset(heap->head, 0x00, (sizeof(struct Bheap_node*) * (heap->size + 1)));
heap->size = 0;
}
/* 增长二叉堆容量 */
static size_t __inc_Bheap_size(struct Bheap* heap)
{
if (NULL == heap)
return (-1);
heap->max_size = (size_t)((INC_RATE) * heap->max_size);
heap->head = (struct Bheap_node**)realloc(heap->head, sizeof(struct Bheap_node*) * (heap->max_size + 1));
if (NULL == heap->head)
return (-1);
return (0);
}
/* 交换节点内容 */
static void __swap_Bheap_node(struct Bheap_node** n1, struct Bheap_node** n2)
{
struct Bheap_node* temp;
if (*n1 == *n2)
return;
temp = *n1;
*n1 = *n2;
*n2 = temp;
}
/* 入堆操作 */
static int Bheap_push(struct Bheap* heap, struct Bheap_node* node, Bheap_compare_t comp)
{
int n = 0, c;
if ((NULL == heap) || (NULL == node) || (NULL == comp))
return (-1);
if ((heap->size == heap->max_size) && (-1 == __inc_Bheap_size(heap)))
{
fprintf(stderr, "increase heap size error!\n");
return (-1);
}
heap->size++;
n = heap->size;
heap->head[heap->size] = node;
/* 冗余代码,减少循环中的判断 */
if (BHEAP_TYPE_SMALL == heap->Btype)
{
/* 小根堆 */
for ( ; ; )
{
if (1 == n)
break;
c = n;
n = n>>1;
if (1 == comp(heap->head[n], heap->head[c]))
__swap_Bheap_node(&(heap->head[n]), &(heap->head[c]));
else
break;
}
}
else
{
/* 大根堆 */
for ( ; ; )
{
if (1 == n)
break;
c = n;
n = n>>1;
if (-1 == comp(heap->head[n], heap->head[c]))
__swap_Bheap_node(&(heap->head[n]), &(heap->head[c]));
else
break;
}
}
return (0);
}
/* 移除节点 */
static struct Bheap_node* Bheap_remove(struct Bheap* heap, size_t idx, Bheap_compare_t comp)
{
int n = 0, c;
if ((NULL == heap) || (idx < 1) || (idx > heap->size) || (NULL == comp))
return (NULL);
__swap_Bheap_node(&(heap->head[idx]), &(heap->head[heap->size]));
n = idx;
heap->size--;
/* 冗余代码,减少循环中的判断 */
if (BHEAP_TYPE_SMALL == heap->Btype)
{
/* 小根堆 */
for ( ; ; )
{
c = n;
n = n<<1;
if (n > heap->size)
break;
if ((n + 1) > heap->size)
{
if (1 == comp(heap->head[c], heap->head[n]))
__swap_Bheap_node(&(heap->head[c]), &(heap->head[n]));
else
break;
}
else
{
if (1 == comp(heap->head[n], heap->head[n + 1]))
{
if (1 == comp(heap->head[c], heap->head[n + 1]))
{
__swap_Bheap_node(&(heap->head[c]), &(heap->head[n + 1]));
n += 1;
}
else
break;
}
else
{
if (1 == comp(heap->head[c], heap->head[n]))
__swap_Bheap_node(&(heap->head[c]), &(heap->head[n]));
else
break;
}
}
}/* end for */
}
else
{
/* 大根堆 */
for ( ; ; )
{
c = n;
n = n<<1;
if (n > heap->size)
break;
if ((n + 1) > heap->size)
{
if (-1 == comp(heap->head[c], heap->head[n]))
__swap_Bheap_node(&(heap->head[c]), &(heap->head[n]));
else
break;
}
else
{
if (-1 == comp(heap->head[n], heap->head[n + 1]))
{
if (-1 == comp(heap->head[c], heap->head[n + 1]))
{
__swap_Bheap_node(&(heap->head[c]), &(heap->head[n + 1]));
n += 1;
}
else
break;
}
else
{
if (-1 == comp(heap->head[c], heap->head[n]))
__swap_Bheap_node(&(heap->head[c]), &(heap->head[n]));
else
break;
}
}
}/* end for */
}
return (heap->head[heap->size + 1]);
}
/* 出堆操作 */
static struct Bheap_node* Bheap_pop(struct Bheap* heap, Bheap_compare_t comp)
{
if ((NULL == heap) || (NULL == comp) || (0 == heap->size))
return (NULL);
return Bheap_remove(heap, 1, comp);
}
/* 判断堆中是否包含某节点 */
static size_t is_Bheap_contain(struct Bheap* heap, struct Bheap_node* node, Bheap_equal_t eq)
{
int n = 1;
if ((NULL == heap) || (node == NULL) || (NULL == eq))
return (-1);
for ( ; n <= heap->size; n++)
{
if (1 == eq(heap->head[n], node))
return (n);
}
return (-1);
}
/* 获取某节点 */
static struct Bheap_node* Bheap_get(struct Bheap* heap, size_t idx)
{
if ((NULL == heap) || (idx < 1) || (idx > heap->max_size))
return (NULL);
return (heap->head[idx]);
}
/* 堆销毁,如果需要销毁节点，
* is_need_free_node 传入非零值(建议传1) 并且指定自己实现的Bheap_free_node_t,
* 如不需要销毁堆中节点 则is_need_free_node传入0,Bheap_free_node_t为NULL
*/
static void Bheap_destory(struct Bheap** heap, int is_need_free_value, Bheap_free_node_t free_node)
{
size_t o_idx = 1;
if ((NULL == *heap))
return;
if (is_need_free_value && (NULL != free_node))
for ( ; o_idx <= (*heap)->size; o_idx++)
{
if (NULL != ((*heap)->head[o_idx])->value)
{
if (NULL != (*heap)->head[o_idx])
{
free_node((*heap)->head[o_idx]);
(*heap)->head[o_idx] = NULL;
}
}
}
if (NULL != (*heap)->head)
{
free((*heap)->head);
(*heap)->head = NULL;
}
free(*heap);
*heap = NULL;
}
#endif /*_BHEAP_H*/
堆操作都放在bheap.h文件中，接下来是测试文件:
bheap_test.c
#include "bheap.h"
#define LENGTH(a) (sizeof(a) / sizeof(a[0]))
struct point
{
int x;
int y;
long weight;
};
struct point nodes[] = {
{10,10,10}, {3,3,3}, {2,2,2},
{5,5,5}, {4,4,4}, {6,6,6},
{7,7,7}, {8,8,8}, {9,9,9},
{10,10,10},{11,11,11}, {12,12,12}
};
static int _comp(struct Bheap_node* n1, struct Bheap_node* n2)
{
if ((NULL != n1) && (NULL != n2))
{
if (((struct point*)(n1->value))->weight > ((struct point*)(n2->value))->weight)
return (1);
else if(((struct point*)(n1->value))->weight == ((struct point*)(n2->value))->weight)
return (0);
else
return (-1);
}
else
return (0);
}
static int _eq(struct Bheap_node* n1, struct Bheap_node* n2)
{
if ((NULL != n1) && (NULL != n2))
return ((((struct point*)(n1->value))->x == ((struct point*)(n2->value))->x)
&& (((struct point*)(n1->value))->y ==((struct point*)(n2->value))->y));
else
return (0);
}
static void _free_node(struct Bheap_node* bn)
{
free(bn->value);
free(bn);
}
static void print_point(struct point* p, char c)
{
if (NULL != p)
printf("{%d,%d,%ld}%c", p->x, p->y, p->weight, c);
}
/* 打印heap内容 */
static void print_heap(struct Bheap* heap)
{
int i = 1;
for (; i <= heap->size; i++)
print_point((struct point*)((heap->head[i])->value), ' ');
printf("\n");
}
int main(int argc, char* argv[])
{
int i;
struct Bheap* heap = NULL;
struct Bheap_node* inode = NULL, *onode = NULL;
struct point* p = NULL;
heap = Bheap_create(2, BHEAP_TYPE_SMALL);
Bheap_init(heap);
for (i = 0; i < LENGTH(nodes); i++)
{
inode = (struct Bheap_node*)malloc(sizeof(struct Bheap_node));
inode->value = nodes + i;
Bheap_push(heap, inode, _comp);
#if 0
print_heap(heap);
#endif
}
for ( ; ; )
{
if (NULL == (onode = Bheap_pop(heap, _comp)))
break;
#if 1
print_point((struct point*)onode->value, '\n');
#endif
free(onode);
}
Bheap_destory(&heap, 0, NULL);
return (0);
}CC= cc
CFLAGS= -g
LDFLAGS=
TARGETS= btest
BHEAPOBJS= bheap_test.o
all: $(TARGETS)
btest: $(BHEAPOBJS)
$(CC) $(CFLAGS) -o $@ $(BHEAPOBJS) $(LDFLAGS)
.c.o:
$(CC) -c $(CFLAGS) $<
clean:
rm -f $(TARGETS) *.o
run:
./$(TARGETS)
执行结果
大根堆:
小根堆:
3.2 估算函数的选择
在静态网格中，距离的计算一般有如下几种:欧氏距离,曼哈顿距离,切比雪夫距离等。
以下是 “欧氏距离”,“曼哈顿距离”,“切比雪夫距离”的几何定义: (也可参考: [转载]机器学习中的相似性度量 (多种几何距离定义)),距离估算函数的选择需要按实际情况而定。
3.2.1 欧氏距离:
欧氏距离是最易于理解的一种距离计算方法，源自欧氏空间中两点间的距离公式。
(1) 二维平面上两点a(x1,y1)与b(x2,y2)间的欧氏距离:
(2) 三维空间两点a(x1,y1,z1)与b(x2,y2,z2)间的欧氏距离:
(3) 两个n维向量a(x11,x12,…,x1n)与 b(x21,x22,…,x2n)间的欧氏距离:
也可以用表示成向量运算的形式:
3.2.2 曼哈顿距离
从名字就可以猜出这种距离的计算方法了。想象你在曼哈顿要从一个十字路口开车到另外一个十字路口，驾驶距离是两点间的直线距离吗？显然不是，除非你能穿越大楼。实际驾驶距离就是这个“曼哈顿距离”。而这也是曼哈顿距离名称的来源， 曼哈顿距离也称为城市街区距离(City Block distance)。
(1) 二维平面两点a(x1,y1)与b(x2,y2)间的曼哈顿距离
(1) 二维平面两点a(x1,y1)与b(x2,y2)间的曼哈顿距离
(2) 两个n维向量a(x11,x12,…,x1n)与 b(x21,x22,…,x2n)间的曼哈顿距离
3.2.3 切比雪夫距离
国际象棋玩过么？国王走一步能够移动到相邻的8个方格中的任意一个。那么国王从格子(x1,y1)走到格子(x2,y2)最少需要多少步？自己走走试试。你会发现最少步数总是max( | x2-x1 | , | y2-y1 | ) 步。有一种类似的一种距离度量方法叫切比雪夫距离。
(1) 二维平面两点a(x1,y1)与b(x2,y2)间的切比雪夫距离
(1) 二维平面两点a(x1,y1)与b(x2,y2)间的切比雪夫距离
(2) 两个n维向量a(x11,x12,…,x1n)与 b(x21,x22,…,x2n)间的切比雪夫距离
这个公式的另一种等价形式是
4. A* 在最短路径寻优中的实现
以上已经对A*算法、二叉堆、几何距离进行了介绍，接下来是具体的C代码实现。(二叉堆实现代码在上面已经给出，不再列出).
Astar.h
/*
* author: Atom
* date: 2012/12/03
* file: Astar.h
*/
#ifndef ASTAR_H
#define ASTAR_H
#include
#include
#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*/
Astar.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");
}
}
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");
}
}
Makefile 文件:
CC= cc
CFLAGS= -g
LDFLAGS= -lm
TARGETS= astar
BHEAPOBJS= Astar.o
all: $(TARGETS)
astar: $(BHEAPOBJS)
$(CC) $(CFLAGS) -o $@ $(BHEAPOBJS) $(LDFLAGS)
.c.o:
$(CC) -c $(CFLAGS) $<
clean:
rm -f $(TARGETS) *.o
run:
./Astar
执行结果:
欧氏距离:
曼哈顿距离:
切比雪夫距离:
5. A*在8数码问题中的实现
类似于最短路径问题，A*同样应用于最初的8数码问题中,具体实现看阅读下一篇博文。
参考:
1. 百度百科A*算法
2. [转载]机器学习中的相似性度量 (多种几何距离定义)
3. Using Binary Heaps in A* Pathfinding
代码包下载:
astar实现代码(c)
Github地址: https://github.com/xielianghai/astar