A星搜索(A* search, A Star search algorithm)算法实现代码

为了对A*算法的印象比较深刻一点，这几天抽了空参照前面转载的那篇介绍A*算法的文章实现了一个简易的A*算法，完全用纯C语言实现。代码贴在这里供以后参考。如果用来解决其他问题，可以移植过去，重新实现几个函数就行了，主要是扩展节点的函数。

 

注：如果想使用已有的比较好使的A*算法，推荐一个以前我使用过的C++写的A*程序，参见这里(http://code.google.com/p/a-star-algorithm-implementation/)。这个代码里有几个例子程序，可以参照一下，也有详细的readme，参照说明重新实现一下Node就行了。

 

/* * file: astar_algorithm.h * author: MulinB@HUST * date: 2010-10-10 * modified: 2012-05-09 * A-star algorithm implemented in C. Only for study. */ #ifndef _ASTAR_ALGORITHM_H #define _ASTAR_ALGORITHM_H #include <math.h> #define M 6 #define N 8 //map marks #define AVAIL 0 #define UNAVAIL -1 #define START 100 #define END 111 #define ROAD 10 #define GET_F(X) (X->G + X->H) typedef struct Node { //for node itself int type; //node type int i; //i index int j; //j index //for A star algorithm double G; //past road cost double H; //heuristic, F = G + H struct Node* parent; //parent node, used for trace road struct Node* next; //only used for open and close list }Node; //==========================open close list operation================ Node* open_list; Node* close_list; void init_openlist() { open_list = NULL; } void init_closelist() { close_list = NULL; } void destroy_openlist() { Node* q; Node* p = open_list; while (p != NULL) { q = p->next; free(p); p = q; } } void destroy_closelist() { Node* q; Node* p = close_list; while (p != NULL) { q = p->next; free(p); p = q; } } void insert_into_openlist(Node* new_node) //insert and sort by F { Node* p; Node* q; if (open_list == NULL) { open_list = new_node; //insert as the first return; } p = open_list; while (p != NULL) { q = p; p = p->next; if (p == NULL) { q->next = new_node; //insert as the last return; } else if (GET_F(new_node) < GET_F(p)) { q->next = new_node; //insert before p, sorted new_node->next = p; return; } } } void insert_into_closelist(Node* new_node) //just insert before head { if (close_list == NULL) { close_list = new_node; //insert as the first return; } else { new_node->next = close_list; //insert before head close_list = new_node; return; } } Node* find_node_in_list_by_ij(Node* node_list, int di, int dj) { Node* p = node_list; while (p) { if (p->i == di && p->j == dj) return p; p = p->next; } return NULL; } Node* pop_firstnode_from_openlist() //get the minimum node sorted by F { Node* p = open_list; if (p == NULL) { return NULL; } else { open_list = p->next; p->next = NULL; return p; } } void remove_node_from_openlist(Node* nd) //just remove it, do not destroy it { Node* q; Node* p = open_list; if (open_list == nd) { open_list = open_list->next; return; } while (p) { q = p; p = p->next; if (p == nd) //found { q->next = p->next; p->next = NULL; return; } } } void remove_node_from_closelist(Node* nd) //just remove it, do not destroy it { Node* q; Node* p = close_list; if (close_list == nd) { close_list = close_list->next; return; } while (p) { q = p; p = p->next; if (p == nd) //found { q->next = p->next; p->next = NULL; return; } } } //=================================================================== //=======================calculate H, G ============================= //calculate Heuristic value //(reimplemented when porting a star to another application) double calc_H(int cur_i, int cur_j, int end_i, int end_j) { return (abs(end_j - cur_j) + abs(end_i - cur_i)) * 10.0; //the heuristic } //calculate G value //(reimplemented when porting a star to another application) double calc_G(Node* cur_node) { Node* p = cur_node->parent; if (abs(p->i - cur_node->i) + abs(p->j - cur_node->j) > 1) return 14.0 + p->G; //the diagonal cost is 14 else return 10.0 + p->G; //the adjacent cost is 10 } void init_start_node(Node* st, int si, int sj, int ei, int ej) { memset(st, 0, sizeof(Node)); st->type = START; st->i = si; st->j = sj; st->H = calc_H(si, sj, ei, ej); st->G = 0; } void init_end_node(Node* ed, int ei, int ej) { memset(ed, 0, sizeof(Node)); ed->type = END; ed->i = ei; ed->j = ej; ed->H = 0; ed->G = 9999; //temp value } void init_pass_node(Node* pd, int pi, int pj) { memset(pd, 0, sizeof(Node)); pd->type = AVAIL; pd->i = pi; pd->j = pj; } //check the candidate node (i,j) when extending parent_node int check_neighbor(int map[][N], int width, int height, int di, int dj, Node* parent_node, Node* end_node) { Node* p; Node* temp; double new_G; if (di < 0 || dj < 0 || di > height-1 || dj > width-1) return UNAVAIL; //1. check available if (map[di][dj] == UNAVAIL) return UNAVAIL; //2. check if existed in close list p = find_node_in_list_by_ij(close_list, di, dj); if (p != NULL) { //found in the closed list, check if the new G is better, added 2012-05-09 temp = p->parent; p->parent = parent_node; new_G = calc_G(p); if (new_G >= p->G) { p->parent = temp; //if new_G is worse, recover the parent } else { //the new_G is better, remove it from close list, insert it into open list p->G = new_G; remove_node_from_closelist(p); //remove it insert_into_openlist(p); //insert it, sorted } return AVAIL; } //3. check if existed in open list p = find_node_in_list_by_ij(open_list, di, dj); //in open list if (p != NULL) { //found in the open list, check if the new G is better temp = p->parent; p->parent = parent_node; new_G = calc_G(p); if (new_G >= p->G) { p->parent = temp; //if new_G is worse, recover the parent } else { //the new_G is better, resort the list p->G = new_G; remove_node_from_openlist(p); //remove it insert_into_openlist(p); //insert it, sorted } return AVAIL; } //4. none of above, insert a new node into open list if (map[di][dj] == END) { //4~. check if it is end node end_node->parent = parent_node; end_node->G = calc_G(end_node); insert_into_openlist(end_node); //insert into openlist return AVAIL; } else { //4~~. create a new node p = malloc(sizeof(Node)); init_pass_node(p, di, dj); p->parent = parent_node; p->H = calc_H(di, dj, end_node->i, end_node->j); p->G = calc_G(p); insert_into_openlist(p); //insert into openlist return AVAIL; } } //extend the current node on the map //(reimplemented when porting a star to another application) void extend_node(Node* cd, int map[][N], int width, int height, Node* end_node) { int up_status, down_status, left_status, right_status; int ci, cj; //cur node i, j int ti, tj; //temp i, j ci = cd->i; cj = cd->j; //1. up ti = ci - 1; tj = cj; up_status = check_neighbor(map, width, height, ti, tj, cd, end_node); //2. down ti = ci + 1; tj = cj; down_status = check_neighbor(map, width, height, ti, tj, cd, end_node); //3. left ti = ci; tj = cj - 1; left_status = check_neighbor(map, width, height, ti, tj, cd, end_node); //4. right ti = ci; tj = cj + 1; right_status = check_neighbor(map, width, height, ti, tj, cd, end_node); //5. leftup ti = ci - 1; tj = cj - 1; if (up_status == AVAIL && left_status == AVAIL) check_neighbor(map, width, height, ti, tj, cd, end_node); //6. rightup ti = ci - 1; tj = cj + 1; if (up_status == AVAIL && right_status == AVAIL) check_neighbor(map, width, height, ti, tj, cd, end_node); //7. leftdown ti = ci + 1; tj = cj - 1; if (down_status == AVAIL && left_status == AVAIL) check_neighbor(map, width, height, ti, tj, cd, end_node); //8. rightdown ti = ci + 1; tj = cj + 1; if (down_status == AVAIL && right_status == AVAIL) check_neighbor(map, width, height, ti, tj, cd, end_node); } //=======================search algorithm====================================== /* A*方法总结 (from http://www.policyalmanac.org/games/aStarTutorial.htm): 1. 把起始格添加到开启列表。 2. 重复如下的工作： a) 寻找开启列表中F值最低的格子。我们称它为当前格。 b) 把它切换到关闭列表。 c) 对相邻的8格中的每一个？ * 如果它不可通过或者已经在关闭列表中，略过它。反之如下。(MulinB 2012-05-09 按：在关闭列表中是否也应该检查它，看是否可以获得更低的G值?? ref: http://theory.stanford.edu/~amitp/GameProgramming/ImplementationNotes.html ) * 如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。 * 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。 如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。 如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。 d) 停止，当你 * 把目标格添加进了关闭列表(注解)，这时候路径被找到，或者 * 没有找到目标格，开启列表已经空了。这时候，路径不存在。 3. 保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是你的路径。 */ //search a road on a map, return node_list Node* a_star_search(int map[M][N], int width, int height, int start_i, int start_j, int end_i, int end_j) { Node* cur_node; Node* start_node; Node* end_node; //create start and end node start_node = malloc(sizeof(Node)); init_start_node(start_node, start_i, start_j, end_i, end_j); end_node = malloc(sizeof(Node)); init_end_node(end_node, end_i, end_j); //init open and close list init_openlist(); init_closelist(); //put start_node into open list insert_into_openlist(start_node); //start searching while (1) { cur_node = pop_firstnode_from_openlist(); //it has the minimum F value if (cur_node == NULL || cur_node->type == END) { break; //found the road or no road found } extend_node(cur_node, map, width, height, end_node); //the key step!! insert_into_closelist(cur_node); } //you can track the road by the node->parent return cur_node; } #endif /* file end */ 

 

/* * file: test_astar.c * author: MulinB@HUST * date: 2010-10-10 * Test the A-star algorithm. Only for study. */ #include <stdio.h> #include "astar_algorithm.h" int map[M][N]; //map for test int main() { int i, j; Node* node_list; Node* p; printf("Test A Star Algorithm! by MulinB@HUST, 2010-10-10/n"); //setup passable place for (i=0; i<M; i++) { for (j=0; j<N; j++) { map[i][j] = AVAIL; } } //setup obstacle for (i=1; i<4; i++) { map[i][4] = UNAVAIL; } //setup start and end point map[2][2] = START; map[2][6] = END; //print map printf("-------------------map------------------/n"); for (i=0; i<M; i++) { for (j=0; j<N; j++) { printf("%3d ", map[i][j]); } printf("/n"); } printf("----------------------------------------/n"); //search node_list = a_star_search(map, N, M, 2, 2, 2, 6); if (node_list == NULL) { printf("No road found!/n"); } else { printf("The cost of this road is %f/n", node_list->G); printf("The road list is: /n"); p = node_list; while (p) { printf(" (%d, %d)/n", p->i, p->j); map[p->i][p->j] = ROAD; p = p->parent; //track the road by the node->parent } //print result printf("-------------------result---------------/n"); for (i=0; i<M; i++) { for (j=0; j<N; j++) { printf("%3d ", map[i][j]); } printf("/n"); } printf("----------------------------------------/n"); } //clear the open and close list destroy_openlist(); destroy_closelist(); return 0; } 

 

运行结果：

Test A Star Algorithm! by MulinB@HUST, 2010-10-10 -------------------map------------------ 0 0 0 0 0 0 0 0 0 0 0 0 -1 0 0 0 0 0 100 0 -1 0 111 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ---------------------------------------- The cost of this road is 68.000000 The road list is: (2, 6) (1, 6) (0, 5) (0, 4) (0, 3) (1, 2) (2, 2) -------------------result--------------- 0 0 0 10 10 10 0 0 0 0 10 0 -1 0 10 0 0 0 10 0 -1 0 10 0 0 0 0 0 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ----------------------------------------