使用A星寻路要把地图网格化,这是为了简化地图,图中阴影是障碍物
一些关键概念
开放列表(open list):记下所有被考虑用来寻找最短路径的格子
封闭列表(close list):记下已经搜索过的格子
路径代价 : F = G + H
G是指从起始点到当前方格的移动成本,当前方格的G = 父方格的G + 1
H是指从当前方格到目标点的估计移动成本,这通常被称为启发式,因为我们还没有真正知道成本,这只是一个估计,这里使用曼哈顿距离法,也就是横向格子的差值 + 纵向格子的差值
F :方块左上角值(估计值)
G :方块左下角值(精确值)
H : 方块右下角值 (估计值)
算法流程:
首先将起点添加到封闭列表,并设置父节点为空,将起点周围一圈非障碍节点添加到开放列表,然后循环执行下面流程
- 从开放列表中找F值最小的格子,记为curNode,将curNode从开放列表移动到封闭列表,如果curNode为终点,结束
- 将curNode周围一圈非障碍节点添加到开放列表中,计算F值,并设置curNode为它们的父节点
- 如果开放列表为空,表明找不到合适路径,如果不为空,回到1
找到终点后,就可以根据父节点回溯到起点,得到一条路径,这里需要保证路径上所有点都在封闭列表中,不然算法有问题
代码实现
格子类
public enum E_Node_Type
{
Walk, //可以走的地方
Stop, //障碍物
}
/// <summary>
/// A星格子类
/// 节点用于实现算法逻辑,不需要挂载在场景上,因此不用继承MonoBehaviour
/// </summary>
public class AStarNode
{
public int x;
public int y;
public float f; //寻路消耗
public float g; //离起点的距离
public float h; //离终点的距离
public AStarNode father; //父节点
public E_Node_Type type; //格子类型
public AStarNode(int x, int y, E_Node_Type type)
{
this.x = x;
this.y = y;
this.type = type;
}
}
管理器类
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Random = UnityEngine.Random;
/// <summary>
/// A星寻路管理器类
/// </summary>
public class AStarMgr
{
private static AStarMgr instance;
public static AStarMgr Instance
{
get
{
if (instance == null)
instance = new AStarMgr();
return instance;
}
}
public AStarNode[,] nodes; //地图相关所有格子对象容器
private List<AStarNode> openList; //开放列表
private List<AStarNode> closeList; //关闭列表
private int mapW;
private int mapH;
private Vector2 startPos;
private Vector2 endPos;
//障碍物比例
private float blockRate;
/// <summary>
/// 初始化地图宽高
/// </summary>
public void InitMapInfo(int w, int h, float rate)
{
mapW = w;
mapH = h;
blockRate = rate;
nodes = new AStarNode[w, h];
for(int i = 0; i < mapW; ++i)
{
for(int j = 0; j < mapH; ++j)
{
E_Node_Type type = Random.Range(0, 1f) > blockRate ? E_Node_Type.Walk : E_Node_Type.Stop;
AStarNode node = new AStarNode(i, j, type);
nodes[i, j] = node;
}
}
openList = new List<AStarNode>();
closeList = new List<AStarNode>();
}
public List<AStarNode> FindPath(Vector2 startPos, Vector2 endPos)
{
//判断起点终点是否合法
if(!IsLegalNode(startPos) || !IsLegalNode(endPos))
{
Debug.LogError("illegal node");
return null;
}
this.startPos = startPos;
this.endPos = endPos;
openList.Clear();
closeList.Clear();
//会有多次寻路,故要清理开始点数据
AStarNode startNode = GetNode(startPos);
startNode.father = null;
startNode.f = 0;
startNode.g = 0;
startNode.h = 0;
this.closeList.Add(startNode);
AddNodeAroundToOpenList(startPos);
AStarNode endNode = GetNode(endPos);
while (!closeList.Contains(endNode))
{
AStarNode node = GetMinimumCostNode();
if (node != null)
{
AddNodeAroundToOpenList(node);
Debug.Log("add close " + node.x + " " + node.y + " father " + node.father.x + " " + node.father.y);
this.closeList.Add(node);
this.openList.Remove(node);
}
else
{
Debug.LogError("死路");
break;
}
}
return GetPath();
}
/// <summary>
/// 判断该位置的格子是否可以作为起点终点
/// </summary>
private bool IsLegalNode(Vector2 pos)
{
if(pos.x < 0 || pos.x >= mapW || pos.y < 0 || pos.y >= mapH)
return false;
if (nodes[(int)pos.x, (int)pos.y].type == E_Node_Type.Stop)
return false;
return true;
}
/// <summary>
/// 判断节点是否已经添加到开放或者关闭列表了
/// </summary>
private bool IsNodeAdded(Vector2 pos)
{
AStarNode node = GetNode(pos);
if (openList.Contains(node) || closeList.Contains(node))
return true;
return false;
}
private AStarNode GetNode(Vector2 pos)
{
return nodes[(int)pos.x, (int)pos.y];
}
/// <summary>
/// 将节点添加到开放列表
/// </summary>
private void AddNodeToOpenList(Vector2 fatherPos, Vector2 nearPos)
{
if (!IsLegalNode(nearPos) || IsNodeAdded(nearPos))
return;
AStarNode node = GetNode(nearPos);
AStarNode fatherNode = GetNode(fatherPos);
node.father = fatherNode;
node.g = fatherNode.g + Vector2.Distance(fatherPos, nearPos);
node.h = Math.Abs(nearPos.x - endPos.x) + Math.Abs(nearPos.y - endPos.y);
node.f = node.g + node.h;
Debug.Log("add open " + node.x + " " + node.y + " father " + node.father.x + " " + node.father.y);
this.openList.Add(node);
}
/// <summary>
/// 找到周围合法的节点添加到开放列表中,如果不能斜着走就只添加上下左右
/// </summary>
private void AddNodeAroundToOpenList(Vector2 currentPos)
{
Vector2 topLeft = currentPos + Vector2.left + Vector2.up;
Vector2 top = currentPos + Vector2.up;
Vector2 topRight= currentPos + Vector2.right + Vector2.up;
Vector2 left = currentPos + Vector2.left;
Vector2 right = currentPos + Vector2.right;
Vector2 bottomLeft = currentPos + Vector2.left + Vector2.down;
Vector2 bottom = currentPos + Vector2.down;
Vector2 bottomRight = currentPos + Vector2.right + Vector2.down;
Debug.Log("currentPos " + currentPos + " topLeft " + topLeft);
AddNodeToOpenList(currentPos, topLeft);
Debug.Log("currentPos " + currentPos + " top " + top);
AddNodeToOpenList(currentPos, top);
Debug.Log("currentPos " + currentPos + " topRight " + topRight);
AddNodeToOpenList(currentPos, topRight);
Debug.Log("currentPos " + currentPos + " left " + left);
AddNodeToOpenList(currentPos, left);
Debug.Log("currentPos " + currentPos + " right " + right);
AddNodeToOpenList(currentPos, right);
Debug.Log("currentPos " + currentPos + " bottomLeft " + bottomLeft);
AddNodeToOpenList(currentPos, bottomLeft);
Debug.Log("currentPos " + currentPos + " bottom " + bottom);
AddNodeToOpenList(currentPos, bottom);
Debug.Log("currentPos " + currentPos + " bottomRight " + bottomRight);
AddNodeToOpenList(currentPos, bottomRight);
}
private void AddNodeAroundToOpenList(AStarNode node)
{
Vector2 pos = new Vector2(node.x, node.y);
AddNodeAroundToOpenList(pos);
}
/// <summary>
/// 在开放列表中找到f值最小的节点
/// </summary>
private AStarNode GetMinimumCostNode()
{
if(openList.Count > 0)
{
openList.Sort(SortOpenList);
return openList[0];
}
return null;
}
private int SortOpenList(AStarNode a, AStarNode b)
{
return a.f.CompareTo(b.f);
}
/// <summary>
/// 获得最后的路径
/// </summary>
private List<AStarNode> GetPath()
{
List<AStarNode> path = new List<AStarNode>();
if(closeList.Count > 0)
{
AStarNode node = GetNode(endPos);
while(node.father != null)
{
path.Add(node);
Debug.Log(node.x + " " + node.y + " father " + node.father.x + " " + node.father.y);
if (!closeList.Contains(node))
{
Debug.LogError("算法有问题");
break;
}
node = node.father;
}
path.Add(GetNode(startPos));
}
path.Reverse();
return path;
}
}
这里做一个简单的测试,用Cube表示方格,选择起点和终点后,生成一条路径
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class AStarTest : MonoBehaviour
{
//左下角第一个立方体位置
public int beginX = -3;
public int beginY = -3;
//立方体之间的间距
public int offsetX = 2;
public int offsetY = 2;
//地图格子的宽高
public int mapW = 10;
public int mapH = 10;
//障碍物的比例
[Range(0, 1f)]
public float blockRate = 0.2f;
public Material red;
public Material yellow;
public Material green;
public Material normal;
private Dictionary<string, GameObject> cubeDict = new Dictionary<string, GameObject>();
private Vector2 beginPos = Vector2.right * -1;
List<AStarNode> path;
void Start()
{
AStarMgr.Instance.InitMapInfo(mapW, mapH, blockRate);
for(int i = 0; i < mapH; ++i)
{
for (int j = 0; j < mapW; ++j)
{
GameObject obj = GameObject.CreatePrimitive(PrimitiveType.Cube);
obj.transform.position = new Vector3(beginX + j * offsetX, beginY + i * offsetY);
obj.name = j + "_" + i;
cubeDict.Add(obj.name, obj);
AStarNode node = AStarMgr.Instance.nodes[j, i];
if(node.type == E_Node_Type.Stop)
{
obj.GetComponent<MeshRenderer>().material = red;
}
}
}
}
void Update()
{
if (Input.GetMouseButtonDown(0))
{
//射线检测
RaycastHit info;
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
if(Physics.Raycast(ray, out info, 100))
{
if(beginPos == Vector2.right * -1)
{
//清理上次的路径
if(path != null)
{
foreach (AStarNode node in path)
{
GameObject obj = cubeDict[node.x + "_" + node.y];
if (obj != null)
{
obj.GetComponent<MeshRenderer>().material = normal;
}
}
}
//设置起点黄色
string[] strs = info.collider.gameObject.name.Split('_');
beginPos = new Vector2(int.Parse(strs[0]), int.Parse(strs[1]));
info.collider.gameObject.GetComponent<MeshRenderer>().material = yellow;
}
else
{
//设置终点并显示路径
string[] strs = info.collider.gameObject.name.Split('_');
info.collider.gameObject.GetComponent<MeshRenderer>().material = yellow;
Vector2 endPos = new Vector2(int.Parse(strs[0]), int.Parse(strs[1]));
path = AStarMgr.Instance.FindPath(beginPos, endPos);
StartCoroutine(ShowPath());
beginPos = Vector2.right * -1;
}
}
}
}
IEnumerator ShowPath()
{
foreach(AStarNode node in path)
{
GameObject obj = cubeDict[node.x + "_" + node.y];
if(obj != null)
{
yield return new WaitForSeconds(0.2f);
obj.GetComponent<MeshRenderer>().material = green;
}
}
}
}
实现效果