1. 程式人生 > >Unity3d 中的 A*尋路

Unity3d 中的 A*尋路

在本章中,我們將在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的該特色.

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).使用導航網格,在崎嶇的地形上尋路將變得容易得多.