unity 接入A*寻路算法总结

1.先来一篇博客

这两天研究了下 A* 寻路算法, 主要学习了这篇文章, 但这篇翻译得不是很好, 我花了很久才看明白文章中的各种指代. 特写此篇博客用来总结, 并写了寻路算法的代码, 觉得有用的同学可以看看. 另外因为图片制作起来比较麻烦, 所以我用的是原文里的图片.

当然寻路算法不止 A* 这一种, 还有递归, 非递归, 广度优先, 深度优先, 使用堆栈等等, 有兴趣的可以研究研究~~

简易地图

如图所示简易地图, 其中绿色方块的是起点 (用 A 表示), 中间蓝色的是障碍物, 红色的方块 (用 B 表示) 是目的地. 为了可以用一个二维数组来表示地图, 我们将地图划分成一个个的小方块.

二维数组在游戏中的应用是很多的, 比如贪吃蛇和俄罗斯方块基本原理就是移动方块而已. 而大型游戏的地图, 则是将各种"地貌"铺在这样的小方块上.

寻路步骤

1. 从起点A开始, 把它作为待处理的方格存入一个"开启列表", 开启列表就是一个等待检查方格的列表.

2. 寻找起点A周围可以到达的方格, 将它们放入"开启列表", 并设置它们的"父方格"为A.

3. 从"开启列表"中删除起点 A, 并将起点 A 加入"关闭列表", "关闭列表"中存放的都是不需要再次检查的方格

图中浅绿色描边的方块表示已经加入 "开启列表" 等待检查. 淡蓝色描边的起点 A 表示已经放入 "关闭列表" , 它不需要再执行检查.

从 "开启列表" 中找出相对最靠谱的方块, 什么是最靠谱? 它们通过公式 F=G+H 来计算.

F = G + H

G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动).

H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动).

我们假设横向移动一个格子的耗费为10, 为了便于计算, 沿斜方向移动一个格子耗费是14. 为了更直观的展示如何运算 FGH, 图中方块的左上角数字表示 F, 左下角表示 G, 右下角表示 H. 看看是否跟你心里想的结果一样?

从 "开启列表" 中选择 F 值最低的方格 C (绿色起始方块 A 右边的方块), 然后对它进行如下处理:

4. 把它从 "开启列表" 中删除, 并放到 "关闭列表" 中.

5. 检查它所有相邻并且可以到达 (障碍物和 "关闭列表" 的方格都不考虑) 的方格. 如果这些方格还不在 "开启列表" 里的话, 将它们加入 "开启列表", 计算这些方格的 G, H 和 F 值各是多少, 并设置它们的 "父方格" 为 C.

6. 如果某个相邻方格 D 已经在 "开启列表" 里了, 检查如果用新的路径 (就是经过C 的路径) 到达它的话, G值是否会更低一些, 如果新的G值更低, 那就把它的 "父方格" 改为目前选中的方格 C, 然后重新计算它的 F 值和 G 值 (H 值不需要重新计算, 因为对于每个方块, H 值是不变的). 如果新的 G 值比较高, 就说明经过 C 再到达 D 不是一个明智的选择, 因为它需要更远的路, 这时我们什么也不做.

如图, 我们选中了 C 因为它的 F 值最小, 我们把它从 "开启列表" 中删除, 并把它加入 "关闭列表". 它右边上下三个都是墙, 所以不考虑它们. 它左边是起始方块, 已经加入到 "关闭列表" 了, 也不考虑. 所以它周围的候选方块就只剩下 4 个. 让我们来看看 C 下面的那个格子, 它目前的 G 是14, 如果通过 C 到达它的话, G将会是 10 + 10, 这比 14 要大, 因此我们什么也不做.

然后我们继续从 "开启列表" 中找出 F 值最小的, 但我们发现 C 上面的和下面的同时为 54, 这时怎么办呢? 这时随便取哪一个都行, 比如我们选择了 C 下面的那个方块 D.

D 右边已经右上方的都是墙, 所以不考虑, 但为什么右下角的没有被加进 "开启列表" 呢? 因为如果 C 下面的那块也不可以走, 想要到达 C 右下角的方块就需要从 "方块的角" 走了, 在程序中设置是否允许这样走. (图中的示例不允许这样走)

就这样, 我们从 "开启列表" 找出 F 值最小的, 将它从 "开启列表" 中移掉, 添加到 "关闭列表". 再继续找出它周围可以到达的方块, 如此循环下去...

那么什么时候停止呢? —— 当我们发现 "开始列表" 里出现了目标终点方块的时候, 说明路径已经被找到.

如何找回路径

如上图所示, 除了起始方块, 每一个曾经或者现在还在 "开启列表" 里的方块, 它都有一个 "父方块", 通过 "父方块" 可以索引到最初的 "起始方块", 这就是路径.

将整个过程抽象

把起始格添加到 "开启列表"

{

寻找开启列表中F值最低的格子, 我们称它为当前格.

把它切换到关闭列表.

对当前格相邻的8格中的每一个

if (它不可通过 || 已经在 "关闭列表" 中)

{

什么也不做.

}

if (它不在开启列表中)

{

把它添加进 "开启列表", 把当前格作为这一格的父节点, 计算这一格的 FGH

if (它已经在开启列表中)

{

if (用G值为参考检查新的路径是否更好, 更低的G值意味着更好的路径)

{

把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值.

}

} while( 目标格已经在 "开启列表", 这时候路径被找到)

如果开启列表已经空了, 说明路径不存在.

最后从目标格开始, 沿着每一格的父节点移动直到回到起始格, 这就是路径.

总结插入

1.理解FGH代表的意义

2.寻找相邻区域（这里可以自己设定寻找规则）

3.计算G值

4.理解opnelist和closelist

主要实现函数

1.先计算出所有点到终点的预计消耗（规则自定，这里也是只可以上下左右移动）

1.在开放列表中寻找F值最新的区域，作为寻路起始点，该函数只需要对openlist数组进行F值对比排序：FnidMinF_Piont

2.计算相邻区域列表，该函数需要找出起始点周围区域数组：FindSurroundPoints（该函数规则自定：包括相邻区域和对角区域规则）

3.在openlist列表中，计算从其实区域到相邻点的G值，如果该G值大于相邻点的G值不做任何处理，否则将相邻点的parentPoint设置为该起始点，重新计算相邻点的G

4.不在openlist列表中，设置所以相邻点的parentPoint设置为该起始点，计算所以相邻点的G值

增加功能

1.动态添加障碍物，为了降低计算量，以起始点为原点，radius为半径，计算地图所有点与原点的距离M，如果M<ridius，设置为占领（计算待优化，不需要扫描所有地图点）

2.动态检查障碍物之间是否可通过，找到相邻点创建半径为radius的圆，（依据动态添加障碍物规则，如果半径内存在障碍物，则不添加进openlist）

//附带我自己实现的寻路算法

namespace Ai
{
using System;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using UnityEngine;

//F值比较函数
class PointFComparator : IComparer<Point>
{
public int Compare(Point t1, Point t2)
{
if (t2.F > t1.F)
{
return -1;
}
if (t2.F == t1.F)
{
return 0;
}
return 1;
}
}

//数据点类
public class Point
{
public float F;
public float G;
public float H;
private int index;
public ulong occupyerId;
public Point ParentPoint;
public Vector3 Pos;
public Vector2 VecIndex;
public int wight;

public Point(Vector2 _pos)
{
this.Pos = new Vector3(_pos.x, 0f, _pos.y);
}

public float CalcF()
{
this.F = this.G + this.H;
return this.F;
}

public void SetH(Point sEnd, float sConsume)
{
float introduced0 = Mathf.Abs(sEnd.VecIndex.x - this.VecIndex.x);
this.H = (introduced0 + Mathf.Abs(sEnd.VecIndex.y - this.VecIndex.y)) * sConsume;
}

public int Index
{
get
{
return this.index;
}
set
{
this.index = value;
}
}
}

//生成a星数据类
public class FactoryAStarData
{
public Vector3 centerPos = Vector3.zero;
public List<Point> closeList = new List<Point>();
public List<Point> endList = new List<Point>();
private float fGridLength = 1f;
private int fGridNum = 500;
public float fRadius = 2f;
public Dictionary<ulong, Point> obstacleDic = new Dictionary<ulong, Point>();
public List<Point> openList = new List<Point>();
public Dictionary<Vector2, Point> starMap = new Dictionary<Vector2, Point>();
public float unitConsume = 10f;

public void ClearAStarData()
{
this.openList.Clear();
this.closeList.Clear();
}

//寻路接口
public Point FindPointByPos(Vector3 _targetPos)
{
Vector2 vector = new Vector2(_targetPos.x, _targetPos.z);
int num = Mathf.FloorToInt(vector.x / this.fGridLength);
int num2 = Mathf.FloorToInt(vector.y / this.fGridLength);
if (((num > this.fGridNum) || (num2 > this.fGridNum)) || ((num < 0) || (num2 < 0)))
{
Debug.LogError("目标不在地图内，或地图过小");
return null;
}
return this.starMap[new Vector2((float)num, (float)num2)];
}

//通过index找点
public Vector3 FindPointPosByIndex(Vector2 gIndex)
{
Vector3 pos = Vector3.one;
if (this.starMap.ContainsKey(gIndex))
{
pos = this.starMap[gIndex].Pos;
}
return pos;
}

//刷新地图信息
public void ReFreshMap()
{
if (this.starMap.Count <= 0)
{
for (int i = 0; i < this.fGridNum; i++)
{
for (int j = 0; j < this.fGridNum; j++)
{
Vector2 key = new Vector2(this.centerPos.x + j, this.centerPos.z + i);
Vector2 vector2 = new Vector2(this.centerPos.x + (j * this.fGridLength), this.centerPos.z + (i * this.fGridLength));
Point point = new Point(vector2)
{
Index = (i * this.fGridNum) + j,
VecIndex = key
};
this.starMap.Add(key, point);
}
}
}
}
}

//寻路类
public class A_Star
{
private static A_Star _instance = new A_Star();
public FactoryAStarData currentData;
private List<Vector2> endVecList = new List<Vector2>();
private Dictionary<ulong, FactoryAStarData> factoryDic = new Dictionary<ulong, FactoryAStarData>();
private float fGridLength = 2f;
private Vector2 vecD = new Vector2(0f, -1f);
private Vector2 vecL = new Vector2(1f, 0f);
private Vector2 vecLD = new Vector2(1f, -1f);
private Vector2 vecLU = new Vector2(1f, 1f);
private Vector2 vecR = new Vector2(-1f, 0f);
private Vector2 vecRD = new Vector2(-1f, -1f);
private Vector2 vecRU = new Vector2(-1f, 1f);
private Vector2 vecU = new Vector2(0f, 1f);

private A_Star()
{
}

public void CalculatePointsH(Point cEnd, FactoryAStarData tempFindData)
{
Vector2 vecIndex = tempFindData.starMap[cEnd.VecIndex].VecIndex;
for (int i = -5; i <= 5; i++)
{
for (int j = -5; j <= 5; j++)
{
if (tempFindData.starMap.ContainsKey(vecIndex + new Vector2((float)i, (float)j)))
{
tempFindData.starMap[vecIndex + new Vector2((float)i, (float)j)].SetH(cEnd, tempFindData.unitConsume);
tempFindData.starMap[vecIndex + new Vector2((float)i, (float)j)].ParentPoint = null;
tempFindData.starMap[vecIndex + new Vector2((float)i, (float)j)].G = 0f;
}
}
}
}

public Point FindMinF_Point(FactoryAStarData tempFindData)
{
tempFindData.openList.Sort(new PointFComparator());
return tempFindData.openList[0];
}

public List<Vector2> FindPath(Vector3 startPos, Vector3 endPos, ulong selfId)
{
FactoryAStarData currentData = this.currentData;
if (currentData == null)
{
Debug.LogError("无数据！");
return null;
}
Point item = currentData.FindPointByPos(startPos);
Point cEnd = currentData.FindPointByPos(endPos);
this.endVecList = new List<Vector2>();
if ((item == null) || (cEnd == null))
{
Debug.LogError("无数据！");
return null;
}
if (item.Index == cEnd.Index)
{
return null;
}
currentData.ClearAStarData();
this.CalculatePointsH(cEnd, currentData);
currentData.openList.Add(item);
item.ParentPoint = null;
while (currentData.openList.Count != 0)
{
Point point3 = this.FindMinF_Point(currentData);
currentData.openList.RemoveAt(0);
currentData.closeList.Add(point3);
List<Point> list = this.FindSurroundPoints(point3, currentData.fRadius, selfId, cEnd, currentData);
for (int i = 0; i < list.Count; i++)
{
if (currentData.openList.Contains(list[i]))
{
this.InOpenList(point3, list[i]);
}
else
{
this.NotInOpenList(point3, list[i], currentData);
}
}
if (currentData.openList.Contains(cEnd))
{
return this.GetEndPath(cEnd);
}
if (list.Count == 0)
{
return this.GetEndPath(point3);
}
}
Debug.LogError("目标点不可到达");
return this.GetEndPath(cEnd);
}

public List<Point> FindSurroundPoints(Point fStart, float radius, ulong selfId, Point fObstacle, FactoryAStarData tempFindData)
{
List<Point> list = new List<Point>();
Vector2 vecIndex = fStart.VecIndex;
if ((tempFindData.starMap.ContainsKey(this.vecL + vecIndex) && (tempFindData.starMap[this.vecL + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecL + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecR + vecIndex) && (tempFindData.starMap[this.vecR + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecR + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecU + vecIndex) && (tempFindData.starMap[this.vecU + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecU + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecD + vecIndex) && (tempFindData.starMap[this.vecD + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecD + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecLU + vecIndex) && (tempFindData.starMap[this.vecLU + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecLU + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecRU + vecIndex) && (tempFindData.starMap[this.vecRU + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecRU + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecLD + vecIndex) && (tempFindData.starMap[this.vecLD + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecLD + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecRD + vecIndex) && (tempFindData.starMap[this.vecRD + vecIndex].occupyerId != 0)) && (tempFindData.starMap[this.vecRD + vecIndex].occupyerId == fObstacle.occupyerId))
{
return (list = new List<Point>());
}
if ((tempFindData.starMap.ContainsKey(this.vecL + vecIndex) && (tempFindData.starMap[this.vecL + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecL + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecL + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecR + vecIndex) && (tempFindData.starMap[this.vecR + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecR + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecR + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecU + vecIndex) && (tempFindData.starMap[this.vecU + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecU + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecU + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecD + vecIndex) && (tempFindData.starMap[this.vecD + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecD + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecD + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecLU + vecIndex) && (tempFindData.starMap[this.vecLU + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecLU + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecLU + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecRU + vecIndex) && (tempFindData.starMap[this.vecRU + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecRU + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecRU + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecLD + vecIndex) && (tempFindData.starMap[this.vecLD + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecLD + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecLD + vecIndex]);
}
if ((tempFindData.starMap.ContainsKey(this.vecRD + vecIndex) && (tempFindData.starMap[this.vecRD + vecIndex].occupyerId == 0)) && !tempFindData.closeList.Contains(tempFindData.starMap[this.vecRD + vecIndex]))
{
list.Add(tempFindData.starMap[this.vecRD + vecIndex]);
}
return list;
}

public FactoryAStarData GetAStarMapById(ulong targetId)
{
FactoryAStarData currentDataById = new FactoryAStarData();
currentDataById = this.GetCurrentDataById(targetId);
currentDataById.ReFreshMap();
this.currentData = currentDataById;
return currentDataById;
}

private float GetCalcDistance(Point Rpoint1, Point Rpoint2)
{
float introduced2 = Mathf.Pow(Rpoint1.VecIndex.x - Rpoint2.VecIndex.x, 2f);
if (Mathf.Ceil(Mathf.Sqrt(introduced2 + Mathf.Pow(Rpoint1.VecIndex.y - Rpoint2.VecIndex.y, 2f))) > 1f)
{
return 14f;
}
return 10f;
}

public FactoryAStarData GetCurrentDataById(ulong fTargetId)
{
FactoryAStarData data = new FactoryAStarData();
if ((this.factoryDic.Count > 0) && this.factoryDic.ContainsKey(fTargetId))
{
return this.factoryDic[fTargetId];
}
this.factoryDic.Add(fTargetId, data);
return data;
}

private List<Vector2> GetEndPath(Point end)
{
if (end != null)
{
this.endVecList.Add(end.VecIndex);
}
if (end.ParentPoint != null)
{
this.GetEndPath(end.ParentPoint);
}
return this.endVecList;
}

private void InOpenList(Point Rpoint1, Point Rpoint2)
{
float calcDistance = this.GetCalcDistance(Rpoint1, Rpoint2);
if (Rpoint2.G >= (Rpoint1.G + calcDistance))
{
Rpoint2.ParentPoint = Rpoint1;
Rpoint2.G = Rpoint1.G + calcDistance;
Rpoint2.CalcF();
}
}

private void NotInOpenList(Point Rpoint1, Point Rpoint2, FactoryAStarData tempFindData)
{
Rpoint2.ParentPoint = Rpoint1;
Rpoint2.G = Rpoint1.G + this.GetCalcDistance(Rpoint1, Rpoint2);
Rpoint2.CalcF();
tempFindData.openList.Add(Rpoint2);
}

public void SetDynamicObstacle(Vector3 targetPos, ulong _occupyerId)
{
if (this.currentData != null)
{
targetPos = new Vector3(targetPos.x, 0f, targetPos.z);
Vector2 vecIndex = this.currentData.FindPointByPos(targetPos).VecIndex;
if (this.currentData.starMap[vecIndex].occupyerId == 0)
{
if (this.currentData.obstacleDic.ContainsKey(_occupyerId))
{
this.currentData.obstacleDic[_occupyerId].occupyerId = 0L;
this.currentData.obstacleDic[_occupyerId] = this.currentData.starMap[vecIndex];
this.currentData.starMap[vecIndex].occupyerId = _occupyerId;
}
else
{
this.currentData.starMap[vecIndex].occupyerId = _occupyerId;
this.currentData.obstacleDic.Add(_occupyerId, this.currentData.starMap[vecIndex]);
}
}
}
else
{
Debug.LogError("currentData is null");
}
}

public static A_Star Instance
{
get
{
if (_instance == null)
{
Debug.LogError("new astar");
_instance = new A_Star();
}
return _instance;
}
}
}
}