【算法】A* 寻路 可视化
如下图 【寻路图A】
使用A*算法，需要将地图抽象成一个个方块，蓝色代表不可以动【墙】，黄色为起始点，红色为目标点。其地图的二维坐标如图所示，每一个单位为1米
A*的基本公式为 F(n)=G(n)+H(n)
F：初始状态到目标状态n的代价估计【起始点到n的代价估计】
G：状态空间中初始状态到n的实际代价【n为中间某点，n到其parent的实际距离，n为parent的最小F所得】
H：从状态n到目标状态最佳路径的估计代价【n到B的，X之差的绝对值与Y之差的绝对值 之和】
算法的基本思路
首先我们有一个起始点，一个目标点，还有一些墙，
并声明一个开启【开启列表】一个【关闭列表】
【开启列表】：当前待判断点，
【关闭列表】：已经判断过的点，不再参与寻路搜索计算
1.开始时我们把起始点加入开启列表，然后获取起始点周围所有可达到的点，将它们的parent设置为起始点，然后分别计算它们的FGH值，并把这些点加入开启列表中，最后将起始点从开启列表中移除，再加入关闭列表。
2.接着遍历开始列表，寻找开始列表中F(到达目标点的预计代价)值最小的点，对此点进行如下操作
将此点加入关闭列表并从开始列表中移除
寻找此点周围所有【按你自己的规则】【可达到的点】，并移除已经加入关闭列表中的部分，然后计算这些新点的FGH值，这里需要重点注意的是更新parent的操作！
每个【可达到的点】都有自己的FGH，每次计算完一个【可达到的点】的G值后，重新计算下此点到该【可达到的点】的G值，比较这两个G值，若后者较小，这更新当前【可达到的点】的parent为此点
当此点所有【可达到的点】计算完成后，重复2，重新遍历【开启列表】寻找F值最小的点，并标记为此点，
终止的条件就是
1.开启列表为空：目标点无法到达
2.开启列表中包含目标点：已经到达
对于结果，我们每一个点都计算出了其最小代价的parent，我们根据目标点的parent依次向上寻找直到起始点，即为最佳路径
举一个代码示例如下：【by siki】
每个路点绑定如下point
public class Point {
public Point Parent { get; set; }
public float F { get; set; }
public float G { get; set; }
public float H { get; set; }
public int X { get; set; }
public int Y { get; set; }
public bool IsWall { get; set; }
public Point(int X,int Y,Point Parent=null) {
this.X = X;
this.Y = Y;
this.Parent = Parent;
IsWall = false;
}
public void UpdateParent(Point parent,float g) {
this.Parent = parent;
this.G = g;
this.F = G + H;
}
}
新建另一继承自Mono的脚本，挂到任意物体上
这里我们固定地图，固定障碍物，固定起始点，定义规则【正常可走斜角，若斜角周围有障碍物则不可通过】如上【寻路图A】
定义好地图的宽高等信息
public int mapWidth=8;
public int mapHeight = 6;
private Point[,] map = new Point[8, 6];
生成地图
private void InitMap() {
for (int x = 0; x < mapWidth; x++) {
for (int y = 0; y < mapHeight; y++) {
map[x, y] = new Point(x, y);
}
}
//设置障碍物
map[4, 2].IsWall = true;
map[4, 3].IsWall = true;
map[4, 4].IsWall = true;
map[4, 5].IsWall = true;
}
寻找路径
private void FindPath(Point start,Point end) {
List<Point> openList = new List<Point> { };
List<Point> closeList = new List<Point> { };
openList.Add(start);
while (openList.Count > 0) {
Point point = FindMinFOfPoint(openList);
openList.Remove(point);
closeList.Add(point);
List<Point> surroundPoints = GetSurroundPoints(point);
//过滤
PointsFilter(surroundPoints, closeList);
foreach (Point s in surroundPoints) {
if (openList.Contains(s)) {
float nowG = CalcG(s, point);
if (nowG < s.G) {
s.UpdateParent(point, nowG);
}
}
else {
s.Parent = point;
//Debug.Log(s.X + " " + s.Y + "--" + point.X + " " + point.Y);
CalcF(s, end);
openList.Add(s);
}
}
//判断end是否在开启列表里
if (openList.Contains(end)) {
break;
}
}
}
寻找周围点
//找到某point周围所有可达到的点
private List<Point> GetSurroundPoints(Point point) {
//上下左右的四个点
Point up = null, down = null, left = null, right = null;
if (point.Y < mapHeight - 1) {
up = map[point.X, point.Y + 1];
}
if (point.Y >0) {
down = map[point.X, point.Y - 1];
}
if (point.X < mapWidth - 1) {
right = map[point.X + 1, point.Y];
}
if (point.X > 0) {
left = map[point.X - 1, point.Y];
}
//四个角
Point upLeft = null, upRight = null, downLeft = null, downRight = null;
if (up != null && left != null) {
upLeft = map[point.X - 1, point.Y + 1];
}
if (up != null && right != null) {
upRight = map[point.X + 1, point.Y + 1];
}
if (down != null && left != null) {
downLeft = map[point.X - 1, point.Y - 1];
}
if (down != null && right != null) {
downRight = map[point.X + 1, point.Y - 1];
}
//构造集合
List<Point> list = new List<Point> { };
//不是墙，加入集合
if (down != null && down.IsWall == false) {
list.Add(down);
}
if (up != null && up.IsWall == false) {
list.Add(up);
}
if (left != null && left.IsWall == false) {
list.Add(left);
}
if (right != null && right.IsWall == false) {
list.Add(right);
}
//不是墙，且没有障碍物阻拦，加入集合
if (upLeft != null && upLeft.IsWall == false && left.IsWall == false && up.IsWall == false) {
list.Add(upLeft);
}
if (upRight != null && upRight.IsWall == false && up.IsWall == false && right.IsWall == false) {
list.Add(upRight);
}
if (downLeft != null && downLeft.IsWall == false && down.IsWall == false && left.IsWall == false) {
list.Add(downLeft);
}
if (downRight != null && downRight.IsWall == false && down.IsWall == false && right.IsWall == false) {
list.Add(downRight);
}
return list;
}
去除已经处于关闭列表中的 周围可到达点
private void PointsFilter(List<Point> src,List<Point> closeList) {
foreach (Point p in closeList) {
if (src.Contains(p)) {
src.Remove(p);
}
}
}
寻找开启列表中 F值最小的点
private Point FindMinFOfPoint(List<Point> openList) {
float f = float.MaxValue;
Point temp = null;
foreach (Point p in openList) {
if (p.F < f) {
temp = p;
f = p.F;
}
}
return temp;
}
//计算F值
private void CalcF(Point now,Point end) {
//F=G+H
float h = Mathf.Abs(end.X - now.X) + Mathf.Abs(end.Y - now.Y);
float g = 0;
if (now.Parent == null) {
g = 0;
}
else {
g=Vector2.Distance(new Vector2(now.X, now.Y), new Vector2(now.Parent.X, now.Parent.Y)) + now.Parent.G;
}
float f = g + h;
now.F = f;
now.G = g;
now.H = h;
}
//计算G值
private float CalcG(Point now,Point parent) {
return Vector2.Distance(new Vector2(now.X, now.Y), new Vector2(parent.X, parent.Y)) + parent.G;
}
打印结果
private void ShowPath(Point start,Point end) {
Point temp = end;
while (true) {
Debug.Log(temp.X + " " + temp.Y);
if (temp.Parent == null) {
break;
}
temp = temp.Parent;
}
}
最后开启算法
void Start() {
InitMap();
Point start = map[2, 3];
Point end = map[6, 3];
FindPath(start, end);
ShowPath(start, end);
}
执行后打印结果如下：6 3，6 2，5 1，4 1，3 1，3 2，2 3
笔者笔记原文链接