寻路算法之A*算法

 1 typedef struct node
 2 {
 3 float currentCost;//h(n)
 4 float toTalCost;//f(n)
 5 node * parent;//父节点
 6 }Node;
 7 Vector<Node>  OpenList;//开启列表
 8 Vector<Node>  CloseList;//关闭列表
 9 
10 Node  AStar(Node start,Node Goal){
11   OpenList.Add(start);
12   while(OpenList.Length!=0)
13 {
14       Node* minNode=  OpenLIst.Min();//Min()选出最小消耗成本节点
15       if(minNode == Goal)
16 {
17 return minNode;
18 }
19        Vector<Node> Neighbors;//邻居列表
20          GetNeighBors(Neighbors,minNode）;/*得到最小消耗成本节点的周围邻居节点*/
21            for(int i=0;i<NeighBors.Count;i++)
22 {
23          if(Neighbors[i].obstacle)//如果邻居节点被标志障碍物,跳过。
24 {
25 continue;/*也可以在GetNeighBors函数里进行处理,障碍物节点不加入邻居列表*/
26 }
27 if(CloseList.Contain(Neighbors[i]))//节点已经走过不作处理
28 continue;
29 float currentCost=CacluateDistance(minNode,Neighbors[i])+minNode.currentCost;
30 float totalCost = currentCost+CacluateDistance(Neighbors[i],Goal);
31 if(OpenList.Cotain(Neighbors[i]))//如果开启列表包含该邻居节点
32 {
33 
34 if(totalCost<Neighbors[i].totalCost)
35 {
36 Neighbors[i].totalCost=totalCost;
37 Neighbors[i].currentCost =currentCost;
38 Neighbors[i].parent=minNode;
39 }
40 }
41 else
42 {
43 Neighbors[i].totalCost=totalCost;
44 Neighbors[i].currentCost =currentCost;
45 Neighbors[i].parent=minNode;
46 }
47 }
48 OpenList.Remove(minNode);
49 CloseList.Add(minNode);
50  }
51 return NULL;//找不到目标节点，返回空
52 }