A星寻路算法最简单理解

对于a星寻路算法最直白的理解：

从a点走到b点，首先把地图画成网格，让障碍物在网格内

      如图，从s点要走到e点，把障碍物设成黑色，还要创建2个队列，一个是寻找新的节点队列（开启队列），一个是保存已走过的节点队列（关闭队列）。在寻找新的节点时，要判断该节点距离，公式为总距离=当前节点距离起点步数+当前节点距离终点步数。这里注意的是即使有障碍物在计算当前节点距离起终点步数也要按照没有障碍物来算。

       拿该图来说，从起点s走到终点e（假设可以斜着走且斜走距离为1）。图上所有open，close的标识是走完后的，所以先不看，从s点出发，把s点加入开启队列，进入第一次循环，条件是如果开启队列不为空，找到开启队列中总距离最小点（只有一个点s），则找到s点旁边可走的点（相邻s点且不在open队列，不在close队列），可以找到3,5,8,9,10点，将这些点移入open队列，计算每点总距离，将s点移除open队列，s点移入close队列，进入第二次循环，当open队列不为空，找到open队列总距离最小点，可以看到总距离最小点有3个，8,9,10并且都是4（4=1+3这个1指的是该点距离起点距离3点是该点距离终点距离），假设第一个点是8，那么找到8点的旁边的可走点，由于5，9点在开启队列，4点在关闭队列，所以找到6,7点移入开启队列并计算总距离，把8移入关闭队列

      此时，开启队列{3,10,9,5,6,7}  关闭队列{4,8}

      进入第三次循环，开启队列不为空时，找到开启队列中总距离最小的点，这里有两个9,10总距离为4，假设是9，找邻点，所有邻点要么在开启队列要么在关闭队列，所以9移入关闭队列，

     进入第四次循环，找到10点，找邻点2,11,14，此时开启队列{2,3,5,6,7,11,14}  关闭队列{4,8,9,10}

     第五次循环，找到5点，移入关闭，开启队列{2,3,6,7,11,14}  关闭队列{4,5,8,9,10}

     第六次开启队列{2,6,7,11,14}  关闭队列{3,4,5,8,9,10}

     第七次开启队列{2,6,11,14}  关闭队列{3,4,5,7,8,9,10}

     第八次开启队列{2,11,14}  关闭队列{3,4,5,6,7,8,9,10}

     不说啦，可能有点错误，你们明白意思就行

     最后如何找到最佳路径呢，每次在开启队列找到的点选取邻点时候，都要将邻点的父节点选为该点，图中用箭头表示，这样当第n次循环，当从开启队列找到的第一个点为终点时，回溯父节点就找到了最短路径。

    贴出来一张c#代码供参考：

[csharp] view plain copy

1. using UnityEngine;  
2. using System.Collections;  
3.   
4. public class AStar {  
5.   
6.     public static PriorityQueue closedList, openList;   //开启关闭队列  
7.   
8.     private static float NodeCost(Node a, Node b)    //已知a，b点，计算两点距离  
9.     {  
10.         Vector3 vecCost = a.position - b.position;  
11.         return vecCost.magnitude;  
12.     }  
13.   
14.     public static ArrayList FindPath(Node start, Node goal)    //a星寻路算法  
15.     {  
16.         openList = new PriorityQueue();  
17.         openList.Push(start);                     //首先将起点压入队列，计算该点距起点距离和该点距终点距离  
18.         start.nodeTotalCost = 0.0f;  
19.         start.estimatedCost = NodeCost(start, goal);  
20.   
21.         closedList = new PriorityQueue();  
22.   
23.         Node node = null;  
24.   
25.         while (openList.Length != 0)             //当开启队列不为空，进入正式循环  
26.         {  
27.             node = openList.First();             //找到开启队列中总距离最小的点  
28.             if (node.position == goal.position)   //如果开启队列中总距离最小的点是终点就退出循环  
29.             {  
30.                 return CalculatePath(node);  
31.             }  
32.   
33.             ArrayList neighbours = new ArrayList();   
34.             GridManager.instance.GetNeighbours(node, neighbours);     //寻找该点的所有邻点用neighbours保存，这个函数是调用另一个脚本  
35.             for (int i = 0; i < neighbours.Count; i++)  
36.             {  
37.                 Node neighbourNode = (Node)neighbours[i];  
38.                 if(!closedList.Contains(neighbourNode))             //如果该邻点不在关闭队列中就计算该点总距离  
39.                 {  
40.                     float cost = NodeCost(node, neighbourNode);  
41.                     float totalCost = node.nodeTotalCost + cost;  
42.   
43.                     float neighbourNodeEstCost = NodeCost(neighbourNode, goal);  
44.   
45.                     neighbourNode.nodeTotalCost = totalCost;  
46.                     neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost;  
47.                     neighbourNode.parent = node;  
48.   
49.                     if (!openList.Contains(neighbourNode))        //邻点不在开启队列就压入开启队列  
50.                     {  
51.                         openList.Push(neighbourNode);  
52.                     }  
53.                 }  
54.             }  
55.             closedList.Push(node);                       //将该点从开启队列移除，移入关闭队列  
56.             openList.Remove(node);  
57.         }  
58.   
59.         if (node.position != goal.position)                //如果开启队列全部点都遍历了没找到目标点，报错  
60.         {  
61.             Debug.LogError("Goal Not Found");  
62.             return null;  
63.         }  
64.         return CalculatePath(node);                        //该方法返回最佳路径  
65.     }  
66.   
67.     private static ArrayList CalculatePath(Node node)         //找到从终点的所有父节点也即终点到起点路径，将list倒过来  
68.     {  
69.         ArrayList list = new ArrayList();  
70.         while (node != null)  
71.         {  
72.             list.Add(node);  
73.             node = node.parent;  
74.         }  
75.         list.Reverse();  
76.         return list;  
77.     }  
78. }  

避障算法：

                              

对于一个物体要想通过障碍物需要避障算法，算法思想，从坦克前方做射线和障碍物的焦点处做法线，让坦克的新方向为（前方+法线方向*力的大小），这样坦克向左走，当射线碰不到障碍物的时候就没有法线，就不会转向了

代码中将障碍物的layer层射到第8层为Obstacles层

参考代码

[csharp] view plain copy

1. public class AvoidingObstacles : MonoBehaviour {  
2.   
3.     public float speed = 20.0f;  
4.     public float mass = 5.0f;  
5.     public float force = 50.0f;  
6.     public float minimunDistToAvoid = 20.0f;  
7.   
8.     private float curSpeed;  
9.     private Vector3 targetPoint;  
10.   
11.     // Use this for initialization  
12.     void Start () {  
13.         targetPoint = Vector3.zero;  
14.     }  
15.   
16.     void OnGUI()  
17.     {  
18.         GUILayout.Label("Click anywhere to move the vehicle.");  
19.     }  
20.   
21.     // Update is called once per frame  
22.     void Update () {  
23.         RaycastHit hit;  
24.         //找到目标点  
25.         var ray = Camera.main.ScreenPointToRay(Input.mousePosition);  
26.   
27.         if (Input.GetMouseButton(0) && Physics.Raycast(ray, out hit, 100.0f))   //发射100单位长度射线  
28.         {  
29.             targetPoint = hit.point;  
30.         }  
31.         //找到坦克的目标移动方向，并得到方向向量  
32.         Vector3 dir = targetPoint - transform.position;  
33.         dir.Normalize();  
34.   
35.         AvoidObstacles(ref dir);    //执行避障算法  
36.   
37.         if (Vector3.Distance(targetPoint, transform.position) < 3.0f)  
38.             return;  
39.   
40.         curSpeed = speed * Time.deltaTime;  
41.         var rot = Quaternion.LookRotation(dir);  
42.         //坦克一直朝着目标方向旋转  
43.         transform.rotation = Quaternion.Slerp(transform.rotation, rot, 5.0f * Time.deltaTime);     
44.         transform.position += transform.forward * curSpeed;  
45.     }  
46.   
47.     private void AvoidObstacles(ref Vector3 dir)    //在避障算法中传入移动方向并修改移动方向  
48.     {  
49.         RaycastHit hit;  
50.   
51.         // 0000 0000 0000 0000 0000 0001 0000 0000   
52.         //                             1  
53.         int layerMask = 1 << 8;         //因为把障碍物层设成第8层所以这样  
54.        //从坦克点的前方发射一条长度为minimunDistToAvoid=20单位长度的射线，与layer为第8层的物体相交  
55.         if (Physics.Raycast(transform.position, transform.forward,                
56.             out hit, minimunDistToAvoid, layerMask))  
57.         {  
58.             Vector3 hitNormal = hit.normal;  
59.             hitNormal.y = 0.0f;  
60.             dir = transform.forward + hitNormal * force;  
61.         }  
62.     }  
63. }  

Flocking算法：

用于群体物体追踪一个领头物体，比如gta5警车追人，红警派出一群小兵去哪里，如图绿色的物体一直在追踪红色物体移动，把红色物体的移动代码写好，在hierarchy中把绿色物体拖动到红色物体下面，给绿色物体添加flock代码即可

红色物体代码：比较简单就不注释了：

[csharp] view plain copy

1. public class FlockControl : MonoBehaviour {  
2.   
3.     public float speed = 100.0f;  
4.     public Vector3 bound;  
5.   
6.     private Vector3 initialPosition;  
7.     private Vector3 nextMovementPoint;  
8.   
9.     // Use this for initialization  
10.     void Start () {  
11.         initialPosition = transform.position;  
12.         CalculateNextMovementPoint();  
13.     }  
14.   
15.     private void CalculateNextMovementPoint()  
16.     {  
17.         float posX = Random.Range(-bound.x, bound.x);  
18.         float posY = Random.Range(-bound.y, bound.y);  
19.         float posZ = Random.Range(-bound.z, bound.z);  
20.   
21.         nextMovementPoint = initialPosition + new Vector3(posX, posY, posZ);  
22.     }  
23.       
24.     // Update is called once per frame  
25.     void Update () {  
26.         transform.Translate(Vector3.forward * speed * Time.deltaTime);  
27.         transform.rotation = Quaternion.Slerp(transform.rotation, Quaternion.LookRotation(nextMovementPoint - transform.position), 2.0f * Time.deltaTime);  
28.         if (Vector3.Distance(nextMovementPoint,transform.position) <= 20.0f)  
29.             CalculateNextMovementPoint();  
30.     }  
31. }  

绿色物体代码，比较复杂，只管用就行，不用跟我客气：

[csharp] view plain copy

1. public class Flock : MonoBehaviour   
2. {  
3.     public float minSpeed = 100.0f;         //movement speed of the flock  
4.     public float turnSpeed = 20.0f;         //rotation speed of the flock  
5.     public float randomFreq = 20.0f;          
6.   
7.     public float randomForce = 20.0f;       //Force strength in the unit sphere  
8.     public float toOriginForce = 20.0f;       
9.     public float toOriginRange = 100.0f;  
10.   
11.     public float gravity = 2.0f;            //Gravity of the flock  
12.   
13.     public float avoidanceRadius = 400.0f;  //Minimum distance between flocks  
14.     public float avoidanceForce = 20.0f;  
15.   
16.     public float followVelocity = 4.0f;  
17.     public float followRadius = 40.0f;      //Minimum Follow distance to the leader  
18.   
19.     private Transform origin;               //Parent transform  
20.     private Vector3 velocity;               //Velocity of the flock  
21.     private Vector3 normalizedVelocity;  
22.     private Vector3 randomPush;             //Random push value  
23.     private Vector3 originPush;  
24.     private Transform[] objects;            //Flock objects in the group  
25.     private Flock[] otherFlocks;       //Unity Flocks in the group  
26.     private Transform transformComponent;   //My transform  
27.   
28.     void Start ()  
29.     {  
30.         randomFreq = 1.0f / randomFreq;  
31.   
32.         //Assign the parent as origin  
33.         origin = transform.parent;     
34.           
35.         //Flock transform             
36.         transformComponent = transform;  
37.   
38.         //Temporary components  
39.         Component[] tempFlocks= null;  
40.   
41.         //Get all the unity flock components from the parent transform in the group  
42.         if (transform.parent)  
43.         {  
44.             tempFlocks = transform.parent.GetComponentsInChildren<Flock>();  
45.         }  
46.   
47.         //Assign and store all the flock objects in this group  
48.         objects = new Transform[tempFlocks.Length];  
49.         otherFlocks = new Flock[tempFlocks.Length];  
50.   
51.         for(int i = 0;i<tempFlocks.Length;i++)  
52.         {  
53.             objects[i] = tempFlocks[i].transform;  
54.             otherFlocks[i] = (Flock)tempFlocks[i];  
55.         }  
56.   
57.         //Null Parent as the flock leader will be UnityFlockController object  
58.         transform.parent = null;  
59.   
60.         //Calculate random push depends on the random frequency provided  
61.         StartCoroutine(UpdateRandom());  
62.     }  
63.   
64.     IEnumerator UpdateRandom ()  
65.     {  
66.         while(true)  
67.         {  
68.             randomPush = Random.insideUnitSphere * randomForce;  
69.             yield return new WaitForSeconds(randomFreq + Random.Range(-randomFreq / 2.0f, randomFreq / 2.0f));  
70.         }  
71.     }  
72.   
73.     void Update ()  
74.     {   
75.         //Internal variables  
76.         float speed= velocity.magnitude;  
77.         Vector3 avgVelocity = Vector3.zero;  
78.         Vector3 avgPosition = Vector3.zero;  
79.         float count = 0;  
80.         float f = 0.0f;  
81.         float d = 0.0f;  
82.         Vector3 myPosition = transformComponent.position;  
83.         Vector3 forceV;  
84.         Vector3 toAvg;  
85.         Vector3 wantedVel;  
86.   
87.         for(int i = 0;i<objects.Length;i++)  
88.         {  
89.             Transform transform= objects[i];  
90.             if (transform != transformComponent)  
91.             {  
92.                 Vector3 otherPosition = transform.position;  
93.   
94.                 // Average position to calculate cohesion  
95.                 avgPosition += otherPosition;  
96.                 count++;  
97.   
98.                 //Directional vector from other flock to this flock  
99.                 forceV = myPosition - otherPosition;  
100.   
101.                 //Magnitude of that directional vector(Length)  
102.                 d= forceV.magnitude;  
103.   
104.                 //Add push value if the magnitude is less than follow radius to the leader  
105.                 if (d < followRadius)  
106.                 {  
107.                     //calculate the velocity based on the avoidance distance between flocks   
108.                     //if the current magnitude is less than the specified avoidance radius  
109.                     if(d < avoidanceRadius)  
110.                     {  
111.                         f = 1.0f - (d / avoidanceRadius);  
112.   
113.                         if(d > 0)   
114.                             avgVelocity += (forceV / d) * f * avoidanceForce;  
115.                     }  
116.                       
117.                     //just keep the current distance with the leader  
118.                     f = d / followRadius;  
119.                     Flock tempOtherFlock = otherFlocks[i];  
120.                     avgVelocity += tempOtherFlock.normalizedVelocity * f * followVelocity;    
121.                 }  
122.             }     
123.         }  
124.           
125.         if(count > 0)  
126.         {  
127.             //Calculate the average flock velocity(Alignment)  
128.             avgVelocity /= count;  
129.   
130.             //Calculate Center value of the flock(Cohesion)  
131.             toAvg = (avgPosition / count) - myPosition;   
132.         }     
133.         else  
134.         {  
135.             toAvg = Vector3.zero;         
136.         }  
137.           
138.         //Directional Vector to the leader  
139.         forceV = origin.position -  myPosition;  
140.         d = forceV.magnitude;     
141.         f = d / toOriginRange;  
142.   
143.         //Calculate the velocity of the flock to the leader  
144.         if(d > 0)   
145.             originPush = (forceV / d) * f * toOriginForce;  
146.           
147.         if(speed < minSpeed && speed > 0)  
148.         {  
149.             velocity = (velocity / speed) * minSpeed;  
150.         }  
151.           
152.         wantedVel = velocity;  
153.           
154.         //Calculate final velocity  
155.         wantedVel -= wantedVel *  Time.deltaTime;     
156.         wantedVel += randomPush * Time.deltaTime;  
157.         wantedVel += originPush * Time.deltaTime;  
158.         wantedVel += avgVelocity * Time.deltaTime;  
159.         wantedVel += toAvg.normalized * gravity * Time.deltaTime;  
160.   
161.         //Final Velocity to rotate the flock into  
162.         velocity = Vector3.RotateTowards(velocity, wantedVel, turnSpeed * Time.deltaTime, 100.00f);  
163.         transformComponent.rotation = Quaternion.LookRotation(velocity);  
164.           
165.         //Move the flock based on the calculated velocity  
166.         transformComponent.Translate(velocity * Time.deltaTime, Space.World);  
167.   
168.         //normalise the velocity  
169.         normalizedVelocity = velocity.normalized;  
170.     }  
171. }