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

Question

求A* 算法C语言源程序

Answer 1

#include <iostream>
#include <cstring>
#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<<"No road found!"<<endl;

else
{
cout<<"The cost of this road is "<<node_list->G<<endl;
cout<<"The road is "<<endl;
p=node_list->parent;
while(p->next)
{
map[p->x][p->y]=ROAD;
p=p->parent;
}

for(int i=0;i<WEIDTH;i++)
{
for(int j=0;j<HEIGTH;j++)
cout<<map[i][j]<<" ";
cout<<endl;
}

cout<<endl;
}

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;
}