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 F

void insert_into_closelist(Node* new_node); //just insert before head

Node* find_node_in_list_by_ij(Node* node_list, int di, int dj);

Node* pop_firstnode_from_openlist(); //get the minimum node sorted by F

void remove_node_from_openlist(Node* nd); //just remove it, do not destroy it

void remove_node_from_closelist(Node* nd); //just remove it, do not destroy it

double 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<

else

{

cout<G<

cout<

p=node_list->parent;

while(p->next)

{

map[p->x][p->y]=ROAD;

p=p->parent;

}

for(int i=0;i

{

for(int j=0;j

cout<

cout<

}

cout<

}

return 0;

}

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->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 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->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 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 = 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 openlist

return AVAIL;

}

}

void extend_node(Node* cd, int map[WEIDTH][HEIGTH], int width, int height, Node* end_node)

{

int cx, cy; //cur node i, j

int tx, ty; //temp i, j

cx = cd->x;

cy = cd->y;

//1. up

tx = 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. left

tx = cx;

ty = cy - 1;

check_neighbor(map, width, height, tx, ty, cd, end_node);

//4. right

tx = 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 value

if (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->parent

return cur_node;

}