Unity3D实现A*寻路算法

目录(?)[+]

1. A算法复习
2. 实现
3. Node
4. PriorityQueue
5. GridManager
6. AStar
7. TestCode Class
8. Scene setup
9. Testing
10. 总结

这篇文章翻译自Unity 4.x Game AI Programming这本书第七章

在本章中,我们将在Unity3D环境中使用C#实现A*算法.尽管有很多其他算法,像Dijkstra算法,但A*算法以其简单性和有效性而广泛的应用于游戏和交互式应用中.我们之前在第一章AI介绍中短暂的涉及到了该算法.不过现在我们从实现的角度来再次复习该算法.

A*算法复习

在我们进入下一部分实现A*之前,我们再次复习一下.首先,我们将用可遍历的数据结构来表示地图.尽管可能有很多结果,在这个例子中我们将使用2D格子数组.我们稍后将实现GridManager类来处理这个地图信息.<Todo>我们的类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静态变量里.

 <span style="white-space:pre">  </span>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; }
 }

紧接着我们声明所有的变量;<todo>我们需要表示我们的地图,像地图行数和列数,每个格子的大小,以及一些布尔值来形象化(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);
         }
     }
 }

<Todo>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.<Todo>之后我们调用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).使用导航网格,在崎岖的地形上寻路将变得容易得多.