[Unity算法]A星寻路：基础版本

Posted 2020-11-07 lyh916

参考链接：

https://www.cnblogs.com/yangyxd/articles/5447889.html

 

一.原理

1.将场景简化，分割为一个个正方形格子，这些格子称之为节点(node)，从一个节点到另一个节点的距离称之为代价(cost)。一个节点与水平/垂直方向的相邻节点的代价是1，与对角节点的代价是1.4。这里引用公式f = g + h，f表示该节点的总代价，g表示该节点与上一路径节点的代价，h表示该节点与目标节点的代价。

2.需要两个列表，开启列表(openList)和关闭列表(closeList)。开启列表用来记录需要考虑的节点，关闭列表用来记录不会再考虑的节点。

3.在开启列表中添加起始节点。

4.在开启列表中找到总代价最低的节点，然后从开启列表中移除该节点，从关闭列表中添加该节点，把与该节点相邻的可通行的节点添加到开启列表，并且更新这些节点的代价。

5.循环第四步，如果当前节点等于目标节点，则退出循环。

 

二.实现

FindWayNode.cs

 1 using UnityEngine;
 2 
 3 public class FindWayNode {
 4 
 5     public bool isObstacle;//是否是障碍物
 6     public Vector3 scenePos;//场景位置
 7     public int x, y;//坐标
 8 
 9     public int gCost;//与起始点的距离
10     public int hCost;//与目标点的距离
11     public int fCost {
12         get { return gCost + hCost; }
13     }//总距离
14 
15     public FindWayNode parentNode;//父节点
16 
17     public FindWayNode(bool isObstacle, Vector3 scenePos, int x, int y)
18     {
19         this.isObstacle = isObstacle;
20         this.scenePos = scenePos;
21         this.x = x;
22         this.y = y;
23     }
24 }

FindWayGrid.cs

  1 using System.Collections.Generic;
  2 using UnityEngine;
  3 
  4 public class FindWayGrid {
  5 
  6     public int width;//格子水平方向个数
  7     public int height;//格子垂直方向个数
  8     public float nodeLength;//格子长度
  9 
 10     private FindWayNode[,] findWayNodes;//格子数组
 11     private float halfNodeLength;//格子长度的一半
 12     private Vector3 startPos;//场景坐标起点
 13 
 14     public FindWayGrid(int width, int height, float nodeLength = 1f)
 15     {
 16         this.width = width;
 17         this.height = height;
 18         this.nodeLength = nodeLength;
 19 
 20         findWayNodes = new FindWayNode[width, height];
 21         halfNodeLength = nodeLength / 2;
 22         startPos = new Vector3(-width / 2 * nodeLength + halfNodeLength, 0, -height / 2 * nodeLength + halfNodeLength);
 23 
 24         for (int x = 0; x < width; x++)
 25         {
 26             for (int y = 0; y < height; y++)
 27             {
 28                 Vector3 pos = CoordinateToScenePos(x, y);
 29                 findWayNodes[x, y] = new FindWayNode(false, pos, x, y);
 30             }
 31         }
 32     }
 33 
 34     //坐标转场景坐标
 35     public Vector3 CoordinateToScenePos(int x, int y)
 36     {
 37         Vector3 pos = new Vector3(startPos.x + x * nodeLength, startPos.y, startPos.z + y * nodeLength);
 38         return pos;
 39     }
 40 
 41     //根据场景坐标获取节点
 42     public FindWayNode GetNode(Vector3 pos)
 43     {
 44         int x = (int)(Mathf.RoundToInt(pos.x - startPos.x) / nodeLength);
 45         int y = (int)(Mathf.RoundToInt(pos.z - startPos.z) / nodeLength);
 46         x = Mathf.Clamp(x, 0, width - 1);
 47         y = Mathf.Clamp(y, 0, height - 1);
 48         return GetNode(x, y);
 49     }
 50 
 51     //根据坐标获取节点
 52     public FindWayNode GetNode(int x, int y)
 53     {
 54         return findWayNodes[x, y];
 55     }
 56 
 57     //获取相邻节点列表
 58     public List<FindWayNode> GetNearbyNodeList(FindWayNode node)
 59     {
 60         List<FindWayNode> list = new List<FindWayNode>();
 61         for (int i = -1; i <= 1; i++)
 62         {
 63             for (int j = -1; j <= 1; j++)
 64             {
 65                 if (i == 0 && j == 0)
 66                 {
 67                     continue;
 68                 }
 69                 int x = node.x + i;
 70                 int y = node.y + j;
 71                 if (x >= 0 && x < width && y >= 0 && y < height)
 72                 {
 73                     list.Add(findWayNodes[x, y]);
 74                 }
 75             }
 76         }
 77         return list;
 78     }
 79 
 80     //获取两个节点之间的距离
 81     int GetDistance(FindWayNode nodeA, FindWayNode nodeB)
 82     {
 83         int countX = Mathf.Abs(nodeA.x - nodeB.x);
 84         int countY = Mathf.Abs(nodeA.y - nodeB.y);
 85         if (countX > countY)
 86         {
 87             return 14 * countY + 10 * (countX - countY);
 88         }
 89         else
 90         {
 91             return 14 * countX + 10 * (countY - countX);
 92         }
 93     }
 94 
 95     //找出起点到终点的最短路径
 96     public List<FindWayNode> FindWay(Vector3 startPos, Vector3 endPos)
 97     {
 98         FindWayNode startNode = GetNode(startPos);
 99         FindWayNode endNode = GetNode(endPos);
100 
101         List<FindWayNode> openList = new List<FindWayNode>();
102         List<FindWayNode> closeList = new List<FindWayNode>();
103         openList.Add(startNode);
104 
105         while (openList.Count > 0)
106         {
107             FindWayNode nowNode = openList[0];
108 
109             //选择花费最低的
110             for (int i = 0; i < openList.Count; i++)
111             {
112                 if (openList[i].fCost <= nowNode.fCost &&
113                     openList[i].hCost < nowNode.hCost)
114                 {
115                     nowNode = openList[i];
116                 }
117             }
118 
119             openList.Remove(nowNode);
120             closeList.Add(nowNode);
121 
122             //找到目标节点
123             if (nowNode == endNode)
124             {
125                 return GeneratePath(startNode, endNode);
126             }
127 
128             List<FindWayNode> nearbyNodeList = GetNearbyNodeList(nowNode);
129             for (int i = 0; i < nearbyNodeList.Count; i++)
130             {
131                 FindWayNode node = nearbyNodeList[i];
132                 //如果是墙或者已经在关闭列表中
133                 if (node.isObstacle || closeList.Contains(node))
134                 {
135                     continue;
136                 }
137                 //计算当前相邻节点与开始节点的距离
138                 int gCost = nowNode.gCost + GetDistance(nowNode, node);
139                 //如果距离更小，或者原来不在打开列表
140                 if (gCost < node.gCost || !openList.Contains(node))
141                 {
142                     //更新与开始节点的距离
143                     node.gCost = gCost;
144                     //更新与结束节点的距离
145                     node.hCost = GetDistance(node, endNode);
146                     //更新父节点为当前选定的节点
147                     node.parentNode = nowNode;
148                     //加入到打开列表
149                     if (!openList.Contains(node))
150                     {
151                         openList.Add(node);
152                     }
153                 }
154             }
155         }
156 
157         return null;
158     }
159 
160     //生成路径
161     public List<FindWayNode> GeneratePath(FindWayNode startNode, FindWayNode endNode)
162     {
163         List<FindWayNode> nodeList = new List<FindWayNode>();
164         if (endNode != null)
165         {
166             FindWayNode tempNode = endNode;
167             while (tempNode != startNode)
168             {
169                 nodeList.Add(tempNode);
170                 tempNode = tempNode.parentNode;
171             }
172             nodeList.Reverse();//反转路径
173         }
174         return nodeList;
175     }
176 }

TestFindWay.cs

 1 using UnityEngine;
 2 using System.Collections;
 3 using System.Collections.Generic;
 4 
 5 public class TestFindWay : MonoBehaviour {
 6     
 7     public Transform startTra;//起点tra
 8     public Transform endTra;//终点tra
 9     public Transform floorTra;//地板tra
10 
11     public GameObject obstacleGridPrefab;//障碍物格子
12     public GameObject pathGridPrefab;//路径格子
13     public LayerMask obstacleLayer;//障碍物所在的层
14     
15     private FindWayGrid findWayGrid;
16 
17     private GameObject obstacleRootGo;//障碍物格子的父go
18     private GameObject pathRootGo;//路径格子的父go
19     private List<GameObject> pathGridGoList;//路径格子go列表
20 
21     void Start ()
22     {
23         MeshFilter meshFilter = floorTra.GetComponent<MeshFilter>();
24         int width = Mathf.CeilToInt(meshFilter.mesh.bounds.size.x) * (int)floorTra.localScale.x;
25         int height = Mathf.CeilToInt(meshFilter.mesh.bounds.size.z) * (int)floorTra.localScale.z;
26 
27         findWayGrid = new FindWayGrid(width, height);
28 
29         obstacleRootGo = new GameObject("ObstacleRoot");
30         pathRootGo = new GameObject("PathRoot");
31         pathGridGoList = new List<GameObject>();
32 
33         ShowObstacle();
34     }
35     
36     void Update ()
37     {
38         List<FindWayNode> nodeList = findWayGrid.FindWay(startTra.position, endTra.position);
39         if (nodeList != null)
40         {
41             ShowPath(nodeList);
42         }
43     }
44 
45     //展示路径
46     public void ShowPath(List<FindWayNode> list)
47     {
48         for (int i = 0; i < list.Count; i++)
49         {
50             if (i < pathGridGoList.Count)
51             {
52                 pathGridGoList[i].transform.position = list[i].scenePos;
53                 pathGridGoList[i].SetActive(true);
54             }
55             else
56             {
57                 GameObject go = Instantiate(pathGridPrefab);
58                 go.transform.SetParent(pathRootGo.transform);
59                 go.transform.position = list[i].scenePos;
60                 pathGridGoList.Add(go);
61             }
62         }
63 
64         for (int i = list.Count; i < pathGridGoList.Count; i++)
65         {
66             pathGridGoList[i].SetActive(false);
67         }
68     }
69 
70     //展示障碍物
71     public void ShowObstacle()
72     {
73         int width = findWayGrid.width;
74         int height = findWayGrid.height;
75         float halfNodeLength = findWayGrid.nodeLength / 2;
76 
77         for (int x = 0; x < width; x++)
78         {
79             for (int y = 0; y < height; y++)
80             {
81                 FindWayNode node = findWayGrid.GetNode(x, y);
82                 bool isObstacle = Physics.CheckSphere(node.scenePos, halfNodeLength, obstacleLayer);
83                 if (isObstacle)
84                 {
85                     GameObject go = GameObject.Instantiate(obstacleGridPrefab, node.scenePos, Quaternion.identity) as GameObject;
86                     go.transform.SetParent(obstacleRootGo.transform);
87                 }
88                 node.isObstacle = isObstacle;
89             }
90         }
91     }
92 }

 

三.使用

把TestFindWay.cs挂上，然后对public变量进行拖拽赋值即可。效果如下：