Question

求A* 算法C语言源程序

Answer 1

#include <string.h>
#include <stdio.h>
#include <malloc.h>
#include <time.h>
#define NULL 0

const int nmax = 200;
const int nend = 99; /*终点坐标的代表点*/
static char achar[10][10];
static int npayo = 0; /*0 表示空 1为非空*/
static int npayc = 0; /*0 表示空 1为非空*/
static int npay_x = 0; /*起点*/
static int npay_y = 0;
static int nend_x = 9; /*终点*/
static int nend_y = 9;
static int nnewpay_x;
static int nnewpay_y;
static int ndian = 101;
static int nh;
static long i = 10000000L;

struct Spaydian
{
int ng;
int nf; /*路径评分*/
int nmy_x; /*自己位置*/
int nmy_y;
int nfatherx; /*父节点*/
int nfathery;
int nflag; /* 0 wei O; 1 wei @ */
};
static struct Spaydian spaydian[200];

/* open close list 表 */
typedef struct spaylist
{
int n_f;
int n_x;
int n_y;
int nfather_x;
int nfather_y;
struct spaylist *next;
};
static struct spaylist *open_list, *close_list;

static void smline();
static int sjudge(int nx,int ny,int i); /*判断在第nx列ny行向第i个方向走是否可以,可以返回1否则返回0。
i=1表示向右，2表示向下，3表示向左，4表示向上*/
static void spath();
static void spayshow(int nxx,int nyy);
static int sifopen( int nx,int ny); /*判断点是否在 open 表上*/
static int sifclose(int nx,int ny); /*判断点是否在 close 表上*/
static int snewx(int nx,int i);
static int snewy(int ny,int i);
static spaylist *creat(); /*建立链表*/
static spaylist *del(spaylist *head,int num_x,int num_y); /*删除链表的结点*/
static spaylist *snewdianx(spaylist *head);/*新的点*/
static spaylist *snewdiany(spaylist *head);
static spaylist *insert(spaylist *head,int ndian); /* 点插入链表 */
static spaylist *srebirth(spaylist *head,int ndian); /*更新表*/

int main()
{
FILE *fp ;
char ach ;
int ni = 0 ; /*统计个数*/
int nii = 0; /*achar[nii][njj]*/
int njj = 0;
if ((fp=fopen("route.txt","rt")) == NULL) /* 判断打开文件是否为空 */
{
printf("文件为空！~\n");
return 0;
/* exit(1);*/
}
ach = fgetc(fp);
while(ach != EOF)
{
if(ach == 'O' || ach == '@') /*当值为@或O时候*/
{
spaydian[ni].ng = 0;
spaydian[ni].nf = nmax;
spaydian[ni].nmy_x = njj;
spaydian[ni].nmy_y = nii;
spaydian[ni].nfathery = -1;
spaydian[ni].nfatherx = -1;
if(ach == '@')
{
spaydian[ni].nflag = 1;
}
else if(ach == 'O')
{
spaydian[ni].nflag = 0;
}
ni++;
achar[nii][njj] = ach;
njj++;
if(njj == 10)
{
nii++;
njj = 0;
}
} /*end if*/
ach = fgetc(fp);
}/*end while*/
smline(); /* a*算法 */
fp=fopen("answer.txt","w");
for(int i=0;i<10;i++ )
{ for(int j=0;j<10;j++ )
{
printf("%c",achar[i][j]);
if(j==9)
printf("\n");
fprintf(fp,"%c",achar[i][j]);
if (j==9)
fprintf(fp,"\n");
}
}
fclose(fp);
return 0;
}

/* a* 算法 */
static void smline()
{ close_list = open_list = NULL;
open_list = creat();
while(open_list != NULL) /* 当open 表不为空时 */
{
open_list = del(open_list,npay_x,npay_y); /*删除open 链表的结点*/
if(npay_x == 9 && npay_y == 9)
{

achar[9][9] = '=';
spath(); /*寻找并画出路径*/
break;
}
for (int i=1; i<=4; i++) /*四个方向逐个行走，i=1向右 2向下 3向左 4向上*/
{
if (sjudge(npay_x,npay_y,i))
{

nnewpay_x = snewx(npay_x,i);
nnewpay_y = snewy(npay_y,i);
if(open_list != NULL)
npayo = sifopen(nnewpay_x,nnewpay_y) ; /*判断点是否在 open 表中*/
else
npayo = 0;

if(close_list != NULL)
npayc = sifclose(nnewpay_x,nnewpay_y) ; /*判断点是否在 close 表中*/
else
npayc = 0;
ndian = 10*nnewpay_x+nnewpay_y ;

if (npayo == 0 && npayc == 0 ) /*点不在open表也不在close表中*/
{
spaydian[ndian].ng = spaydian[10*npay_x+npay_y].ng + 1; /*更新点的基本信息*/
nh = (nend - ndian)/10 + (nend - ndian)%10 ;
spaydian[ndian].nf = spaydian[ndian].ng+nh;
spaydian[ndian].nfathery = npay_y;
spaydian[ndian].nfatherx = npay_x;
spaydian[ndian].nmy_y = nnewpay_y;
spaydian[ndian].nmy_x = nnewpay_x;

open_list = insert(open_list,ndian);/*点插入open 表中*/
}
else if (npayo == 1) /*点在open表中*/
{
spaydian[ndian].ng = spaydian[10*npay_x+npay_y].ng + 1;
nh = (nend - ndian)/10 + (nend - ndian)%10 ;
if(spaydian[ndian].nf > (spaydian[ndian].ng+nh) && spaydian[ndian].nf != nmax)
{
spaydian[ndian].nf = spaydian[ndian].ng+nh;
open_list = srebirth(open_list,ndian); /*点插入open 表中*/
}
}
else if(npayc == 1) /*新生成的点在close表中*/
{
spaydian[ndian].ng = spaydian[10*npay_x+npay_y].ng + 1;
nh = (nend - ndian)/10 + (nend - ndian)%10 ;
if(spaydian[ndian].nf > (spaydian[ndian].ng+nh) && spaydian[ndian].nf != nmax)
{
spaydian[ndian].nf = spaydian[ndian].ng+nh;
close_list = srebirth(close_list,ndian);
close_list = del(close_list,nnewpay_x,nnewpay_y); /*删除close链表的结点*/
open_list = insert(open_list,ndian);/*点插入open 表中*/
}
}/*end else if*/
}/*end if*/
}/*end for*/
close_list = insert(close_list,(10*npay_x+npay_y));/*点插入close 表中*/
if(open_list != NULL)
{
npay_x = open_list->n_x;
npay_y = open_list->n_y;
}

}/*end while*/
if(open_list == NULL)
{printf("无路可走 \n");}
}

/*建立链表*/
spaylist *creat(void)
{
spaylist *head;
spaylist *p1;
int n=0;
p1=(spaylist*)malloc(sizeof(spaylist));
p1->n_f = 18;
p1->n_x = 0;
p1->n_y = 0;
p1->nfather_x = -1;
p1->nfather_x = -1;
p1->next = NULL;
head = NULL;
head=p1;
return(head);
}

/*删除结点*/
spaylist *del(spaylist *head,int num_x,int num_y)
{
spaylist *p1, *p2;
if(head == NULL)
{
printf("\nlist null!\n");
return (head);
}
p1 = head;
while((num_y != p1->n_y ||num_x != p1->n_x )&& p1->next != NULL)
{
p2=p1;
p1=p1->next;
}
if(num_x == p1->n_x && num_y == p1->n_y )
{
if(p1==head)
head=p1->next;
else
p2->next=p1->next;
}

return (head);
}

/*输出*/
static void spath()
{
int nxx;
int nyy;
nxx = spaydian[nend].nfatherx;
nyy = spaydian[nend].nfathery;

spayshow(nxx,nyy) ;
}

/*递归*/
void spayshow(int nxx,int nyy)
{ achar[nxx][nyy] = '=';
if( nxx != 0 || nyy != 0 )
{
int nxxyy = 10*nxx+nyy;
nxx = spaydian[nxxyy].nfatherx;
nyy = spaydian[nxxyy].nfathery;
spayshow(nxx,nyy);
}
}

/* 判断周围四个点是否可行 */
static int sjudge(int nx,int ny,int i)
{
if (i==1) /*判断向右可否行走*/
{
if (achar[nx][ny+1]=='O' && ny<9)
{
return 1;
}
else
return 0;
}
else if (i==2) /*判断向下可否行走*/
{
if (achar[nx+1][ny]=='O' && nx<9)
{
return 1;
}
else
return 0;
}
else if (i==3)/*判断向左可否行走 */
{
if (ny > 0&&achar[nx][ny-1]=='O')
{
return 1;
}
else
return 0;
}
else if (i==4)/*判断向上可否行走 */
{
if (nx > 0&&achar[nx-1][ny]=='O')
{
return 1;
}
else
return 0;
}
else
return 0;
}

/* 求新的x点 */
static int snewx(int nx,int i)
{
if(i == 1)
nx = nx;
else if(i == 2)
nx = nx+1;
else if(i == 3)
nx = nx;
else if(i == 4)
nx = nx-1;
return nx;
}
/* 求新的y点 */
static int snewy(int ny, int i)
{
if(i == 1)
ny = ny+1;
else if(i == 2)
ny = ny;
else if(i == 3)
ny = ny-1;
else if(i == 4)
ny = ny;
return ny;
}

/*判定点是否在open表中*/
int sifopen(int nx,int ny)
{
spaylist *p1, *p2;
p1 = open_list;
while((ny != p1->n_y || nx != p1->n_x )&& p1->next != NULL)
{
p2 = p1;
p1 = p1->next;
}
if(nx == p1->n_x && ny == p1->n_y)
return 1;
else
return 0;
}

/*判定点是否在close表中*/
int sifclose(int nx,int ny)
{

spaylist *p1, *p2;
p1 = close_list;
while((ny != p1->n_y ||nx != p1->n_x )&& p1->next != NULL)
{
p2=p1;
p1=p1->next;
}
if(nx == p1->n_x && ny == p1->n_y)
return 1;
else
return 0;
}

/*插入结点*/
spaylist * insert(spaylist *head,int ndian)
{
spaylist *p0,*p1,*p2;
p1=head;
p0=(spaylist*)malloc(sizeof(spaylist));
p0->n_f = spaydian[ndian].nf;
p0->n_x = spaydian[ndian].nmy_x;
p0->n_y = spaydian[ndian].nmy_y;
p0->nfather_x = spaydian[ndian].nfatherx;
p0->nfather_x = spaydian[ndian].nfathery;
p0->next = NULL;
if(head==NULL)
{
head=p0;
p0->next=NULL;
}
else
{
while((p0->n_f > p1->n_f)&&(p1->next!=NULL))
{
p2=p1;
p1=p1->next;
}
if(p0->n_f <= p1->n_f)
{
if(head==p1)
head=p0;
else
p2->next=p0;
p0->next=p1;
}
else
{
p1->next=p0;
p0->next=NULL;
}
}
return (head);
}

/* 更新链表 */
spaylist * srebirth(spaylist *head,int ndian)
{
spaylist *p1, *p2;
p1=head;
while(spaydian[ndian].nmy_x!=p1->n_x&&spaydian[ndian].nmy_x!=p1->n_x&&p1->next!=NULL)
{
p2=p1;
p1=p1->next;
}
if(spaydian[ndian].nmy_x==p1->n_x&&spaydian[ndian].nmy_x==p1->n_x)
{
p1->n_f = spaydian[ndian].nf;
}
return (head);
}

Answer 2

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

Answer 3

呀 保留下 route.txt要写什么内容