c语言a*算法,求A* 算法C语言源程序？

拉丁的传说

#include #include #define AVAIL 0#define UNAVAIL 1#define START 8#define END 9#define ROAD 4#define WEIDTH 5#define HEIGTH 10#define GET_F(X) (X->G + X->H)using namespace std;int map[WEIDTH][HEIGTH]={0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,1,8,0,1,0,0,9,1,0,0,1,0,0,1,0,1,1,1,0,0,0,0,1,0,0,0,0,0,0};typedef struct Node{int type;int x,y;int G,H;struct Node *parent;struct Node *next;}Node;Node* open_list;Node* close_list;Node* a_star_search(int map[WEIDTH][HEIGTH],int weidth,int heigth,int start_x,int start_y,int end_x,int end_y);void init_openlist();void init_closelist();void destroy_openlist();void destroy_closelist();void insert_into_openlist(Node* new_node); //insert and sort by Fvoid insert_into_closelist(Node* new_node); //just insert before headNode* find_node_in_list_by_ij(Node* node_list, int di, int dj);Node* pop_firstnode_from_openlist(); //get the minimum node sorted by Fvoid remove_node_from_openlist(Node* nd); //just remove it, do not destroy itvoid remove_node_from_closelist(Node* nd); //just remove it, do not destroy itdouble calc_H(int cur_i, int cur_j, int end_i, int end_j);double calc_G(Node* cur_node);void init_start_node(Node* st, int si, int sj, int ei, int ej);void init_end_node(Node* ed, int ei, int ej);void init_pass_node(Node* pd, int pi, int pj);int check_neighbor(int map[WEIDTH][HEIGTH], int width, int height,int di, int dj, Node* parent_node, Node* end_node);void extend_node(Node* cd, int map[WEIDTH][HEIGTH], int width, int height, Node* end_node);int main(){Node *node_list;Node *p;node_list = a_star_search(map,WEIDTH,HEIGTH,2,2,2,8);if(node_list == NULL)cout<G<parent;while(p->next){map[p->x][p->y]=ROAD;p=p->parent;}for(int i=0;inext;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 firstreturn;}p = open_list;while (p != NULL){q = p;p = p->next;if (p == NULL){q->next = new_node; //insert as the lastreturn;}else if (GET_F(new_node) < GET_F(p)){q->next = new_node; //insert before p, sortednew_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 firstreturn;}else{new_node->next = close_list; //insert before headclose_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->x == di && p->y == 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;}}}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}double calc_G(Node* cur_node){Node* p = cur_node->parent;if (abs(p->x - cur_node->x) + abs(p->y - cur_node->y) > 1)return 14.0 + p->G; //the diagonal cost is 14elsereturn 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->x = si;st->y = 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->x = ei;ed->y = 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->x = pi;pd->y= pj;}int check_neighbor(int map[WEIDTH][HEIGTH], 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 > width-1 || dj > height-1)return UNAVAIL;//1. check availableif (map[di][dj] == UNAVAIL)return UNAVAIL;//2. check if existed in close listp = 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-09temp = 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 listp->G = new_G;remove_node_from_closelist(p); //remove itinsert_into_openlist(p); //insert it, sorted}return AVAIL;}//3. check if existed in open listp = find_node_in_list_by_ij(open_list, di, dj); //in open listif (p != NULL){//found in the open list, check if the new G is bettertemp = 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 listp->G = new_G;remove_node_from_openlist(p); //remove itinsert_into_openlist(p); //insert it, sorted}return AVAIL;}//4. none of above, insert a new node into open listif (map[di][dj] == END){//4~. check if it is end nodeend_node->parent = parent_node;end_node->G = calc_G(end_node);insert_into_openlist(end_node); //insert into openlistreturn AVAIL;}else{//4~~. create a new nodep = new Node();init_pass_node(p, di, dj);p->parent = parent_node;p->H = calc_H(di, dj, end_node->x, end_node->y);p->G = calc_G(p);insert_into_openlist(p); //insert into openlistreturn AVAIL;}}void extend_node(Node* cd, int map[WEIDTH][HEIGTH], int width, int height, Node* end_node){int cx, cy; //cur node i, jint tx, ty; //temp i, jcx = cd->x;cy = cd->y;//1. uptx = cx - 1;ty = cy;check_neighbor(map, width, height, tx, ty, cd, end_node);tx = cx + 1;ty = cy;check_neighbor(map, width, height, tx, ty, cd, end_node);//3. lefttx = cx;ty = cy - 1;check_neighbor(map, width, height, tx, ty, cd, end_node);//4. righttx = cx;ty = cy + 1;check_neighbor(map, width, height, tx, ty, cd, end_node);}Node* a_star_search(int map[WEIDTH][HEIGTH],int weidth,int heigth,int start_x,int start_y,int end_x,int end_y){Node* cur_node;Node* start_node;Node* end_node;start_node = new Node();end_node =new Node();init_start_node(start_node,start_x,start_y,end_x,end_y);init_end_node(end_node,end_x,end_y);init_openlist();init_closelist();insert_into_openlist(start_node);while (1){cur_node = pop_firstnode_from_openlist(); //it has the minimum F valueif (cur_node == NULL || cur_node->type == END){break; //found the road or no road found}extend_node(cur_node, map, WEIDTH, HEIGTH, end_node); //the key step!!insert_into_closelist(cur_node);}//you can track the road by the node->parentreturn cur_node;}