A*尋徑演算法

作者：yuki

原帖及討論：http://www.bc-cn.net/bbs/dispbbs.asp?BoardID=5&ID=82148
貼個小東西，也許是許多遊戲開發愛好者都想要獲得演算法。

下面我來說說我理解的A*演算法的原理：
A*演算法是一個求最短路徑的函數，為許多即時戰略遊戲所用刀（或許人家大型的即時戰略遊戲筆者演算法更好，不管它）。它由兩個函數組成，一個是評估函數，也就是確定人物移動的下一個位置必須離目標位置最近，評估函數評估的結果越精確，則尋徑的速度越快；另一個就是尋徑函數，也就根據評估的結果做出回應，然後從新位置繼續評估下一個位置，若無路可走（四周都是障礙什麼的），那麼折回一個路徑節點，嘗試其他方向，這個演算法有個缺點，隨著遊戲中人物增多，相應的處理節點就增多了，會影響處理速度，而且佔用大量的記憶體。

有興趣的朋友可以改成動態的尋徑，就是當入口和出口位置都在變化的時候進行尋徑，這個代碼也只有200多行。
我的演算法還不能算是最優的，因為評估函數只不過是簡單的測試兩點距離（這會帶來誤差），選擇離出口最短的且非障礙物的方向，進行下一個路徑節點的移動。

這裏說一句，我希望大家將我的代碼用於學習目的，不希望看見是為了交作業而拷貝過去，我會很傷心的。

/* AStar.cpp */
/* 設計者： yuki */
typedef unsigned char byte_t;
typedef unsigned int uint_t;
/* 路徑節點 */
typedef struct footprint {

/* 存放在陣列中的位置 */
uint_t pos;
/* 存放方向信號量 */
byte_t direct;
struct footprint *next;
struct footprint *prev;
} path_t;

/*
方向信號量查詢表
0x01(0000 0001) : 上
0x02(0000 0010) : 下
0x04(0000 0100) : 左
0x08(0000 1000) : 右
*/
static byte_t d_signal[4] = {0x01, 0x02, 0x04, 0x08};
/*
方向信號量使用表
如果指定方向已經走過，那麼使用“與”運算去處該方向
0x0E(0000 1110) : 上
0x0D(0000 1101) : 下
0x0B(0000 1011) : 左
0x07(0000 0111) : 右
*/
static byte_t d_spend[4] = {0x0E, 0x0D, 0x0B, 0x07};
/* 指定方向移動偏量 */
static int move[4][2] = { {0, -1}, {0, 1}, {-1, 0}, {1, 0} };
/* 列印迷宮用的符號 */
static byte_t symbolic[3] = {'#',0x20,'*'};
/* 求兩點間的距離 */
inline uint_t
distance( uint_t pos1X, uint_t pos1Y, uint_t pos2X, uint_t pos2Y ) {
uint_t ret = 0;
/* 距離公式 */
ret = (uint_t)sqrt((pow((double)((int)pos1X - (int)pos2X),2) + pow((double)((int)pos1Y - (int)pos2Y),2)));
return ret;
}
/* 壓縮座標 */
inline uint_t
create_pos( uint_t pX, uint_t pY ) {
uint_t ret = 0;
/* 將pX賦給ret，這樣pX座標在ret的低八位 */
ret = pX;
/* 將pX座標移到高八位去，這樣低位就能存放pY */
ret <<= 8;
/* 將pY存放放到ret的低八位，並保持高八位元的資料不變 */
ret |= pY;
return ret;
}
/*
== 估計函數 ===========================================
-p : 當前移動到的節點指針
-quit_x
-quit_y : quit_x 和 quit_y表示迷宮出口座標
-maze : 迷宮矩陣
=======================================================
*/
inline path_t *
evaluate( path_t *p, uint_t quit_x, uint_t quit_y, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] ) {
uint_t pX, pY;
/* 用於接收四個方向離開出口的距離，以便選擇最近的方向移動 */
int dis[4];
int minDis = 32767;
int minId = -1;
path_t *pnode = (path_t *)0;
register int i;
/* 計算當前節點的座標 */
pX = p->pos >> 8;
pY = p->pos & 0x00FF;
memset(dis, (int)-1, sizeof(int)*4);
/* 計算每個方向離開出口的距離，一次存放到dis陣列，若沒有i方向，則dis[i]任保留-1 */
for( i = 0; i < 4; ++i ) {
if( (p->direct & d_signal[i]) >> i == 0x01 )
dis[i] =(int)distance(pX + move[i][0], pY + move[i][1], quit_x, quit_y);
}
/* 獲得最短距離的方向 */
for(i = 0; i < 4; ++i) {
if(dis[i] != -1 && dis[i] < minDis) {
minId = i;
minDis = dis[i];
}
}
/* 若沒有可用的方向，則通知尋徑函數折回 */
if(minId == -1)
return (path_t *)0;
/* 用去最近距離方向的信號量 */
p->direct &= d_spend[minId];
/* 在移動到新位置之前，在舊位置處留下足跡 */
maze[pY][pX] = (byte_t)PATH_FOOTPRINT;
/* 構建新的路徑節點 */
pnode = (path_t *)malloc(sizeof(path_t));
assert(pnode);
/* 計算下一個位置的座標 */
pX += move[minId][0];
pY += move[minId][1];
pnode->pos = create_pos(pX, pY);
pnode->prev = p;
pnode->next = (path_t *)0;
pnode->direct = 0;

/* 在新位置處，計算下一個位置可用的移動方向 */
for(i = 0; i < 4; ++i) {
if(maze[pY + move[i][1]][pX + move[i][0]] != PATH_BLOCK && maze[pY + move[i][1]][pX + move[i][0]] != PATH_FOOTPRINT) {
/* 若嘗試的下一個位置不是障礙物或自己走過的足跡，則視為可用方向，獲得該方向的信號量 */
pnode->direct |= d_signal[i];
}
}

return pnode;
}
/*
== A*演算法尋徑函數 ===========================================
-eX
-eY :入口座標
-qX
-qY :出口座標
-maze :迷宮矩陣
=============================================================
*/
inline path_t *
AStar(uint_t eX, uint_t eY, uint_t qX, uint_t qY, byte_t maze[MAZE_HEIGHT][MAZE_WIDTH]) {
register int i;
/* 壓縮座標 */
uint_t quit_pos = create_pos(qX, qY);

/* 構建入口路徑節點，視為路徑鏈的頭 */
path_t *head = (path_t *)malloc(sizeof(path_t));
path_t *p = (path_t *)0;
path_t *back = (path_t *)0;
assert(head);
p = head;
p->direct = 0;
p->pos = create_pos(eX,eY);
p->next = (path_t *)0;
p->prev = (path_t *)0;
/* 創建入口處的可用方向 */
for(i = 0; i < 4; ++i) {
if(maze[eY + move[i][1]][eX + move[i][0]] != PATH_BLOCK)
/* 若無障礙物則獲得該方向的信號量 */
p->direct |= d_signal[i];
}
do {

/* 獲得下個路徑的節點指針 */
back = evaluate(p, qX, qY, maze);
if(back) {
p->next = back;
p = p->next;
}

/* 無路可走則折回 */
if(p->direct == 0 && p != head && p->pos != quit_pos) {
back = p->prev;
back->next = (path_t *)0;

/* 清楚腳印 */
maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_WALKON;

free(p);
p = back;
}
/* 若走不出迷宮，即折回到入口，且入口處的可用方向全部嘗試過 */
if(p == head && p->direct == 0) {
free(head);
return (path_t *)0;
}
} while( p->pos != quit_pos );

/* 在出口處留下腳印，便於列印 */
maze[p->pos & 0x00FF][p->pos >> 8] = (byte_t)PATH_FOOTPRINT;
return head;
}

/* AStar.h */
/* 設計者： yuki */
#ifndef __ASTAR_H
#define __ASTAR_H
#define MAZE_WIDTH 10 /* 迷宮寬度 */
#define MAZE_HEIGHT 10 /* 迷宮高度 */
#define PATH_BLOCK 0 /* 障礙物 */
#define PATH_WALKON 1 /* 可行走 */
#define PATH_FOOTPRINT 2 /* 腳印 */
#include "AStar.cpp"
#endif

/* main.cpp */
/* 設計者： yuki */
#include
#include
#include
#include
#include
#include
#include "AStar.h"
static byte_t maze[MAZE_HEIGHT][MAZE_WIDTH] = {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1, 1, 0, 1, 1, 1, 0, 1, 0,
0, 1, 1, 0, 1, 1, 1, 0, 1, 0,
0, 1, 1, 1, 1, 0, 0, 0, 1, 0,
0, 1, 0, 0, 0, 1, 1, 0, 1, 0,
0, 1, 1, 1, 0, 1, 1, 0, 1, 0,
0, 1, 0, 1, 1, 1, 0, 1, 1, 0,
0, 1, 0, 0, 0, 1, 0, 0, 1, 0,
0, 0, 1, 1, 1, 1, 1, 1, 1, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
int main() {
register int i,j;
path_t *pHead = AStar((uint_t)1,(uint_t)1,(uint_t)2,(uint_t)8,maze);
path_t *p = pHead;
path_t *bak;
if(p) {
bak = p->next;
printf("(%u,%u)",p->pos >> 8, p->pos & 0x00FF);
free(p);
p = bak;
while(p) {
bak = p->next;
printf("->(%u,%u)",p->pos >> 8, p->pos & 0x00FF);
free(p);
p = bak;
}
printf("\n");
}
else
printf("No path to get out of the maze\n");

pHead = p = bak = (path_t *)0;

/* 列印迷宮 */
for(i = 0; i < MAZE_HEIGHT; ++i) {
for(j = 0; j < MAZE_WIDTH; ++j)
printf("%c",symbolic[maze[i][j]]);
printf("\n");
}
getch();
return 0;
}