最近搞MTK斯凱冒泡平臺的遊戲開發,碰到了自動尋路的問題,很多程式設計師都知道A*演算法,既簡單有使用!
所以我也選擇了A*演算法,由於時間比較緊,就在網上百度此演算法的C實現,確實有很多!
但經測試都有不同的問題,並不能用在商業遊戲中,所以最後決定還是自己寫吧!
A*原理 比較簡單,網上有很多介紹的!我也是在網上看的,這裡就不重複了!
由於我是Java程式設計師剛開始搞嵌入式C開發不久,所以有很多C用法不是很熟悉,通過搞這個演算法又知道不少知識
比如
Java裡的集合 C裡要用連結串列
這也是此演算法比較重要的一個技術點,遍歷連結串列,還有刪減節點,這些對於C程式設計師來說應該都是很簡單的事情,
這裡還是說一下,以便那些從JAVA轉入C開發的程式設計師快速理解
PS: 這裡使用的是 前插式單向連結串列
1. 資料定義
- typedef struct Node
- {//節點結構體
- int f,g,h;
- int row; //該節點所在行
- int col; //該節點所在列
- int direction;//parent節點要移動的方向就能到達本節點
- struct Node * parent;
- }Node, *Lnode;
- typedef struct Stack
- {//OPEN CLOSED 表結構體
- Node * npoint;
- struct Stack * next;
- }Stack, *Lstack;
2. 插入連結串列
連結串列有頭,有下一個節點的指標,新建的連結串列都是 HEAD->NULL,就是 頭 下一個節點是NULL,
增加節點是在 頭和NEXT之間插入的,這時就是 HEAD->NEXT0->NULL,
再增加一個節點: HEAD->NEXT1->NEXT0->NULL,
插入連結串列的程式碼示例
- void PutintoOpen(Node * suc )
- {//把節點放入OPEN 或CLOSED 表中
- Stack * temp;
- temp =(Stack *) malloc(sizeof(Stack));
- temp->npoint = suc;
- temp->next = Open->next;
- Open->next = temp;
- }
3. 連結串列上節點的刪除
有時,需要將一個節點從連結串列中刪除,比如當前連結串列資料為 HEAD->NEXT1->NEXT0->NULL,
將其中的NEXT1節點刪除(釋放)掉,
那麼就需要先有一個指標指向NEXT1
然後把HEAD指向NEXT1的下一個節點,也就是NEXT0
最後再free( NEXT1 );
刪除連結串列某節點的示例程式碼
- Node * getNodeFromOpen()
- {//選取OPEN表上f值最小的節點,返回該節點地址
- Lstack temp = Open->next,min = Open->next,minp = Open;
- Node * minx;
- if( temp == NULL )
- return NULL;
- while(temp->next != NULL)
- {
- if( (temp->next ->npoint->f) < (min->npoint->f) )
- {
- min = temp->next;
- minp = temp;
- }
- temp = temp->next;
- }
- minx = min->npoint;
- temp = minp->next;
- minp->next = minp->next->next;
- free(temp);
- return minx;
- }
4. 遍歷連結串列
連結串列的遍歷,就是從HEAD開始,按下一個節點,順序找到一個為NULL的節點就遍歷完整個連結串列了!
遍歷連結串列的程式碼示例
- Node * BelongInOpen( Node * suc )
- {//判斷節點是否屬於OPEN表或CLOSED表,是則返回節點地址,否則返回空地址
- Lstack temp = Open -> next ;
- if(temp == NULL)
- return NULL;
- while( temp != NULL )
- {
- if( Equal(suc,temp->npoint) )
- {
- return temp -> npoint;
- }
- else
- {
- temp = temp->next;
- }
- }
- return NULL;
- }
5. 連結串列的刪除
連結串列,連結串列上節點的資料都是在程式中臨時建立出來的,這些資料都使用的是堆記憶體,當連結串列不在使用的時候需要手動釋放,
連結串列的刪除程式碼示例
- //正序遍歷連結串列
- void printfOpenData()
- {//判斷節點是否屬於OPEN表或CLOSED表,是則返回節點地址,否則返回空地址
- Lstack temp = Open -> next;
- Node *p_node;
- if(temp == NULL)
- return;
- while(temp != NULL)
- {
- Lstack head = temp;
- temp = temp->next;
- p_node = head->npoint;
- printf("Open庫資料![%d,%d] ", p_node->col, p_node->row );
- free(p_node);
- free( head );
- Open->next = temp;
- }
- printf(" Open庫資料 資料全部清楚 ");
- return;
- }
好了,連結串列的操作基本講完了!
接下來的全部程式碼應該都比較好理解,整個演算法的演示程式僅有一個 .c檔案即可,您可以在TC, VC6, C-Free5 下編譯通過!
A*演算法 C語言實現的程式碼示例
- /*******************************************************************************
- * CopyRight (c) HYTC Ltd. All rights reserved.
- * Filename: main.c
- * Creator: GaoLei
- * Version: 0.0
- * Date: 2011-06-15
- * QQ: 38929568
- * Description: A*尋路演算法 測試類
- *******************************************************************************/
- #include <stdlib.h>
- #include <stdio.h>
- #include <math.h>
- #define FALSE 0
- #define TRUE 1
- #define NULL 0
- typedef int BOOL;
- int map[20][20] =
- {
- { 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0 },
- { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },
- { 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0 },
- { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
- };
- typedef struct Node
- {//節點結構體
- int f,g,h;
- int row; //該節點所在行
- int col; //該節點所在列
- int direction;//parent節點要移動的方向就能到達本節點
- struct Node * parent;
- }Node, *Lnode;
- typedef struct Stack
- {//OPEN CLOSED 表結構體
- Node * npoint;
- struct Stack * next;
- }Stack, *Lstack;
- int rows = 20; //地圖行數
- int cols = 20; //地圖列數
- int G_OFFSET = 1; //每個圖塊G值的增加值
- int destinationRow; //目標所在行
- int destinationCol; //目標所在列
- int canMoveIndex = 0; //可以通行的地圖圖塊索引
- int tileSize = 1; //圖塊大小
- Lstack Open = NULL;
- Lstack Closed = NULL;
- Node * getNodeFromOpen()
- {//選取OPEN表上f值最小的節點,返回該節點地址
- Lstack temp = Open->next,min = Open->next,minp = Open;
- Node * minx;
- if( temp == NULL )
- return NULL;
- while(temp->next != NULL)
- {
- if( (temp->next ->npoint->f) < (min->npoint->f) )
- {
- min = temp->next;
- minp = temp;
- }
- temp = temp->next;
- }
- minx = min->npoint;
- temp = minp->next;
- minp->next = minp->next->next;
- free(temp);
- return minx;
- }
- BOOL Equal(Node * suc,Node * goal)
- {//判斷節點是否相等,相等,不相等
- if ( (suc->row == goal->row) && (suc->col == goal->col) )
- {
- return TRUE;
- }
- else
- {
- return FALSE;
- }
- }
- Node * BelongInOpen( Node * suc )
- {//判斷節點是否屬於OPEN表或CLOSED表,是則返回節點地址,否則返回空地址
- Lstack temp = Open -> next ;
- if(temp == NULL)
- return NULL;
- while( temp != NULL )
- {
- if( Equal(suc,temp->npoint) )
- {
- return temp -> npoint;
- }
- else
- {
- temp = temp->next;
- }
- }
- return NULL;
- }
- Node * BelongInClosed( Node * suc )
- {//判斷節點是否屬於OPEN表或CLOSED表,是則返回節點地址,否則返回空地址
- Lstack temp = Closed -> next ;
- if(temp == NULL)
- return NULL;
- while(temp != NULL)
- {
- if( Equal(suc,temp->npoint) )
- {
- return temp -> npoint;
- }
- else
- {
- temp = temp->next;
- }
- }
- return NULL;
- }
- void PutintoOpen(Node * suc )
- {//把節點放入OPEN 或CLOSED 表中
- Stack * temp;
- temp =(Stack *) malloc(sizeof(Stack));
- temp->npoint = suc;
- temp->next = Open->next;
- Open->next = temp;
- }
- void PutintoClosed(Node * suc )
- {//把節點放入OPEN 或CLOSED 表中
- Stack * temp;
- temp =(Stack *) malloc(sizeof(Stack));
- temp->npoint = suc;
- temp->next = Closed->next;
- Closed->next = temp;
- }
- //得到該圖塊的H值
- int getH(int row, int col)
- {
- return (abs(destinationRow - row) + abs(destinationCol - col));
- }
- //得到該位置所在地圖行
- int getRowPosition(int y)
- {
- return (y / tileSize);
- }
- //得到該位置所在地圖列
- int getColPosition(int x)
- {
- return (x / tileSize);
- }
- //檢測該圖塊是否可通行
- BOOL isCanMove(int col, int row)
- {
- if(col < 0 || col >= cols)
- return FALSE;
- if(row < 0 || row >= rows)
- return FALSE;
- return map[col][row] == canMoveIndex;
- }
- Node* checkOpen(int row, int col)
- {
- Lstack temp = Open -> next;
- if ( temp == NULL )
- return NULL;
- while (temp != NULL)
- {
- if ( (temp->npoint->row==row) && (temp->npoint->col == col) )
- {
- return temp -> npoint;
- }
- else
- {
- temp = temp->next;
- }
- }
- return NULL;
- }
- BOOL isInClose(int row, int col)
- {
- Lstack temp = Closed -> next;
- if ( temp == NULL )
- return FALSE;
- while (temp != NULL)
- {
- if ( (temp->npoint->row==row) && (temp->npoint->col == col) )
- {
- return TRUE;
- }
- else
- {
- temp = temp->next;
- }
- }
- return FALSE;
- }
- int directionIndex =0;
- int direction[256];
- void creatSeccessionNode(Node *bestNode, int row, int col)
- {
- int g = bestNode->g + G_OFFSET;
- if(!isInClose(row, col))
- {
- Node *oldNode = NULL;
- if((oldNode = checkOpen(row, col)) != NULL)
- {
- if(oldNode->g < g)
- {
- oldNode->parent = bestNode;
- oldNode->g = g;
- oldNode->f = g + oldNode->h;
- }
- }
- else
- {
- Node *node = (Node *) malloc(sizeof(Node));
- node->parent = bestNode;
- node->g = g;
- node->h = getH(row, col);
- node->f = node->g + node->h;
- node->row = row;
- node->col = col;
- directionIndex++;
- node->direction = directionIndex;
- // openNode.addElement(node);
- PutintoOpen( node );
- }
- }
- }
- /**
- * 根據傳入的節點生成子節點
- * @param bestNode
- * @param destinationRow
- * @param destinationCol
- */
- void seachSeccessionNode(Node *bestNode)
- {
- int row, col;
- // Node *bestNodeInOpen = NULL;
- //上部節點
- if(isCanMove(row = bestNode->row - 1, col = bestNode->col))
- {
- creatSeccessionNode(bestNode, row, col);
- }
- //下部節點
- if(isCanMove(row = bestNode->row + 1, col = bestNode->col))
- {
- creatSeccessionNode(bestNode, row, col);
- }
- //左部節點
- if(isCanMove(row = bestNode->row, col = bestNode->col - 1))
- {
- creatSeccessionNode(bestNode, row, col);
- }
- //右部節點
- if(isCanMove(row = bestNode->row, col = bestNode->col + 1))
- {
- creatSeccessionNode(bestNode, row, col);
- }
- PutintoClosed( bestNode );
- }
- //正序遍歷連結串列
- void printfOpenData()
- {//判斷節點是否屬於OPEN表或CLOSED表,是則返回節點地址,否則返回空地址
- Lstack temp = Open -> next;
- Node *p_node;
- if(temp == NULL)
- return;
- while(temp != NULL)
- {
- Lstack head = temp;
- temp = temp->next;
- p_node = head->npoint;
- printf("Open庫資料![%d,%d] ", p_node->col, p_node->row );
- free(p_node);
- free( head );
- Open->next = temp;
- }
- printf(" Open庫資料 資料全部清楚 ");
- return;
- }
- void printfClosedData()
- {//判斷節點是否屬於OPEN表或CLOSED表,是則返回節點地址,否則返回空地址
- Lstack temp = Closed -> next ;
- Node *p_node;
- if(temp == NULL)
- return;
- while(temp != NULL)
- {
- Lstack head = temp;
- temp = temp->next;
- p_node = head->npoint;
- printf("Closed庫資料![%d,%d] ", p_node->col, p_node->row );
- free(p_node);
- free( head );
- Closed -> next = temp;
- }
- printf(" Closed庫資料 資料全部清楚 ");
- /*
- temp = Closed -> next;
- while(temp != NULL)
- {
- printf("Closed庫資料!節點");
- temp = temp->next;
- }*/
- return;
- }
- void getPath(int startX, int StartY, int destinationX, int destinationY)
- {
- Node *startNode = (Node *) malloc(sizeof(Node));
- Node *bestNode = NULL;
- int index = 0;
- destinationRow = getRowPosition(destinationY);
- destinationCol = getColPosition(destinationX);
- startNode->parent= NULL;
- startNode->row = getRowPosition(StartY);
- startNode->col = getColPosition(startX);
- startNode->g = 0;
- startNode->h = getH( startNode->row, startNode->col );
- startNode->f = startNode->g + startNode->h;
- startNode->direction = 0;
- PutintoOpen( startNode );// openNode.add(startNode);
- while(TRUE)
- {
- bestNode = getNodeFromOpen(); //從OPEN表中取出f值最小的節點
- if(bestNode == NULL)//未找到路徑
- {
- printf("未找到路徑 ");
- return;
- }
- else if(bestNode->row == destinationRow
- && bestNode->col == destinationCol )
- {
- Node *_Node = bestNode;
- int nodeSum = 0;
- int nodeIndex =0;
- printf("程式執行次數=%d ",index);
- while( _Node->parent != NULL )
- {
- printf("x:%d y:%d direction = %d ", _Node->col, _Node->row, _Node->direction );
- _Node = _Node->parent;
- nodeSum += 1;
- }
- printf("節點數量=%d ",nodeSum);
- _Node = bestNode;
- nodeIndex = nodeSum-1;
- while( _Node->parent != NULL && nodeIndex>=0)
- {
- Node *_NodeParent = _Node->parent;
- printf("x:%d y:%d direction = %d ", _Node->col, _Node->row, _Node->direction );
- if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == +1 )
- {//從父節點到本節點的操作是 上
- direction[nodeIndex] = 1;
- }
- else if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == -1 )
- {//從父節點到本節點的操作是 下
- direction[nodeIndex] = 2;
- }
- else if( _NodeParent->col - _Node->col == +1 && _NodeParent->row - _Node->row == 0 )
- {//從父節點到本節點的操作是 左
- direction[nodeIndex] = 3;
- }
- else if( _NodeParent->col - _Node->col == -1 && _NodeParent->row - _Node->row == 0 )
- {//從父節點到本節點的操作是 右
- direction[nodeIndex] = 4;
- }
- else
- {
- direction[nodeIndex] = 0;
- }
- nodeIndex -= 1;
- _Node = _Node->parent;
- }
- for( nodeIndex=0; nodeIndex<nodeSum; nodeIndex++ )
- {
- printf("direction[%d]=%d ",nodeIndex,direction[nodeIndex]);
- }
- return ;
- }
- index++;
- seachSeccessionNode(bestNode);
- }
- }
- void main()
- {//主函式
- //初始操作,建立open和closed表
- Open = (Stack *) malloc(sizeof(Stack));
- Open->next = NULL;
- Closed = (Stack *) malloc(sizeof(Stack));
- Closed->next = NULL;
- //-----------------------------------
- getPath( 0, 0, 19, 19 );
- printf("程式認定該起始狀態無法道達目標狀態! ");
- printfOpenData();
- printfClosedData();
- free(Open);
- free(Closed);
- //--------------------------------------
- }
本程式很容易修改成您工程需要的形式,比如
我的使用方式是這樣的
- //將main() 方法改成
- void autoPathfinding( uint16 *MD, uint8 map_w, uint8 map_h, int16 startX, int16 startY, int16 destinationX, int16 destinationY )
這樣的目的是 將地圖資料 以及當前位置,和目標位置傳遞個 自動尋路方法
還需要修改 能否通過的方法,可以根據自己的地圖來做處理
- //檢測該圖塊是否可通行
- BOOL isCanMove(int col, int row)
- {
- if(col < 0 || col >= cols)
- return FALSE;
- if(row < 0 || row >= rows)
- return FALSE;
- //return map[col][row] == canMoveIndex;
- return isCanGoByMapTile( MapData[ (col)*rows+(row) ] );
- }
PS: 看原始碼的時候,如果你覺得有些變數定義的位置不舒服,是因為我的嵌入式C所用的環境要求,當然您可以根據您的環境自由修改!
時間關係,並沒有特別優化,如果您有好的優化方案,我願意聆聽!