A*算法程序

我曾看過一些有關A*算法的程序,不過寫得比較簡潔、易懂的還是風雲寫的A*算法教學實例,但是這個算法並沒有進行優化,該程序要用到實際應用中, 還會有一定的限制, 所以我對該算法進行了改進,並加上更詳細的算法說明,使其具有更好的教學作用和實用價值。開始前我先給出A*算法的基本思路:
  問題:求出2D的迷宮中起始點S到目標點E的最短路徑?
  算法:

  findpath()
  {
    把S點加入樹根(各點所在的樹的高度表示從S點到該點所走過的步數);
    把S點加入排序隊列(按該點到E點的距離排序+走過的步數從小到大排序);
    1、排序隊列sort_queue中距離最小的第一個點出列,並保存入store_queue
    2、從出列的點出發,分別向4個(或8個)方向中的一個各走出一步
    3、並估算第2步所走到位置到目標點的距離,並把該位置加入樹,最後把該點按距離從小到大排序後並放入隊列中(由trytile函數實現)
    4、如果該點從四個方向上都不能移動,則把該點從store_queue中刪除
    5、回到第一點,直到找到E點則結束

    從目標點回溯樹,直到樹根則可以找到最佳路徑,並保存在path[]
  }

  文末附帶的程序參考了風雲的最短路徑代碼,並加以改進和優化:

把原來用於存放已處理節點的堆棧改爲隊列(store_queue),這樣在從sort_queue隊列出列時可直接放入store_queue中。
解除了地圖大小的限制(如果有64K內存限制時,地圖大小隻能是180x180)。
刪除了原程序中的一些冗餘,見程序中的註釋。
程序繼續使用dis_map數組保存各點歷史歷史最佳距離,也包含了某點是否已經經過的信息,雖然這樣做可能會比使用鏈表多用一些內存,但是在搜索時可以節省不時間。
程 序更具有實用性,可直接或修改後運用於你的程序中,但請你使用該代碼後 應該返回一些信息給我,如算法的改進或使用於什麼程序等。 本程序可以用Borland C++DJGPP編譯,並附帶有一個數據文件,保存有地圖的數據。最後值得一提的是,該地圖文件格式與風雲的源代碼的地圖格式不一樣。
/*-------------------------------------------------------------------------*/

//#define NDEBUG
#include
#include
#include
#include

#define tile_num(x,y) ((y)*map_w+(x)) // x,y 座標轉換爲地圖上塊的編號
#define tile_x(n) ((n)%map_w) //
由塊編號得出 x,y 座標
#define tile_y(n) ((n)/map_w)

#define MAPMAXSIZE 180 //地圖面積最大爲 180x180,如果沒有64K內存限制可以更大
#define MAXINT 32767

//樹結構, 比較特殊, 是從葉節點向根節點反向鏈接,方便從葉節點找到根節點
typedef struct tree_node *TREE;

struct tree_node {
int h; //
節點所在的高度,表示從起始點到該節點所有的步數
int tile; //
該節點的位置
TREE father; //
該節點的上一步
};

//鏈接結構,用於保存處理過的和沒有處理過的結點
typedef struct link_node *LINK;

struct link_node {
TREE node;
int f;
LINK next;
};

LINK sort_queue; // 保存沒有處理的行走方法的節點
LINK store_queue; //
保存已經處理過的節點 (搜索完後釋放)

unsigned char * map; //地圖數據
unsigned int * dis_map; //
保存搜索路徑時,中間目標地最優解

int map_w,map_h; //地圖寬和高
int start_x,start_y,end_x,end_y; //
地點,終點座標

// 初始化隊列
void init_queue()
{
sort_queue=(LINK)malloc(sizeof(*sort_queue));
sort_queue->node=NULL;
sort_queue->f=-1;
sort_queue->next=(LINK)malloc(sizeof(*sort_queue));
sort_queue->next->node=NULL;
sort_queue->next->f=MAXINT;
sort_queue->next->next=NULL;

store_queue=(LINK)malloc(sizeof(*store_queue));
store_queue->node=NULL;
store_queue->f=-1;
store_queue->next=NULL;
}

// 待處理節點入隊列, 依靠對目的地估價距離插入排序
void enter_queue(TREE node,int f)
{
LINK p=sort_queue,father,q;
while(f>p->f) {
father=p;
p=p->next;
assert(p);
}
q=(LINK)malloc(sizeof(*q));
assert(sort_queue);
q->f=f,q->node=node,q->next=p;
father->next=q;
}

// 將離目的地估計最近的方案出隊列
TREE get_from_queue()
{
LINK bestchoice=sort_queue->next;
LINK next=sort_queue->next->next;
sort_queue->next=next;

bestchoice->next=store_queue->next;
store_queue->next=bestchoice;
return bestchoice->node;
}

// 釋放棧頂節點
void pop_stack()
{
LINK s=store_queue->next;
assert(s);
store_queue->next=store_queue->next->next;
free(s->node);
free(s);
}

// 釋放申請過的所有節點
void freetree()
{
int i;
LINK p;
while(store_queue){
p=store_queue;
free(p->node);
store_queue=store_queue->next;
free(p);
}
while (sort_queue) {
p=sort_queue;
free(p->node);
sort_queue=sort_queue->next;
free(p);
}
}

// 估價函數,估價 x,y 到目的地的距離,估計值必須保證比實際值小
int judge(int x,int y)
{
int distance;
distance=abs(end_x-x)+abs(end_y-y);
return distance;
}

// 嘗試下一步移動到 x,y 可行否
int trytile(int x,int y,TREE father)
{
TREE p=father;
int h;
if (map[tile_num(x,y)]!=' ') return 1; //
如果 (x,y) 處是障礙,失敗
//
這一步用來判斷(x,y)點是否已經加入隊列,多餘可以刪除,因爲dis_map已經
//
保存該點是否已經保存
//while (p) {
// if (x==tile_x(p->tile) && y==tile_y(p->tile)) return 1; //
如果 (x,y) 曾經經過,失敗
// p=p->father;
//}
h=father->h+1;
if (h>=dis_map[tile_num(x,y)]) return 1; //
如果曾經有更好的方案移動到 (x,y) 失敗
dis_map[tile_num(x,y)]=h; //
記錄這次到 (x,y) 的距離爲歷史最佳距離

// 將這步方案記入待處理隊列
p=(TREE)malloc(sizeof(*p));
p->father=father;
p->h=father->h+1;
p->tile=tile_num(x,y);
enter_queue(p,p->h+judge(x,y));
return 0;
}

// 路徑尋找主函數
int * findpath(void)
{
TREE root;
int i,j;
int * path;
memset(dis_map,0xff,map_h*map_w*sizeof(*dis_map)); //
填充dis_map0XFF,表示各點未曾經過
init_queue();
root=(TREE)malloc(sizeof(*root));
root->tile=tile_num(start_x,start_y);
root->h=0;
root->father=NULL;
enter_queue(root,judge(start_x,start_y));
for (;;) {
int x,y,child;
TREE p;
root=get_from_queue();
if (root==NULL) {
return NULL;
}
x=tile_x(root->tile);
y=tile_y(root->tile);
if (x==end_x && y==end_y) break; //
達到目的地成功返回

child=trytile(x,y-1,root); //嘗試向上移動
child&=trytile(x,y+1,root); //
嘗試向下移動
child&=trytile(x-1,y,root); //
嘗試向左移動
child&=trytile(x+1,y,root); //
嘗試向右移動
//child&=trytile(x+1,y-1,root);//
嘗試向右上移動
//child&=trytile(x+1,y+1,root); //
嘗試向右下移動
//child&=trytile(x-1,y+1,root); //
嘗試向左下移動
//child&=trytile(x-1,y-1,root); //
嘗試向左上移動

if (child!=0)
pop_stack(); //
如果四個方向均不能移動,釋放這個死節點
}

// 回溯樹,將求出的最佳路徑保存在 path[]
path=(int*)malloc((root->h+2)*sizeof(int));
assert(path);
for (i=0;root;i++) {
path[i]=root->tile;
root=root->father;
}
path[i]=-1;
freetree();
return path;
}

void printpath(int *path)
{
int i;
if(path==NULL) return ;
for (i=0;path[i]>=0;i++) {
gotoxy(tile_x(path[i])+1,tile_y(path[i])+1);
cprintf(".");
}
}

int readmap()
{
FILE *f;
int i,j;
f=fopen("map.dat","r");
assert(f);
fscanf(f,"%d,%d/n",&map_w,&map_h);
map=malloc(map_w*map_h+1);
assert(map);
for(i=0;i fgets(map+tile_num(0,i),map_w+2,f);
fclose(f);
start_x=-1,end_x=-1;
for (i=0;i for (j=0;j if (map[tile_num(j,i)]=='s') map[tile_num(j,i)]=' ',start_x=j,start_y=i;
if (map[tile_num(j,i)]=='e') map[tile_num(j,i)]=' ',end_x=j,end_y=i;
}
assert(start_x>=0 && end_x>=0);
dis_map=malloc(map_w*map_h*sizeof(*dis_map));
assert(dis_map);
return 0;
}

void showmap()
{
int i,j;
clrscr();
for (i=0;i gotoxy(1,i+1);
for (j=0;j if (map[tile_num(j,i)]!=' ') cprintf("O");
else cprintf(" ");
}
gotoxy(start_x+1,start_y+1);
cprintf("s");
gotoxy(end_x+1,end_y+1);
cprintf("e");
}

int main()
{
int * path;
readmap();
showmap();
getch();
path=findpath();
printpath(path);
if(dis_map) free(dis_map);
if(path) free(path);
if(map) free(map);
getch();
return 0;
}

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章