【算法】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

笔者笔记原文链接