Unity3d 中的 A*尋路

這篇文章翻譯自Unity 4.x Game AI Programming這本書第七章

在本章中,我們將在Unity3D環境中使用C#實現A*算法.儘管有很多其他算法,像Dijkstra算法,但A*算法以其簡單性和有效性而廣泛的應用於遊戲和交互式應用中.我們之前在第一章AI介紹中短暫的涉及到了該算法.不過現在我們從實現的角度來再次複習該算法.

A*算法複習

在我們進入下一部分實現A*之前,我們再次複習一下A*算法.首先,我們將需要用可遍歷的數據結構來表示地圖.儘管可能有很多結構可實現,在這個例子中我們將使用2D格子數組.我們稍後將實現GridManager類來處理這個地圖信息.我們的類GridManager將記錄一系列的Node對象,這些Node對象纔是2D格子的主題.所以我們需要實現Node類來處理一些東西,比如節點類型,他是是一個可通行的節點還是障礙物,穿過節點的代價和到達目標節點的代價等等.

我們將用兩個變量來存儲已經處理過的節點和我們要處理的節點.我們分別稱他們爲關閉列表和開放列表.我們將在PriorityQueue類裏面實現該列表類型.我們現在看看它:

1. 首先,從開始節點開始,將開始節點放入開放列表中.
   
2. 只要開放列表中有節點,我們將進行一下過程.
   
3. 從開放列表中選擇第一個節點並將其作爲當前節點(我們將在代碼結束時提到它,這假定我們已經對開放列表排好序且第一個節點有最小代價值).
   
4. 獲得這個當前節點的鄰近節點,它們不是障礙物,像一堵牆或者不能穿越的峽谷一樣.
   
5. 對於每一個鄰近節點,檢查該鄰近節點是否已在關閉列表中.如果不在,我們將爲這個鄰近節點計算所有代價值(F),計算時使用下面公式:F = G + H,在前面的式子中,G是從上一個節點到這個節點的代價總和,H是從當前節點到目的節點的代價總和.
   
6. 將代價數據存儲在鄰近節點中,並且將當前節點保存爲該鄰近節點的父節點.之後我們將使用這個父節點數據來追蹤實際路徑.
   
7. 將鄰近節點存儲在開放列表中.根據到他目標節點的代價總和,以升序排列開放列表.
   
8. 如果沒有鄰近節點需要處理,將當前節點放入關閉列表並將其從開放列表中移除.
   
9. 返回第二步

一旦你完成了這個過程,你的當前節點將在目標節點的位置,但只有當存在一條從開始節點到目標節點的無障礙路徑.如果當前節點不在目標節點,那就沒有從目標節點到當前節點的路徑.如果存在一條正確的路徑我們現在所能做的就是從當前節點的父節點開始追溯,直到我們再次到達開始節點.這樣我們得到一個路徑列表,其中的節點都是我們在尋路過程中選擇的,並且該列表從目標節點排列到開始節點.之後我們翻轉這個路徑列表,因爲我們需要知道從開始節點到目標節點的路徑.

這就是我們將在Unity3D中使用C#實現的算法概覽.所以,搞起吧.

實現

我們將實現我們之前提到過的基礎類比如Node類,GridManager類和PriorityQueue類.我們將在後續的主AStar類裏面使用它們.

Node

Node類將處理代表我們地圖的2D格子中其中的每個格子對象,一下是Node.cs文件.

using UnityEngine;
using System.Collections;
using System;

public class Node : IComparable {
    public float nodeTotalCost;
    public float estimatedCost;
    public bool bObstacle;
    public Node parent;
    public Vector3 position;

    public Node() {
        this.estimatedCost = 0.0f;
        this.nodeTotalCost = 1.0f;
        this.bObstacle = false;
        this.parent = null;
    }

    public Node(Vector3 pos) {
        this.estimatedCost = 0.0f;
        this.nodeTotalCost = 1.0f;
        this.bObstacle = false;
        this.parent = null;
        this.position = pos;
    }

    public void MarkAsObstacle() {
        this.bObstacle = true;
    }

Node類有其屬性,比如代價值(G和H),標識其是否是障礙物的標記,和其位置和父節點. nodeTotalCost是G,它是從開始節點到當前節點的代價值,estimatedCost是H,它是從當前節點到目標節點的估計值.我們也有兩個簡單的構造方法和一個包裝方法來設置該節點是否爲障礙物.之後我們實現如下面代碼所示的CompareTo方法:

public int CompareTo(object obj)
{
    Node node = (Node) obj;
    //Negative value means object comes before this in the sort order.
    if (this.estimatedCost < node.estimatedCost)
            return -1;
    //Positive value means object comes after this in the sort order.
    if (this.estimatedCost > node.estimatedCost)
            return 1;
    return 0;
}

這個方法很重要.我們的Node類繼承自ICompare因爲我們想要重寫這個CompareTo方法.如果你能想起我們在之前算法部分討論的東西,你會注意到我們需要根據所有預估代價值來排序我們的Node數組.ArrayList類型有個叫Sort.Sort的方法,該方法只是從列表中的對象(在本例中是Node對象)查找對象內部實現的CompareTo方法.所以,我們實現這個方法並根據estimatedCost值來排序Node對象.你可以從以下資源中瞭解到更多關於.Net framework的該特色.

提示:IComparable.CompareTo方法可以這個鏈接http://msdn.microsoft.com/en-us/library/%20system.icomparable.compareto.aspx找到

PriorityQueue

PriorityQueue是一個簡短的類,使得ArrayList處理節點變得容易些,PriorityQueue.cs展示如下:

using UnityEngine;
using System.Collections;
public class PriorityQueue {
    private ArrayList nodes = new ArrayList();

    public int Length {
        get { return this.nodes.Count; }
    }

    public bool Contains(object node) {
        return this.nodes.Contains(node);
    }

    public Node First() {
        if (this.nodes.Count > 0) {
            return (Node)this.nodes[0];
        }
                return null;
    }

    public void Push(Node node) {
        this.nodes.Add(node);
        this.nodes.Sort();
    }

    public void Remove(Node node) {
        this.nodes.Remove(node);
        //Ensure the list is sorted
        this.nodes.Sort();
    }
}

上面的代碼很好理解.需要注意的一點是從節點的ArrayList添加或者刪除節點後我們調用了Sort方法.這將調用Node對象的CompareTo方法,且將使用estimatedCost值來排序節點.

GridManager

GridManager類處理所有代表地圖的格子的屬性.我們將GridManager設置單例,因爲我們只需要一個對象來表示地圖.GridManager.cs代碼如下:

using UnityEngine;
using System.Collections;
public class GridManager : MonoBehaviour {
    private static GridManager s_Instance = null;
    public static GridManager instance {
        get {
            if (s_Instance == null) {
                s_Instance = FindObjectOfType(typeof(GridManager))
                        as GridManager;
                if (s_Instance == null)
                    Debug.Log("Could not locate a GridManager " +
                            "object. \n You have to have exactly " +
                            "one GridManager in the scene.");
            }
            return s_Instance;
        }
    }

我們在場景中尋找GridManager對象,如果找到我們將其保存在s_Instance靜態變量裏.

public int numOfRows;
public int numOfColumns;
public float gridCellSize;
public bool showGrid = true;
public bool showObstacleBlocks = true;
private Vector3 origin = new Vector3();
private GameObject[] obstacleList;
public Node[,] nodes { get; set; }
public Vector3 Origin {
    get { return origin; }
}

緊接着我們聲明所有的變量;我們需要表示我們的地圖,像地圖行數和列數,每個格子的大小,以及一些布爾值來形象化(visualize)格子與障礙物.此外還要向下面的代碼一樣保存格子上存在的節點:

void Awake() {
    obstacleList = GameObject.FindGameObjectsWithTag("Obstacle");
    CalculateObstacles();
}
// Find all the obstacles on the map
void CalculateObstacles() {
    nodes = new Node[numOfColumns, numOfRows];
    int index = 0;
    for (int i = 0; i < numOfColumns; i++) {
        for (int j = 0; j < numOfRows; j++) {
            Vector3 cellPos = GetGridCellCenter(index);
            Node node = new Node(cellPos);
            nodes[i, j] = node;
            index++;
        }
    }
    if (obstacleList != null && obstacleList.Length > 0) {
        //For each obstacle found on the map, record it in our list
        foreach (GameObject data in obstacleList) {
            int indexCell = GetGridIndex(data.transform.position);
            int col = GetColumn(indexCell);
            int row = GetRow(indexCell);
            nodes[row, col].MarkAsObstacle();
        }
    }
}

我們查找所有標籤爲Obstacle的遊戲對象(game objects)並將其保存在我們的obstacleList屬性中.之後在CalculateObstacles方法中設置我們節點的2D數組.首先,我們創建具有默認屬性的普通節點屬性.在這之後我們查看obstacleList.將其位置轉換成行,列數據(即節點是第幾行第幾列)並更新對應索引處的節點爲障礙物.

GridManager有一些輔助方法來遍歷格子並得到對應格子的對局.以下是其中一些函數(附有簡短說明以闡述它們的是做什麼的).實現很簡單,所以我們不會過多探究其細節.

GetGridCellCenter方法從格子索引中返回世界座標系中的格子位置,代碼如下:

public Vector3 GetGridCellCenter(int index) {
    Vector3 cellPosition = GetGridCellPosition(index);
    cellPosition.x += (gridCellSize / 2.0f);
    cellPosition.z += (gridCellSize / 2.0f);
    return cellPosition;
}
public Vector3 GetGridCellPosition(int index) {
    int row = GetRow(index);
    int col = GetColumn(index);
    float xPosInGrid = col * gridCellSize;
    float zPosInGrid = row * gridCellSize;
    return Origin + new Vector3(xPosInGrid, 0.0f, zPosInGrid);
}

GetGridIndex方法從給定位置返回格子中的格子索引:

public int GetGridIndex(Vector3 pos) {
    if (!IsInBounds(pos)) {
        return -1;
    }
    pos -= Origin;
    int col = (int)(pos.x / gridCellSize);
    int row = (int)(pos.z / gridCellSize);
    return (row * numOfColumns + col);
}
public bool IsInBounds(Vector3 pos) {
    float width = numOfColumns * gridCellSize;
    float height = numOfRows* gridCellSize;
    return (pos.x >= Origin.x &&  pos.x <= Origin.x + width &&
            pos.x <= Origin.z + height && pos.z >= Origin.z);
}

GetRow和GetColumn方法分別從給定索引返回格子的行數和列數.

public int GetRow(int index) {
    int row = index / numOfColumns;
    return row;
}
public int GetColumn(int index) {
    int col = index % numOfColumns;
    return col;
}

另外一個重要的方法是GetNeighbours,它被AStar類用於檢索特定節點的鄰接點.

public void GetNeighbours(Node node, ArrayList neighbors) {
    Vector3 neighborPos = node.position;
    int neighborIndex = GetGridIndex(neighborPos);
    int row = GetRow(neighborIndex);
    int column = GetColumn(neighborIndex);
    //Bottom
    int leftNodeRow = row - 1;
    int leftNodeColumn = column;
    AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors);
    //Top
    leftNodeRow = row + 1;
    leftNodeColumn = column;
    AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors);
    //Right
    leftNodeRow = row;
    leftNodeColumn = column + 1;
    AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors);
    //Left
    leftNodeRow = row;
    leftNodeColumn = column - 1;
    AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors);
}

void AssignNeighbour(int row, int column, ArrayList neighbors) {
    if (row != -1 && column != -1 &&
        row < numOfRows && column < numOfColumns) {
      Node nodeToAdd = nodes[row, column];
      if (!nodeToAdd.bObstacle) {
        neighbors.Add(nodeToAdd);
      }
    }
  }

首先,我們在當前節點的左右上下四個方向檢索其鄰接節點.之後,在AssignNeighbour方法中,我們檢查鄰接節點看其是否爲障礙物.如果不是我們將其添加neighbours中.緊接着的方法是一個調試輔助方法用於形象化(visualize)格子和障礙物.

void OnDrawGizmos() {
        if (showGrid) {
            DebugDrawGrid(transform.position, numOfRows, numOfColumns,
                    gridCellSize, Color.blue);
        }
        Gizmos.DrawSphere(transform.position, 0.5f);
        if (showObstacleBlocks) {
            Vector3 cellSize = new Vector3(gridCellSize, 1.0f,
                gridCellSize);
            if (obstacleList != null && obstacleList.Length > 0) {
                foreach (GameObject data in obstacleList) {
                    Gizmos.DrawCube(GetGridCellCenter(
                            GetGridIndex(data.transform.position)), cellSize);
                }
            }
        }
    }
    public void DebugDrawGrid(Vector3 origin, int numRows, int
        numCols,float cellSize, Color color) {
        float width = (numCols * cellSize);
        float height = (numRows * cellSize);
        // Draw the horizontal grid lines
        for (int i = 0; i < numRows + 1; i++) {
            Vector3 startPos = origin + i * cellSize * new Vector3(0.0f,
                0.0f, 1.0f);
            Vector3 endPos = startPos + width * new Vector3(1.0f, 0.0f,
                0.0f);
            Debug.DrawLine(startPos, endPos, color);
        }
            // Draw the vertial grid lines
        for (int i = 0; i < numCols + 1; i++) {
            Vector3 startPos = origin + i * cellSize * new Vector3(1.0f,
                0.0f, 0.0f);
            Vector3 endPos = startPos + height * new Vector3(0.0f, 0.0f,
                1.0f);
            Debug.DrawLine(startPos, endPos, color);
        }
    }
}

Gizmos在編輯器場景視圖中可以用於繪製可視化的調試並建立輔助.OnDrawGizmos在每一幀都會被引擎調用.所以,如果調試標識showGrid和showObstacleBlocks被勾選我們就是用線條繪製格子使用立方體繪製障礙物.我們就不講DebugDrawGrid這個簡單的方法了.

注意:你可以從Unity3D參考文檔瞭解到更多有關gizmos的資料

AStar

類AStar是將要使用我們目前所實現的類的主類.如果你想複習這的話,你可以返回算法部分.如下面AStar.cs代碼所示,我們先聲明我們的openList和closedList,它們都是PriorityQueue類型.

using UnityEngine;
using System.Collections;
public class AStar {
    public static PriorityQueue closedList, openList;

接下來我們實現一個叫HeursticEstimatedCost方法來計算兩個節點之間的代價值.計算很簡單.我們只是通過兩個節點的位置向量相減得到方向向量.結果向量的長度便告知了我們從當前節點到目標節點的直線距離.

private static float HeuristicEstimateCost(Node curNode,
        Node goalNode) {
    Vector3 vecCost = curNode.position - goalNode.position;
    return vecCost.magnitude;
}

接下來使我們主要的FindPath方法:

public static ArrayList FindPath(Node start, Node goal) {
    openList = new PriorityQueue();
    openList.Push(start);
    start.nodeTotalCost = 0.0f;
    start.estimatedCost = HeuristicEstimateCost(start, goal);
    closedList = new PriorityQueue();
    Node node = null;

我們初始化開放和關閉列表.從開始節點開始,我們將其放入開放列表.之後我們便開始處理我們的開放列表.

    while (openList.Length != 0) {
        node = openList.First();
        //Check if the current node is the goal node
        if (node.position == goal.position) {
            return CalculatePath(node);
        }
        //Create an ArrayList to store the neighboring nodes
        ArrayList neighbours = new ArrayList();
        GridManager.instance.GetNeighbours(node, neighbours);
        for (int i = 0; i < neighbours.Count; i++) {
            Node neighbourNode = (Node)neighbours[i];
            if (!closedList.Contains(neighbourNode)) {
                float cost = HeuristicEstimateCost(node,
                        neighbourNode);
                float totalCost = node.nodeTotalCost + cost;
                float neighbourNodeEstCost = HeuristicEstimateCost(
                        neighbourNode, goal);
                neighbourNode.nodeTotalCost = totalCost;
                neighbourNode.parent = node;
                neighbourNode.estimatedCost = totalCost +
                        neighbourNodeEstCost;
                if (!openList.Contains(neighbourNode)) {
                    openList.Push(neighbourNode);
                }
            }
        }
        //Push the current node to the closed list
        closedList.Push(node);
        //and remove it from openList
        openList.Remove(node);
    }
    if (node.position != goal.position) {
        Debug.LogError("Goal Not Found");
        return null;
    }
    return CalculatePath(node);
}

這代碼實現類似於我們之前討論過的算法,所以如果你對特定的東西不清楚的話可以返回去看看.

1. 獲得openList的第一個節點.記住每當新節點加入時openList都需要再次排序.所以第一個節點總是有到目的節點最低估計代價值.
   
2. 檢查當前節點是否是目的節點,如果是推出while循環創建path數組.
   
3. 創建數組列表保存當前正被處理的節點的臨近節點.使用GetNeighbours方法來從格子中檢索鄰接節點.
   
4. 對於每一個在鄰接節點數組中的節點,我們檢查它是否已在closedList中.如果不在,計算代價值並使用新的代價值更新節點的屬性值,更新節點的父節點並將其放入openList中.
   
5. 將當前節點壓入closedList中並將其從openList中移除.返回第一步.
   

如果在openList中沒有更多的節點,我們的當前節點應該是目標節點如果路徑存在的話.之後我們將當前節點作爲參數傳入CalculatePath方法中.

private static ArrayList CalculatePath(Node node) {
    ArrayList list = new ArrayList();
    while (node != null) {
        list.Add(node);
        node = node.parent;
    }
    list.Reverse();
    return list;
}

CalculatePath方法跟蹤每個節點的父節點對象並創建數組列表.他返回一個從目的節點到開始節點的ArrayList.由於我們需要從開始節點到目標節點的路徑,我們簡單調用一下Reverse方法就ok.

這就是我們的AStar類.我們將在下面的代碼裏寫一個測試腳本來檢驗所有的這些東西.之後創建一個場景並在其中使用它們.

TestCode Class

代碼如TestCode.cs所示,該類使用AStar類找到從開始節點到目的節點的路徑.

using UnityEngine;
using System.Collections;
public class TestCode : MonoBehaviour {
    private Transform startPos, endPos;
    public Node startNode { get; set; }
    public Node goalNode { get; set; }
    public ArrayList pathArray;
    GameObject objStartCube, objEndCube;
    private float elapsedTime = 0.0f;
    //Interval time between pathfinding
    public float intervalTime = 1.0f;

首先我們創建我們需要引用的變量.pathArray用於保存從AStar的FindPath方法返回的節點數組.

void Start () {
    objStartCube = GameObject.FindGameObjectWithTag("Start");
    objEndCube = GameObject.FindGameObjectWithTag("End");
    pathArray = new ArrayList();
    FindPath();
}
void Update () {
    elapsedTime += Time.deltaTime;
    if (elapsedTime >= intervalTime) {
        elapsedTime = 0.0f;
        FindPath();
    }
}

在Start方法中我們尋找標籤(tags)爲Start和End的對象並初始化pathArray.如果開始和結束的節點位置變了我們將嘗試在每一個我們所設置的intervalTime屬性時間間隔裏找到我們的新路徑.之後我們調用FindPath方法.

void FindPath() {
    startPos = objStartCube.transform;
    endPos = objEndCube.transform;
    startNode = new Node(GridManager.instance.GetGridCellCenter(
            GridManager.instance.GetGridIndex(startPos.position)));
    goalNode = new Node(GridManager.instance.GetGridCellCenter(
            GridManager.instance.GetGridIndex(endPos.position)));
    pathArray = AStar.FindPath(startNode, goalNode);
}

因爲我們在AStar類中實現了尋路算法,尋路現在變得簡單多了.首先,我們獲得開始和結束的遊戲對象(game objects).之後,我們使用GridManager的輔助方法創建新的Node對象,使用GetGridIndex來計算它們在格子中對應的行列位置.一旦我們將開始節點和目標節點作爲參數調用了AStar.FindPath方法並將返回的數組保存在pathArray屬性中.接下我們實現OnDrawGizmos方法來繪製並形象化(draw and visualize)我們找到的路徑.

void OnDrawGizmos() {
    if (pathArray == null)
        return;
    if (pathArray.Count > 0) {
        int index = 1;
        foreach (Node node in pathArray) {
            if (index < pathArray.Count) {
                Node nextNode = (Node)pathArray[index];
                Debug.DrawLine(node.position, nextNode.position,
                    Color.green);
                index++;
            }
        }
    }
}

我們檢查了我們的pathArray並使用Debug.DrawLine方法來繪製線條連接起pathArray中的節點.當運行並測試程序時我們就能看到一條綠線從開始節點連接到目標節點,連線形成了一條路徑.

Scene setup

我們將要創建一個類似於下面截圖所展示的場景:

Sample test scene

我們將有一個平行光,開始以及結束遊戲對象,一些障礙物,一個被用作地面的平面實體和兩個空的遊戲對象,空對象身上放置GridManager和TestAstar腳本.這是我們的場景層級圖.

Scene hierarchy

創建一些立方體實體並給他們加上標籤Obstacle,當運行我們的尋路算法時我們需要尋找帶有該標籤的對象.

Obstacle nodes

創建一個立方體實體並加上標籤Start

Start node

創建另一個立方體實體並加上標籤End

End node

現在創建一個空的遊戲對象並將GridManager腳本賦給它.將其名字也設置回GridManager因爲在我們的腳本中使用該名稱尋找GridManager對象.這裏我們可以設置格子的行數和列數和每個格子的大小.

GridManager script

Testing

我們點擊Play按鈕實打實的看下我們的A*算法.默認情況下,一旦你播放當前場景Unity3D將會切換到Game視圖.由於我們的尋路形象化(visualization)代碼是爲我編輯器視圖中的調試繪製而寫,你需要切換回Scene視圖或者勾選Gizmos來查看找到的路徑.

現在在場景中嘗試使用編輯器的移動工具移動開始和結束節點.(不是在Game視圖中,而是在Scene視圖中)

如果從開始節點到目的節點有合法路徑,你應該看到路徑會對應更新並且是動態實時的更新.如果沒有路徑,你會在控制窗口中得到一條錯誤信息.

總結

在本章中,我們學習瞭如何在Unity3D環境中實現A*尋路算法.我們實現了自己的A*尋路類以及我們自己的格子類,隊列類和節點類.我們學習了IComparable接口並重寫了CompareTo方法.我們使用調試繪製功能(debug draw functionalities)來呈現我們的網格和路徑信息.有了Unity3D的navmesh和navagent功能你可能不必自己實現尋路算法.不管怎麼樣,他幫助你理解實現背後的基礎算法.

在下一章中,我們將查看如何擴展藏在A*背後的思想看看導航網格(navigation meshes).使用導航網格,在崎嶇的地形上尋路將變得容易得多.