Unity實現A*尋路算法學習1.0
一、A*尋路算法的原理
如果現在地圖上存在兩點A、B，這裡設A為起點，B為目標點(終點)
這裡為每一個地圖節點定義了三個值
gCost：距離起點的Cost(距離)
hCost：距離目標點的Cost(距離)
fCost：gCost和gCost之和。
這裡的Cost可以採用直線距離，也可以採用曼哈頓距離等，只要適合就行
那麼先計算起點周圍的所有節點的三個值
這裡設每兩個相鄰節點間的距離為10，那麼對角線距離為14
那麼計算得出，F值最小的是A點左上角的方塊，將節點放入列表(數組也行)將A設為該節點的父節點，然後計算周圍方塊的距離
因為是從A點移動過來的，所以下次比較時不再比較A點
再次計算得出F值最小的仍然為左上角的節點
這樣就求出了A到B點的最短路徑
如果A、B之間存在障礙物那麼又該怎麼辦呢？
同樣也是計算最小的 F 值
但這裡出現了三個相同的F值
那麼接下來優先選擇 H 值最小的路徑，即距離目標點最近的路徑
但是移動過後 F 值 反而變大了
那麼反過來尋找之前 F 值最小的路徑，但接下來還是 F 值更大
那麼仍然選擇 F 值最小的路徑
然後是下一個F值最小的路徑
然後是下一個
直到距離目的地的hCost為0
再來一個例子，這裡說明如何找出最短的路徑，箭頭表示父節點
第一次計算A點周圍節點的F值後，找出最小的那個，將節點的父節點設為A點
再次計算，將周圍節點設為子節點，然後後發現周圍有兩個58的點，選中gCost更小也就是下面A旁邊那個58的點
再次計算
在計算過下面那個58節點後發現旁邊節點從這裡經過所需的cost更小，所以重新設置父節點
再次說明
如果經過黃線的路徑，右下角節點的Cost會達到66
如果經過下面到達，Cost為58，會更小，會重新設置父節點
這裡重新計算的fCost為 A — 58 — 58，fCost為58更小，說明新路徑更小，重新設置父節點
按照此方法循環直至找到目標點
因為設置了父節點(圖中箭頭表示)，那麼只需要從目標點開始，一直獲取父節點即可，將獲取到的所有節點存儲進列表或數組然後進行翻轉，就得到了A-B的最短路徑
二、在Unity中設置路徑點
然後添加Cuble視為障礙物
將Cube的層級設為UnWalkable
接着複製幾個
新建腳本Node，節點目前只包含坐標位置，和是否能行走
public class Node
{
public bool walkable; //節點是否能走動
public Vector3 worldPos; //節點的空間坐標
public Node(bool _walkable, Vector3 _worldPos) //構造器
{
walkable = _walkable;
worldPos = _worldPos;
}
}
新建腳本MyGrid，添加到新建空物體A*上
public class MyGrid : MonoBehaviour
{
public LayerMask unwalkableMask; //節點是否能走動
public Vector2 gridWorldSize; //地圖的範圍，節點在地圖內創建
public float nodeRadius; //節點的大小
Node[,] grid; //節點數組
private void OnDrawGizmos()
{
//首先畫出地圖的範圍 //寬度 厚度 長度
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
}
}
然後設置節點地圖大小
繼續修改MyGrid
public class MyGrid : MonoBehaviour
{
public LayerMask unwalkableMask; //是否能行走
public Vector2 gridWorldSize; //需要尋路的地圖大小
public float nodeRadius; //節點半徑
Node[,] grid; //節點
float nodeDiameter; //節點的直徑
int gridSizeX, gridSizeY;
void Start()
{
nodeDiameter = nodeRadius * 2;
gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter); //計算出x軸方向有多少個節點
gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter); //計算出z軸方向有多少個節點
CreateGrid();
}
void CreateGrid()
{
grid = new Node[gridSizeX, gridSizeY]; //初始化節點數組
//計算網格的起始點(原點)
Vector3 worldButtonLeft = transform.position
- Vector3.right * gridWorldSize.x / 2
- Vector3.forward * gridWorldSize.y / 2;
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
//計算節點的空間坐標
Vector3 worldPoint = worldButtonLeft
+ Vector3.right * (x * nodeDiameter + nodeRadius)
+ Vector3.forward * (y * nodeDiameter + nodeRadius);
//判斷節點是否能行走，根據節點範圍是否與障礙物碰撞
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
grid[x, y] = new Node(walkable, worldPoint); //將節點的數據添加進二位數組
}
}
}
private void OnDrawGizmos()
{
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
if (grid != null)
{
foreach (Node node in grid)
{
//繪製出所有節點，可以行走為白色，不能行走為紅色
Gizmos.color = (node.walkable) ? Color.white : Color.red;
Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));//減少Gizmos方塊的大小便於觀察
}
}
}
}
運行結果
接下來添加一個起點和一個終點
新建兩個Capsule
那麼如何知道起點現在在哪個節點呢？
繼續修改MyGrid
public class MyGrid : MonoBehaviour
{
......
public Node NodeFromWorldPos(Vector3 worldPos) //這裡傳入起點的位置
{
//這裡 percentX 和 percentY 計算起點位置佔地圖區域橫豎坐標的比例
float percentX = (worldPos.x + gridWorldSize.x / 2) / gridWorldSize.x;
float percentY = (worldPos.z + gridWorldSize.y / 2) / gridWorldSize.y;
//將起點的位置限定在地圖範圍之內
percentX = Mathf.Clamp01(percentX);
percentY = Mathf.Clamp01(percentY);
//總節點數量 * 所在區域比例 = 在第幾個節點， -1 是為了從 0 開始計算，因為0也有一個節點
int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
return grid[x, y];
}
private void OnDrawGizmos()
{
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
if (grid != null)
{
//計算出起點的位置
Node playerNode = NodeFromWorldPos(player.position);
foreach (Node node in grid)
{
Gizmos.color = (node.walkable) ? Color.white : Color.red;
if (playerNode == node) //設置起點位置節點的顏色
{
Gizmos.color = Color.cyan;
}
Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
}
}
}
}
運行結果
三、實現尋路算法
修改Node
public class Node
{
......
public int gridX; //地圖中x方向第幾個節點
public int gridY; //地圖中y方向第幾個節點
public int gCost; //g值
public int hCost; //h值
public Node parent; //父節點，最後用於存儲實際路徑
//重新添加了兩個參數，便於計算鄰近節點
public Node(bool _walkable, Vector3 _worldPos,int _gridX, int _gridY)
{
walkable = _walkable;
worldPos = _worldPos;
gridX = _gridX;
gridY = _gridY;
}
public int FCost //屬性，F值
{
get
{
return gCost + hCost;
}
}
}
新建腳本PathFinding，並添加到物體A*上
public class PathFinding : MonoBehaviour
{
public Transform seeker, target; //聲明兩個坐標，起始點和目標點
private MyGrid grid;
......
private void Update()
{
FindPath(seeker.position, target.position); //計算路徑
}
private void FindPath(Vector3 startPos, Vector3 targetPos)
{
Node startNode = grid.NodeFromWorldPos(startPos); //輸入空間坐標，計算出起始點處於哪個節點位置
Node targwtNode = grid.NodeFromWorldPos(targetPos); //輸入空間坐標，計算出目標點處於哪個節點位置
List<Node> openSet = new List<Node>(); //用於存儲需要評估的節點
HashSet<Node> closedSet = new HashSet<Node>(); //用於存儲已經評估的節點
openSet.Add(startNode); //將起始點加入openSet，進行評估
while (openSet.Count > 0) //如果還有待評估的節點
{
#region //獲取待評估列表中 F 值最小的節點
Node currentNode = openSet[0]; //獲取其中一個待評估的節點
for (int i = 0; i < openSet.Count; i++) //將該節點與所有待評估的節點比較，找出 F 值 最小的節點，F
//值相同就h值更小的節點
{
if (openSet[i].FCost < currentNode.FCost
|| openSet[i].FCost == currentNode.FCost
&& openSet[i].hCost < currentNode.hCost)
{
currentNode = openSet[i];
}
}
#endregion
openSet.Remove(currentNode); //待評估節點中去掉 F 值最小的節點
closedSet.Add(currentNode); //將該節點加入已評估的節點，之後不再參與評估
if (currentNode == targwtNode) //如果該節點為目標終點，就計算出實際路徑並結束循環
{
RetracePath(startNode, targwtNode);
return;
}
//如果該節點不是目標點，遍歷該點周圍的所有節點
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
//如果周圍某節點不能行走 或 周圍某節點已經評估，為上一個節點，則跳過
// 說明某節點已經設置父節點
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
//計算前起始點前往某節點的 gCost 值，起始點的 gCost 值就是0
//經過循環這裡會計算周圍所有節點的g值
int newMovementCostToNeighbor = currentNode.gCost + GetDinstance(currentNode, neighbor);
//如果新路線 gCost 值更小(更近)， 或 某節點沒有評估過(為全新的節點)
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor; //計算某節點gCost
neighbor.hCost = GetDinstance(neighbor, targwtNode); //計算某節點hCost
neighbor.parent = currentNode; //將中間節點設為某節點的父節點
//如果存在某節點gCost更小的節點，會重新將中間節點設為某節點父節點
if (!openSet.Contains(neighbor)) //如果某節點沒有評估過
{
openSet.Add(neighbor); //將某節點加入待評估列表，在下一次循環進行評估，
//下一次循環又會找出這些周圍節點 F 值最小的節點
}
}
}
}
}
private void RetracePath(Node startNode, Node endNode) //獲取實際路徑
{
List<Node> path = new List<Node>();
Node currentNode = endNode;
while (currentNode != startNode) //如果當前不為目標點
{
path.Add(currentNode); //將當前節點加入路徑
currentNode = currentNode.parent;//獲取下一個節點(當前節點的父節點)
}
path.Reverse(); //反轉所有元素的順序
grid.path = path; //返回實際路徑
}
private int GetDinstance(Node nodeA, Node nodeB) //計算兩個節點間的cost
{
int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
if (dstX > dstY)
{
return 14 * dstY + 10 * (dstX - dstY);
}
return 14 * dstX + 10 * (dstY - dstX);
}
}
修改腳本MyGrid
public class MyGrid : MonoBehaviour
{
......
public List<Node> path;
......
void CreateGrid()
{
......
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
...... //多了兩個參數，方便計算周圍節點
grid[x, y] = new Node(walkable, worldPoint, x, y); //將節點的數據添加進二位數組
}
}
}
......
public List<Node> GetNeighbors(Node node) //獲取節點周圍的所有節點
{
List<Node> neighbors = new List<Node>();
//節點的相對坐標左側為x-1，右側為x+1，下方y-1，上方y+1
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0) //跳過中間的節點
{
continue;
}
//從x、y相對於中間節點的坐標 加上 中間節點位於地圖的坐標，得到了周圍節點位於地圖的坐標
int checkX = node.gridX + x;
int checkY = node.gridY + y;
//限定節點範圍，防止出現地圖外的不存在的節點
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbors.Add(grid[checkX, checkY]);//添加周圍節點
}
}
}
return neighbors;
}
private void OnDrawGizmos()
{
......
if (path != null)
{
if (path.Contains(node)) //給路徑添加顏色
{
Gizmos.color = Color.yellow;
}
}
Gizmos.DrawCube(node.worldPos, Vector3.one * (nodeDiameter - 0.1f));
}
}
}
}
自行在Inspector面板中設置相應的參數
運行結果
可以隨時修改起點和終點的位置
演示視頻：//www.bilibili.com/video/BV14B4y127YN/
下一篇 A*尋路算法2.0 將使用數組實現堆來代替List列表存儲節點，算法消耗的時間將減少約60％
