【轉】A* A星算法 C語言實現代碼

時間 2019-12-13

標籤算法 c語言實現代碼简体版

原文原文鏈接

http://blog.csdn.net/shanshanpt/article/details/8977512node

關於A*算法，很早就想寫點什麼，但是貌似每天在忙活着什麼，可事實又沒有作什麼，真是浮躁啊！因此今晚仍是來寫一下總結吧！算法

A*算法是很經典的只能啓發式搜索算法，關於只能搜索算法和通常的搜索算法(例如DFS，BFS之類)，在語言描述上的區別，我以爲用《代碼大全》中的一句話描述的很是好：「駕駛汽車達到某人家，寫成算法是：沿167號高速往南行至Puyallup，從XX出口後往山上開4.5英里，在一個雜貨店旁邊的紅綠燈右轉，接着在第一個路口左轉，從左邊的褐色大房子的車道進去就是；而啓發式是：找出上一次咱們給你寄的信，照着信上地址開車到這個鎮，到了那裏你問一下咱們房子在那裏，這裏的每個人都認識咱們，確定會有人願意幫助你，若是你找不到人，就找一個電話亭打電話給咱們，咱們出來接你」。聽上去仍是有點抽象。那麼下面咱們具體看看A*算法！數組

注：本文的圖形以及一些思想借助於http://www.policyalmanac.org/games/aStarTutorial.htm，很是感謝經典文章！函數

咱們要想從綠色的點到紅色的點，須要啓發式得去找一條路徑，到底怎麼去找呢，開始沒有辦法，只能從開始點慢慢嘗試！咱們須要定義一個OPEN表，OPEN表中放的是什麼呢？就是當前考慮的點，及其周邊的點須要添加進來，做爲可能的路徑上的點。這樣說可能有點抽象，那麼先看下面：優化

咱們從起始點開始搜索：spa

1：從點A開始，而且把它做爲待處理點存入一個OPEN表。你的路徑可能會經過它包含的方格，也可能不會。基本上，這是一個待檢查方格的列表。
2：尋找起點周圍(鄰居)全部可到達或者可經過的方格，若是有障礙物方格。那麼就不須要考慮他們，對於其餘的呂劇節點也把他們加入OPEN表。這些鄰居節點認當前點爲父節點。(父節點的保存是必須的！由於當咱們找到最後一個點的時候，須要經過父節點回溯，找到全部點直到開始節點！那麼就是一個完整的路徑！)
3：從開啓列表中刪除點A，把它加入到一個CLOSE列表，列表中保存全部不須要再次檢查的方格。.net

在這一點，你應該造成如圖的結構。在圖中，暗綠色方格是你起始方格的中心。它被用淺藍色描邊，以表示它被加入到關閉列表中了。全部的相鄰格如今都在開啓列表中，它們被用淺綠色描邊。每一個方格都有一個灰色指針反指他們的父方格，也就是開始的方格。指針

結下來咱們應該選擇哪個點繼續考慮呢！我想你們應該知道所謂的啓發函數，所謂權值之類(此處所謂的權值就是路勁的長度)。YES，咱們須要OPEN表中權值F最小的那個點！爲何呢，固然是權值越小，越靠近目標點咯！htm

對於權值咱們設爲F，那麼F怎麼計算到的！咱們有兩個項！G和H， blog

G = 從起點A，沿着產生的路徑，移動到網格上指定方格的路徑耗費。
H = 從網格上那個方格移動到終點B的預估移動耗費。這常常被稱爲啓發式的。這樣叫的緣由是由於它只是個猜想。咱們沒辦法事先知道路徑的長度。(可是咱們須要知道：雖然只是猜想，可是隻要是基於一個統一的標準，相對遠近的趨勢是不變的！這一點是很重要的！ )

例如：H值的估計採用「曼哈頓」法，也就是當前的點，到目標點，橫向和縱向的格子數相加，就是H值！

( 注意：對於障礙物咱們是不考慮是否跳過的問題！即，障礙物也當作正常的一個格子！這也是H值僅僅是預測的而已的緣由！即所謂啓發式！ )

那麼對於第一幅圖，起始點離終點的H值是：橫向相差4個格子，縱向相差0個格子，那麼H=4+0=4；

固然也有其餘的辦法，例如使用直線距離，sqrt( pow( des.x - src.x ) + pow( des.y - src.y ) )也是能夠的~

對於G值！在這個例子，令水平或者垂直移動的耗費爲10，對角線方向耗費爲14。咱們取這些值是由於沿對角線的距離是沿水平或垂直移動耗費的的根號2約1.414倍。爲了簡化，咱們用10和14近似。有時候簡化是很重要的~

( 其實距離只要反應了基本的倍數關係就能夠了！ )

對於起始點以及周圍點的G值，H值和F值，下圖能夠很清晰的看到！( 左上角是F，左下角是G，右下角是H )

對於G值，只要是橫向和豎向的鄰居，設爲+10，斜向的鄰居+14就能夠了~計算真的很easy吧~呵呵~

對於H值，就是數格子就是咯~

F = G + H

注意上面的鄰居節點都加入OPEN表了哦~~~ 起點從OPEN中刪除，加入CLOSE中~

接着計算：

而後從OPEN表中選擇一個F值最小的而後考慮其周圍鄰居，再循環處理！直到終點加入CLOSE中，尋路結束！或者OPEN空了，沒找到路徑！

( 至於咱們怎麼選出最小的那個點呢！？咱們使用堆排序處理的，對於選出最小值是很快的~ )

能夠看到，F最小的是起始點右邊的那個點，下面框框表示的~

而後再考慮其鄰居：

此時對於其鄰居有幾種可能性！

1：鄰居已經在CLOSE表中了，那麼不須要考慮了~

2：鄰居是障礙物，不須要考慮了e

3：鄰居不在OPEN表中，那麼將鄰居加入OPEN，並將次鄰居的父節點賦值爲當前節點

4：鄰居在OPEN中，那麼須要看看此鄰居點的G值與當前點的(G值+當前點到鄰居點的距離(10或者14))的大小，若是今後點走更近(即G值更小)，那麼將此點的父節點換成當前點！(注意：對於同一個點，G值下，那麼F必須小！由於H是相同的！)

下面是進一步的圖示：

那麼咱們按照上面的思路，知道終點加入CLOSE表！（終極圖示以下）

最終咱們能夠獲得路徑：( 以前咱們說過，由父節點往前回溯就很easy的獲得路徑了~ )

說了這麼多，也不知道說的行不行，仍是須要總結一下！

總結：

1：將起始點加入OPEN表中

2：循環直到OPEN爲空或者終點加入CLOSE表中

不然：找到OPEN表中F值最小的節點(使用堆排序獲得小值點)，將此點從OPEN刪除，加入CLOSE！

(此時最小點已經取出，那麼須要重新排序OPEN表，是的第一個點是最小F的點！)

對8個鄰居進行處理：

若：1：鄰居已經在CLOSE表中了，那麼不須要考慮了~

2：鄰居是障礙物，不須要考慮了e

3：鄰居不在OPEN表中，那麼將鄰居加入OPEN，並將次鄰居的父節點賦值爲當前節點

4：鄰居在OPEN中，那麼須要看看此鄰居點的G值與當前點的(G值+當前點到鄰居點的距

離 (10或者14))的大小，若是今後點走更近(即G值更小)，那麼將此點的父節點換成當前點！ (注意：對於同一個點，G值下，那麼F必須小！由於H是相同的！)

注意：當父節點改變後，OPEN表中的最小點可能會變化，那麼須要再次排序獲得最

小點！

3：結束，根據退出循環的條件能夠知道到底有沒有找到路徑！因此下面的工做就easy了~

基本的原理就是這樣的~

下面給一個簡單的C語言的演示代碼，只是爲了演示原理，沒有注重其餘問題，因此你們莫怪啊~

注意：數組中1表明起點，2表明終點，0表明能夠經過，3表明障礙物

[cpp] view plain copy print ?

#include <stdio.h>
#include <stdlib.h>
#define STARTNODE 1
#define ENDNODE 2
#define BARRIER 3
typedef struct AStarNode
{
int s_x; // 座標(最終輸出路徑須要)
int s_y;
int s_g; // 起點到此點的距離( 由g和h能夠獲得f，此處f省略，f=g+h )
int s_h; // 啓發函數預測的此點到終點的距離
int s_style;// 結點類型：起始點，終點，障礙物
struct AStarNode * s_parent; // 父節點
int s_is_in_closetable; // 是否在close表中
int s_is_in_opentable; // 是否在open表中
}AStarNode, *pAStarNode;
AStarNode map_maze[10][10]; // 結點數組
pAStarNode open_table[100]; // open表
pAStarNode close_table[100]; // close表
int open_node_count; // open表中節點數量
int close_node_count; // close表中結點數量
pAStarNode path_stack[100]; // 保存路徑的棧
int top = -1; // 棧頂
// 交換兩個元素
//
void swap( int idx1, int idx2 )
{
pAStarNode tmp = open_table[idx1];
open_table[idx1] = open_table[idx2];
open_table[idx2] = tmp;
}
// 堆調整
//
void adjust_heap( int /*i*/nIndex )
{
int curr = nIndex;
int child = curr * 2 + 1; // 獲得左孩子idx( 下標從0開始，全部作孩子是curr*2+1 )
int parent = ( curr - 1 ) / 2; // 獲得雙親idx
if (nIndex < 0 || nIndex >= open_node_count)
{
return;
}
// 往下調整( 要比較左右孩子和cuur parent )
//
while ( child < open_node_count )
{
// 小根堆是雙親值小於孩子值
//
if ( child + 1 < open_node_count && open_table[child]->s_g + open_table[child]->s_h > open_table[child+1]->s_g + open_table[child+1]->s_h )
{
++child; // 判斷左右孩子大小
}
if (open_table[curr]->s_g + open_table[curr]->s_h <= open_table[child]->s_g + open_table[child]->s_h)
{
break;
}
else
{
swap( child, curr ); // 交換節點
curr = child; // 再判斷當前孩子節點
child = curr * 2 + 1; // 再判斷左孩子
}
}
if (curr != nIndex)
{
return;
}
// 往上調整( 只須要比較cuur child和parent )
//
while (curr != 0)
{
if (open_table[curr]->s_g + open_table[curr]->s_h >= open_table[parent]->s_g + open_table[parent]->s_h)
{
break;
}
else
{
swap( curr, parent );
curr = parent;
parent = (curr-1)/2;
}
}
}
// 判斷鄰居點是否能夠進入open表
//
void insert_to_opentable( int x, int y, pAStarNode curr_node, pAStarNode end_node, int w )
{
int i;
if ( map_maze[x][y].s_style != BARRIER ) // 不是障礙物
{
if ( !map_maze[x][y].s_is_in_closetable ) // 不在閉表中
{
if ( map_maze[x][y].s_is_in_opentable ) // 在open表中
{
// 須要判斷是不是一條更優化的路徑
//
if ( map_maze[x][y].s_g > curr_node->s_g + w ) // 若是更優化
{
map_maze[x][y].s_g = curr_node->s_g + w;
map_maze[x][y].s_parent = curr_node;
for ( i = 0; i < open_node_count; ++i )
{
if ( open_table[i]->s_x == map_maze[x][y].s_x && open_table[i]->s_y == map_maze[x][y].s_y )
{
break;
}
}
adjust_heap( i ); // 下面調整點
}
}
else // 不在open中
{
map_maze[x][y].s_g = curr_node->s_g + w;
map_maze[x][y].s_h = abs(end_node->s_x - x ) + abs(end_node->s_y - y);
map_maze[x][y].s_parent = curr_node;
map_maze[x][y].s_is_in_opentable = 1;
open_table[open_node_count++] = &(map_maze[x][y]);
}
}
}
}
// 查找鄰居
// 對上下左右8個鄰居進行查找
//
void get_neighbors( pAStarNode curr_node, pAStarNode end_node )
{
int x = curr_node->s_x;
int y = curr_node->s_y;
// 下面對於8個鄰居進行處理！
//
if ( ( x + 1 ) >= 0 && ( x + 1 ) < 10 && y >= 0 && y < 10 )
{
insert_to_opentable( x+1, y, curr_node, end_node, 10 );
}
if ( ( x - 1 ) >= 0 && ( x - 1 ) < 10 && y >= 0 && y < 10 )
{
insert_to_opentable( x-1, y, curr_node, end_node, 10 );
}
if ( x >= 0 && x < 10 && ( y + 1 ) >= 0 && ( y + 1 ) < 10 )
{
insert_to_opentable( x, y+1, curr_node, end_node, 10 );
}
if ( x >= 0 && x < 10 && ( y - 1 ) >= 0 && ( y - 1 ) < 10 )
{
insert_to_opentable( x, y-1, curr_node, end_node, 10 );
}
if ( ( x + 1 ) >= 0 && ( x + 1 ) < 10 && ( y + 1 ) >= 0 && ( y + 1 ) < 10 )
{
insert_to_opentable( x+1, y+1, curr_node, end_node, 14 );
}
if ( ( x + 1 ) >= 0 && ( x + 1 ) < 10 && ( y - 1 ) >= 0 && ( y - 1 ) < 10 )
{
insert_to_opentable( x+1, y-1, curr_node, end_node, 14 );
}
if ( ( x - 1 ) >= 0 && ( x - 1 ) < 10 && ( y + 1 ) >= 0 && ( y + 1 ) < 10 )
{
insert_to_opentable( x-1, y+1, curr_node, end_node, 14 );
}
if ( ( x - 1 ) >= 0 && ( x - 1 ) < 10 && ( y - 1 ) >= 0 && ( y - 1 ) < 10 )
{
insert_to_opentable( x-1, y-1, curr_node, end_node, 14 );
}
}
int main()
{
// 地圖數組的定義
//
AStarNode *start_node; // 起始點
AStarNode *end_node; // 結束點
AStarNode *curr_node; // 當前點
int is_found; // 是否找到路徑
int maze[][10] ={ // 僅僅爲了好賦值給map_maze
{ 1,0,0,3,0,3,0,0,0,0 },
{ 0,0,3,0,0,3,0,3,0,3 },
{ 3,0,0,0,0,3,3,3,0,3 },
{ 3,0,3,0,0,0,0,0,0,3 },
{ 3,0,0,0,0,3,0,0,0,3 },
{ 3,0,0,3,0,0,0,3,0,3 },
{ 3,0,0,0,0,3,3,0,0,0 },
{ 0,0,2,0,0,0,0,0,0,0 },
{ 3,3,3,0,0,3,0,3,0,3 },
{ 3,0,0,0,0,3,3,3,0,3 },
};
int i,j,x;
// 下面準備點
//
for( i = 0; i < 10; ++i )
{
for ( j = 0; j < 10; ++j )
{
map_maze[i][j].s_g = 0;
map_maze[i][j].s_h = 0;
map_maze[i][j].s_is_in_closetable = 0;
map_maze[i][j].s_is_in_opentable = 0;
map_maze[i][j].s_style = maze[i][j];
map_maze[i][j].s_x = i;
map_maze[i][j].s_y = j;
map_maze[i][j].s_parent = NULL;
if ( map_maze[i][j].s_style == STARTNODE ) // 起點
{
start_node = &(map_maze[i][j]);
}
else if( map_maze[i][j].s_style == ENDNODE ) // 終點
{
end_node = &(map_maze[i][j]);
}
printf("%d ", maze[i][j]);
}
printf("\n");
}
// 下面使用A*算法獲得路徑
//
open_table[open_node_count++] = start_node; // 起始點加入open表
start_node->s_is_in_opentable = 1; // 加入open表
start_node->s_g = 0;
start_node->s_h = abs(end_node->s_x - start_node->s_x) + abs(end_node->s_y - start_node->s_y);
start_node->s_parent = NULL;
if ( start_node->s_x == end_node->s_x && start_node->s_y == end_node->s_y )
{
printf("起點==終點！\n");
return 0;
}
is_found = 0;
while( 1 )
{
// for test
//
/* for ( x = 0; x < open_node_count; ++x )
{
printf("(%d,%d):%d ", open_table[x]->s_x, open_table[x]->s_y, open_table[x]->s_g+open_table[x]->s_h);
}
printf("\n\n");
*/
curr_node = open_table[0]; // open表的第一個點必定是f值最小的點(經過堆排序獲得的)
open_table[0] = open_table[--open_node_count]; // 最後一個點放到第一個點，而後進行堆調整
adjust_heap( 0 ); // 調整堆
close_table[close_node_count++] = curr_node; // 當前點加入close表
curr_node->s_is_in_closetable = 1; // 已經在close表中了
if ( curr_node->s_x == end_node->s_x && curr_node->s_y == end_node->s_y )// 終點在close中，結束
{
is_found = 1;
break;
}
get_neighbors( curr_node, end_node ); // 對鄰居的處理
if ( open_node_count == 0 ) // 沒有路徑到達
{
is_found = 0;
break;
}
}
if ( is_found )
{
curr_node = end_node;
while( curr_node )
{
path_stack[++top] = curr_node;
curr_node = curr_node->s_parent;
}
while( top >= 0 ) // 下面是輸出路徑看看~
{
if ( top > 0 )
{
printf("(%d,%d)-->", path_stack[top]->s_x, path_stack[top--]->s_y);
}
else
{
printf("(%d,%d)", path_stack[top]->s_x, path_stack[top--]->s_y);
}
}
}
else
{
printf("麼有找到路徑");
}
puts("");
return 0;
}