C语言实现A*算法

 

最近搞MTK斯凯冒泡平台的游戏开发，碰到了自动寻路的问题，很多程序员都知道A*算法，既简单有使用！

 

所以我也选择了A*算法，由于时间比较紧，就在网上百度此算法的C实现，确实有很多！

 

但经测试都有不同的问题，并不能用在商业游戏中，所以最后决定还是自己写吧！

 

A*原理 比较简单，网上有很多介绍的！我也是在网上看的，这里就不重复了！

 

由于我是Java程序员刚开始搞嵌入式C开发不久，所以有很多C用法不是很熟悉，通过搞这个算法又知道不少知识

 

比如

 

Java里的集合   C里要用链表

 

这也是此算法比较重要的一个技术点，遍历链表，还有删减节点，这些对于C程序员来说应该都是很简单的事情，

 

这里还是说一下，以便那些从JAVA转入C开发的程序员快速理解

 

PS: 这里使用的是 前插式单向链表

1. 数据定义

1. typedef struct Node   
2. {//节点结构体   
3.     int f,g,h;   
4.     int row;    //该节点所在行   
5.     int col;    //该节点所在列   
6.     int direction;//parent节点要移动的方向就能到达本节点   
7.     struct Node * parent;   
8. }Node, *Lnode;   
9.    
10. typedef struct Stack   
11. {//OPEN CLOSED 表结构体   
12.     Node * npoint;   
13.     struct Stack * next;   
14. }Stack, *Lstack;   
15.   

2. 插入链表

链表有头，有下一个节点的指针，新建的链表都是 HEAD->NULL，就是 头 下一个节点是NULL,

 

增加节点是在 头和NEXT之间插入的，这时就是 HEAD->NEXT0->NULL,

 

再增加一个节点： HEAD->NEXT1->NEXT0->NULL,

 

插入链表的代码示例

1. void PutintoOpen(Node * suc )   
2. {//把节点放入OPEN 或CLOSED 表中   
3.     Stack * temp;   
4.     temp =(Stack *) malloc(sizeof(Stack));   
5.     temp->npoint = suc;   
6.    
7.     temp->next = Open->next;   
8.     Open->next = temp;   
9. }   

 

3. 链表上节点的删除

有时，需要将一个节点从链表中删除，比如当前链表数据为 HEAD->NEXT1->NEXT0->NULL,

 

将其中的NEXT1节点删除（释放）掉，

 

那么就需要先有一个指针指向NEXT1

 

然后把HEAD指向NEXT1的下一个节点，也就是NEXT0

 

最后再free( NEXT1 );

 

删除链表某节点的示例代码

1. Node * getNodeFromOpen()   
2. {//选取OPEN表上f值最小的节点，返回该节点地址   
3.     Lstack temp = Open->next,min = Open->next,minp = Open;   
4.     Node * minx;   
5.     if( temp == NULL )   
6.         return NULL;   
7.        
8.     while(temp->next != NULL)   
9.     {   
10.         if( (temp->next ->npoint->f) < (min->npoint->f) )   
11.         {   
12.             min = temp->next;   
13.             minp = temp;   
14.         }   
15.         temp = temp->next;   
16.     }   
17.     minx = min->npoint;   
18.     temp = minp->next;   
19.     minp->next = minp->next->next;   
20.     free(temp);   
21.     return minx;   
22. }   
23.   

 

4. 遍历链表

链表的遍历，就是从HEAD开始，按下一个节点，顺序找到一个为NULL的节点就遍历完整个链表了！

 

遍历链表的代码示例

1. Node * BelongInOpen( Node * suc )   
2. {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
3.     Lstack temp = Open -> next ;   
4.     if(temp == NULL)   
5.         return NULL;   
6.     while( temp != NULL )   
7.     {   
8.         if( Equal(suc,temp->npoint) )   
9.         {   
10.             return temp -> npoint;   
11.         }   
12.         else   
13.         {   
14.             temp = temp->next;      
15.         }   
16.     }   
17.     return NULL;   
18. }   

 

 

5. 链表的删除

链表，链表上节点的数据都是在程序中临时创建出来的，这些数据都使用的是堆内存，当链表不在使用的时候需要手动释放，

 

链表的删除代码示例

1. //正序遍历链表   
2. void printfOpenData()   
3. {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
4.     Lstack temp = Open -> next;   
5.     Node *p_node;   
6.     if(temp == NULL)   
7.         return;   
8.     while(temp != NULL)   
9.     {   
10.         Lstack head = temp;   
11.         temp = temp->next;   
12.         p_node = head->npoint;   
13.         printf("Open库数据！[%d,%d]\n", p_node->col, p_node->row );   
14.         free(p_node);   
15.         free( head );   
16.         Open->next = temp;   
17.     }   
18.     printf("\n Open库数据 数据全部清楚 \n");   
19.     return;   
20. }   

 

 

好了，链表的操作基本讲完了！

 

接下来的全部代码应该都比较好理解，整个算法的演示程序仅有一个 .c文件即可，您可以在TC, VC6, C-Free5 下编译通过！

 

A*算法 C语言实现的代码示例

1. /*******************************************************************************  
2. * CopyRight (c) HYTC Ltd. All rights reserved.  
3. * Filename:  main.c  
4. * Creator:   GaoLei  
5. * Version:   0.0  
6. * Date:      2011-06-15  
7. * QQ:        38929568  
8. * Description: A*寻路算法 测试类  
9. *******************************************************************************/   
10. #include <stdlib.h>   
11. #include <stdio.h>   
12. #include <math.h>   
13. #define FALSE   0   
14. #define TRUE    1   
15. #define NULL    0   
16. typedef int BOOL;   
17.    
18. int map[20][20] =    
19. {   
20.     { 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },   
21.     { 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 0 },   
22.     { 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },   
23.     { 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
24.     { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
25.     { 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
26.     { 0, 0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
27.     { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
28.     { 0, 0, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
29.     { 0, 0, 0, 1, 0, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
30.     { 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
31.     { 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0 },   
32.     { 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0 },   
33.     { 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0 },   
34.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0 },   
35.     { 0, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0 },   
36.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },   
37.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 },   
38.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0 },   
39.     { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }   
40. };   
41.     typedef struct Node   
42.     {//节点结构体   
43.         int f,g,h;   
44.         int row;    //该节点所在行   
45.         int col;    //该节点所在列   
46.         int direction;//parent节点要移动的方向就能到达本节点   
47.         struct Node * parent;   
48.     }Node, *Lnode;   
49.        
50.     typedef struct Stack   
51.     {//OPEN CLOSED 表结构体   
52.         Node * npoint;   
53.         struct Stack * next;   
54.     }Stack, *Lstack;   
55.     int rows = 20;          //地图行数   
56.     int cols = 20;          //地图列数   
57.     int G_OFFSET = 1;       //每个图块G值的增加值   
58.     int destinationRow;     //目标所在行   
59.     int destinationCol;     //目标所在列   
60.     int canMoveIndex = 0;   //可以通行的地图图块索引   
61.     int tileSize = 1;       //图块大小   
62.        
63.     Lstack Open = NULL;   
64.     Lstack Closed = NULL;   
65.     Node * getNodeFromOpen()   
66.     {//选取OPEN表上f值最小的节点，返回该节点地址   
67.         Lstack temp = Open->next,min = Open->next,minp = Open;   
68.         Node * minx;   
69.         if( temp == NULL )   
70.             return NULL;   
71.            
72.         while(temp->next != NULL)   
73.         {   
74.             if( (temp->next ->npoint->f) < (min->npoint->f) )   
75.             {   
76.                 min = temp->next;   
77.                 minp = temp;   
78.             }   
79.             temp = temp->next;   
80.         }   
81.         minx = min->npoint;   
82.         temp = minp->next;   
83.         minp->next = minp->next->next;   
84.         free(temp);   
85.         return minx;   
86.     }   
87.        
88.     BOOL Equal(Node * suc,Node * goal)   
89.     {//判断节点是否相等，相等，不相等   
90.         if ( (suc->row == goal->row) && (suc->col == goal->col)  )   
91.         {   
92.             return TRUE;   
93.         }      
94.         else   
95.         {   
96.             return FALSE;   
97.         }   
98.     }   
99.        
100.     Node * BelongInOpen( Node * suc )   
101.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
102.         Lstack temp = Open -> next ;   
103.         if(temp == NULL)   
104.             return NULL;   
105.         while( temp != NULL )   
106.         {   
107.             if( Equal(suc,temp->npoint) )   
108.             {   
109.                 return temp -> npoint;   
110.             }   
111.             else   
112.             {   
113.                 temp = temp->next;      
114.             }   
115.         }   
116.         return NULL;   
117.     }   
118.        
119.     Node * BelongInClosed( Node * suc )   
120.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
121.         Lstack temp = Closed -> next ;   
122.         if(temp == NULL)   
123.             return NULL;   
124.         while(temp != NULL)   
125.         {   
126.             if( Equal(suc,temp->npoint) )   
127.             {   
128.                 return temp -> npoint;   
129.             }   
130.             else   
131.             {   
132.                 temp = temp->next;      
133.             }   
134.         }   
135.         return NULL;   
136.     }   
137.        
138.     void PutintoOpen(Node * suc )   
139.     {//把节点放入OPEN 或CLOSED 表中   
140.         Stack * temp;   
141.         temp =(Stack *) malloc(sizeof(Stack));   
142.         temp->npoint = suc;   
143.        
144.         temp->next = Open->next;   
145.         Open->next = temp;   
146.     }   
147.     void PutintoClosed(Node * suc )   
148.     {//把节点放入OPEN 或CLOSED 表中   
149.         Stack * temp;   
150.         temp =(Stack *) malloc(sizeof(Stack));   
151.         temp->npoint = suc;   
152.         temp->next = Closed->next;   
153.         Closed->next = temp;   
154.     }   
155.        
156.     //得到该图块的H值   
157.     int getH(int row, int col)   
158.     {   
159.         return (abs(destinationRow - row) + abs(destinationCol - col));   
160.     }   
161.        
162.     //得到该位置所在地图行   
163.     int getRowPosition(int y)   
164.     {   
165.         return (y / tileSize);   
166.     }   
167.        
168.     //得到该位置所在地图列   
169.     int getColPosition(int x)   
170.     {   
171.         return (x / tileSize);   
172.     }   
173.     //检测该图块是否可通行   
174.     BOOL isCanMove(int col, int row)   
175.     {   
176.         if(col < 0 || col >= cols)   
177.             return FALSE;   
178.         if(row < 0 || row >= rows)   
179.             return FALSE;   
180.         return map[col][row] == canMoveIndex;   
181.     }   
182.        
183.     Node* checkOpen(int row, int col)   
184.     {   
185.         Lstack temp = Open -> next;   
186.         if ( temp == NULL )   
187.             return NULL;   
188.         while (temp != NULL)    
189.         {   
190.             if ( (temp->npoint->row==row) && (temp->npoint->col == col)  )   
191.             {   
192.                 return temp -> npoint;   
193.             }      
194.             else   
195.             {   
196.                 temp = temp->next;   
197.             }   
198.         }   
199.         return NULL;   
200.     }   
201.        
202.     BOOL isInClose(int row, int col)   
203.     {   
204.         Lstack temp = Closed -> next;   
205.         if ( temp == NULL )   
206.             return FALSE;   
207.         while (temp != NULL)    
208.         {   
209.             if ( (temp->npoint->row==row) && (temp->npoint->col == col)  )   
210.             {   
211.                 return TRUE;   
212.             }      
213.             else   
214.             {   
215.                 temp = temp->next;   
216.             }   
217.         }   
218.         return FALSE;   
219.     }   
220.     int directionIndex =0;   
221.     int direction[256];   
222.     void creatSeccessionNode(Node *bestNode, int row, int col)   
223.     {   
224.         int g = bestNode->g + G_OFFSET;   
225.         if(!isInClose(row, col))   
226.         {   
227.             Node *oldNode = NULL;   
228.             if((oldNode = checkOpen(row, col)) != NULL)   
229.             {   
230.                 if(oldNode->g < g)   
231.                 {   
232.                     oldNode->parent = bestNode;   
233.                     oldNode->g = g;   
234.                     oldNode->f = g + oldNode->h;   
235.                 }   
236.             }   
237.             else   
238.             {   
239.                 Node *node = (Node *) malloc(sizeof(Node));   
240.                 node->parent = bestNode;   
241.                 node->g = g;   
242.                 node->h = getH(row, col);   
243.                 node->f = node->g + node->h;   
244.                 node->row = row;   
245.                 node->col = col;   
246.                 directionIndex++;   
247.                 node->direction = directionIndex;   
248. //              openNode.addElement(node);   
249.                 PutintoOpen( node );   
250.             }   
251.         }   
252.     }   
253.        
254.     /**  
255.      * 根据传入的节点生成子节点  
256.      * @param bestNode  
257.      * @param destinationRow  
258.      * @param destinationCol  
259.      */   
260.     void seachSeccessionNode(Node *bestNode)   
261.     {   
262.         int row, col;   
263. //      Node *bestNodeInOpen = NULL;   
264.         //上部节点   
265.         if(isCanMove(row = bestNode->row - 1, col = bestNode->col))   
266.         {   
267.             creatSeccessionNode(bestNode, row, col);   
268.         }   
269.         //下部节点   
270.         if(isCanMove(row = bestNode->row + 1, col = bestNode->col))   
271.         {   
272.             creatSeccessionNode(bestNode, row, col);   
273.         }   
274.         //左部节点   
275.         if(isCanMove(row = bestNode->row, col = bestNode->col - 1))   
276.         {   
277.             creatSeccessionNode(bestNode, row, col);   
278.         }   
279.         //右部节点   
280.         if(isCanMove(row = bestNode->row, col = bestNode->col + 1))   
281.         {   
282.             creatSeccessionNode(bestNode, row, col);   
283.         }   
284.         PutintoClosed( bestNode );   
285.     }   
286.     //正序遍历链表   
287.     void printfOpenData()   
288.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
289.         Lstack temp = Open -> next;   
290.         Node *p_node;   
291.         if(temp == NULL)   
292.             return;   
293.         while(temp != NULL)   
294.         {   
295.             Lstack head = temp;   
296.             temp = temp->next;   
297.             p_node = head->npoint;   
298.             printf("Open库数据！[%d,%d]\n", p_node->col, p_node->row );   
299.             free(p_node);   
300.             free( head );   
301.             Open->next = temp;   
302.         }   
303.         printf("\n Open库数据 数据全部清楚 \n");   
304.         return;   
305.     }   
306.     void printfClosedData()   
307.     {//判断节点是否属于OPEN表或CLOSED表，是则返回节点地址，否则返回空地址   
308.         Lstack temp = Closed -> next ;   
309.         Node *p_node;   
310.         if(temp == NULL)   
311.             return;   
312.         while(temp != NULL)   
313.         {   
314.             Lstack head = temp;   
315.             temp = temp->next;   
316.             p_node = head->npoint;   
317.             printf("Closed库数据！[%d,%d]\n", p_node->col, p_node->row );   
318.             free(p_node);   
319.             free( head );   
320.             Closed -> next = temp;   
321.         }   
322.         printf("\n Closed库数据 数据全部清楚 \n");   
323.         /*  
324.         temp = Closed -> next;  
325.         while(temp != NULL)  
326.         {  
327.             printf("Closed库数据！节点");  
328.             temp = temp->next;  
329.         }*/   
330.         return;   
331.     }   
332. void getPath(int startX, int StartY, int destinationX, int destinationY)   
333. {   
334.     Node *startNode = (Node *) malloc(sizeof(Node));   
335.     Node *bestNode  = NULL;   
336.     int index = 0;   
337.     destinationRow = getRowPosition(destinationY);   
338.     destinationCol = getColPosition(destinationX);   
339.        
340.     startNode->parent= NULL;   
341.     startNode->row = getRowPosition(StartY);   
342.     startNode->col = getColPosition(startX);   
343.     startNode->g = 0;   
344.     startNode->h = getH( startNode->row, startNode->col );   
345.     startNode->f = startNode->g + startNode->h;   
346.     startNode->direction = 0;   
347.     PutintoOpen( startNode );// openNode.add(startNode);   
348.        
349.     while(TRUE)   
350.     {   
351.         bestNode = getNodeFromOpen(); //从OPEN表中取出f值最小的节点   
352.         if(bestNode == NULL)//未找到路径   
353.         {   
354.             printf("未找到路径\n");   
355.             return;   
356.         }   
357.         else if(bestNode->row == destinationRow   
358.                 && bestNode->col == destinationCol )   
359.         {   
360.             Node *_Node = bestNode;   
361.             int nodeSum = 0;   
362.             int nodeIndex =0;   
363.             printf("程序运行次数=%d\n",index);   
364.             while( _Node->parent != NULL )   
365.             {   
366.                 printf("x:%d  y:%d  direction = %d \n", _Node->col, _Node->row, _Node->direction );   
367.                 _Node = _Node->parent;   
368.                 nodeSum += 1;   
369.             }   
370.             printf("节点数量=%d\n",nodeSum);   
371.             _Node = bestNode;   
372.             nodeIndex = nodeSum-1;   
373.             while( _Node->parent != NULL && nodeIndex>=0)   
374.             {   
375.                 Node *_NodeParent = _Node->parent;   
376.                 printf("x:%d  y:%d  direction = %d \n", _Node->col, _Node->row, _Node->direction );   
377.                 if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == +1 )   
378.                 {//从父节点到本节点的操作是  上   
379.                     direction[nodeIndex] = 1;   
380.                 }   
381.                 else if( _NodeParent->col - _Node->col == 0 && _NodeParent->row - _Node->row == -1 )   
382.                 {//从父节点到本节点的操作是  下   
383.                     direction[nodeIndex] = 2;   
384.                 }   
385.                 else if( _NodeParent->col - _Node->col == +1 && _NodeParent->row - _Node->row == 0 )   
386.                 {//从父节点到本节点的操作是  左   
387.                     direction[nodeIndex] = 3;   
388.                 }   
389.                 else if( _NodeParent->col - _Node->col == -1 && _NodeParent->row - _Node->row == 0 )   
390.                 {//从父节点到本节点的操作是  右   
391.                     direction[nodeIndex] = 4;   
392.                 }   
393.                 else   
394.                 {   
395.                     direction[nodeIndex] = 0;   
396.                 }   
397.                 nodeIndex -= 1;   
398.                 _Node = _Node->parent;   
399.             }   
400.             for( nodeIndex=0; nodeIndex<nodeSum; nodeIndex++ )   
401.             {   
402.                 printf("direction[%d]=%d\n",nodeIndex,direction[nodeIndex]);   
403.             }   
404.             return ;   
405.         }   
406.         index++;   
407.         seachSeccessionNode(bestNode);   
408.     }   
409. }   
410. void main()   
411. {//主函数   
412.     //初始操作，建立open和closed表   
413.     Open = (Stack *) malloc(sizeof(Stack));   
414.     Open->next = NULL;   
415.     Closed = (Stack *) malloc(sizeof(Stack));   
416.     Closed->next = NULL;   
417. //-----------------------------------   
418.     getPath( 0, 0, 19, 19 );   
419.     printf("程序认定该起始状态无法道达目标状态!\n");   
420.        
421.     printfOpenData();   
422.     printfClosedData();   
423.     free(Open);   
424.     free(Closed);   
425. //--------------------------------------   
426. }   

 

  

 

本程序很容易修改成您工程需要的形式，比如

 

我的使用方式是这样的

1. //将main() 方法改成   
2. void autoPathfinding( uint16 *MD, uint8 map_w, uint8 map_h, int16 startX, int16 startY, int16 destinationX, int16 destinationY )   

 

 

这样的目的是 将地图数据 以及当前位置，和目标位置传递个 自动寻路方法

 

还需要修改 能否通过的方法，可以根据自己的地图来做处理

1. //检测该图块是否可通行   
2. BOOL isCanMove(int col, int row)   
3. {   
4.     if(col < 0 || col >= cols)   
5.         return FALSE;   
6.     if(row < 0 || row >= rows)   
7.         return FALSE;   
8.     //return map[col][row] == canMoveIndex;   
9.     return isCanGoByMapTile( MapData[ (col)*rows+(row) ] );   
10. }   
11.   

 

PS: 看源代码的时候，如果你觉得有些变量定义的位置不舒服，是因为我的嵌入式C所用的环境要求，当然您可以根据您的环境自由修改！

 

时间关系，并没有特别优化，如果您有好的优化方案，我愿意聆听！

 

转载于:https://blog.51cto.com/kome2000/590579