- 从起点 A 开始, 把它作为待处理的方格存入一个"开启列表", 开启列表就是一个等待检查方格的列表.
- 寻找起点 A 周围可以到达的方格, 将它们放入"开启列表", 并设置它们的"父方格"为 A.
- 从"开启列表"中删除起点 A, 并将起点 A 加入"关闭列表", "关闭列表"中存放的都是不需要再次检查的方格
图中浅绿色描边的方块表示已经加入 "开启列表" 等待检查. 淡蓝色描边的起点 A 表示已经放入"关闭列表" , 它不需要再执行检查.从 "开启列表" 中找出相对最靠谱的方块, 什么是最靠谱? 它们通过公式 F=G+H 来计算.
G 表示从起点 A 移动到网格上指定方格的移动耗费 (可沿斜方向移动).
H 表示从指定的方格移动到终点 B 的预计耗费 (H 有很多计算方法, 这里我们设定只可以上下左右移动).
我们假设横向移动一个格子的耗费为 10, 为了便于计算, 沿斜方向移动一个格子耗费是 14. 为了更直观的展示如何运算 FGH, 图中方块的左上角数字表示 F, 左下角表示 G, 右下角表示 H.
从 "开启列表" 中选择 F 值最低的方格 C (绿色起始方块 A 右边的方块), 然后对它进行如下处理:
- 把它从 "开启列表" 中删除, 并放到 "关闭列表" 中.
- 检查它所有相邻并且可以到达 (障碍物和 "关闭列表" 的方格都不考虑) 的方格. 如果这些方格
还不在 "开启列表" 里的话, 将它们加入 "开启列表", 计算这些方格的 G, H 和 F 值各是多少, 并设置它们的 "父方格" 为 C.
- 如果某个相邻方格 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.
- 右边已经右上方的都是墙, 所以不考虑, 但为什么右下角的没有被加进 "开启列表" 呢? 因为如
- C 下面的那块也不可以走, 想要到达 C 右下角的方块就需要从 "方块的角" 走了, 在程序中设置是否允许这样走. (图中的示例不允许这样走)
就这样, 我们从 "开启列表" 找出 F 值最小的, 将它从 "开启列表" 中移掉, 添加到 "关闭列表".
再继续找出它周围可以到达的方块, 如此循环下去...
那么什么时候停止呢? —— 当我们发现 "开始列表" 里出现了目标终点方块的时候, 说明路径已经被找到.
如何找回路径
如上图所示, 除了起始方块, 每一个曾经或者现在还在 "开启列表" 里的方块, 它都有一个 "父方块", 通过 "父方块" 可以索引到最初的 "起始方块", 这就是路径.
using System;
using System.Collections.Generic;
namespace ConsoleApplication2
{
class Program
{
static void Main(string[] args)
{
test mytest = new test();
//定义出发位置
Point pa = new Point();
pa.x = 1;
pa.y = 3;
//定义目的地
Point pb = new Point();
pb.x = 8;
pb.y = 8;
mytest.FindWay(pa, pb);
mytest.PrintMap();
Console.ReadKey();
}
}
class test
{
//数组用1表示可通过,0表示障碍物
byte[,] R = new byte[10, 10] {
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 0, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 },
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }
};
//开启列表
List<Point> Open_List = new List<Point>();
//关闭列表
List<Point> Close_List = new List<Point>();
//从开启列表查找F值最小的节点
private Point GetMinFFromOpenList()
{
Point Pmin = null;
foreach (Point p in Open_List)
if (Pmin == null || Pmin.G + Pmin.H > p.G + p.H)
Pmin = p;
return Pmin;
}
//判断一个点是否为障碍物
private bool IsBar(Point p, byte[,] map)
{
if (map[p.y, p.x] == 0) return true;
return false;
}
//判断关闭列表是否包含一个坐标的点
private bool IsInCloseList(int x, int y)
{
foreach (Point p in Close_List) if (p.x == x && p.y == y) return true;
return false;
}
//从关闭列表返回对应坐标的点
private Point GetPointFromCloseList(int x, int y)
{
foreach (Point p in Close_List) if (p.x == x && p.y == y) return p;
return null;
}
//判断开启列表是否包含一个坐标的点
private bool IsInOpenList(int x, int y)
{
foreach (Point p in Open_List) if (p.x == x && p.y == y) return true;
return false;
}
//从开启列表返回对应坐标的点
private Point GetPointFromOpenList(int x, int y)
{
foreach (Point p in Open_List) if (p.x == x && p.y == y) return p;
return null;
}
//计算某个点的G值
private int GetG(Point p)
{
if (p.father == null) return 0;
if (p.x == p.father.x || p.y == p.father.y) return p.father.G + 10;
else return p.father.G + 14;
}
//计算某个点的H值
private int GetH(Point p, Point pb)
{
return Math.Abs(p.x - pb.x) + Math.Abs(p.y - pb.y);
}
//检查当前节点附近的节点
private void CheckP8(Point p0, byte[,] map, Point pa, ref Point pb)
{
for (int xt = p0.x - 1; xt <= p0.x + 1; xt++)
{
for (int yt = p0.y - 1; yt <= p0.y + 1; yt++)
{
//排除超过边界和等于自身的点
if ((xt >= 0 && xt < 10 && yt >= 0 && yt < 10) && !(xt == p0.x && yt == p0.y))
{
//排除障碍点和关闭列表中的点
if (map[yt, xt] != 0 && !IsInCloseList(xt, yt))
{
if (IsInOpenList(xt, yt))
{
Point pt = GetPointFromOpenList(xt, yt);
int G_new = 0;
if (p0.x == pt.x || p0.y == pt.y) G_new = p0.G + 10;
else G_new = p0.G + 14;
if (G_new < pt.G)
{
Open_List.Remove(pt);
pt.father = p0;
pt.G = G_new;
Open_List.Add(pt);
}
}
else
{
//不在开启列表中
Point pt = new Point();
pt.x = xt;
pt.y = yt;
pt.father = p0;
pt.G = GetG(pt);
pt.H = GetH(pt, pb);
Open_List.Add(pt);
}
}
}
}
}
}
public void FindWay(Point pa, Point pb)
{
Open_List.Add(pa);
while (!(IsInOpenList(pb.x, pb.y) || Open_List.Count == 0))
{
Point p0 = GetMinFFromOpenList();
if (p0 == null) return;
Open_List.Remove(p0);
Close_List.Add(p0);
CheckP8(p0, R, pa, ref pb);
}
Point p = GetPointFromOpenList(pb.x, pb.y);
while (p.father != null)
{
p = p.father;
R[p.y, p.x] = 3;
}
}
public void SaveWay(Point pb)
{
Point p = pb;
while (p.father != null)
{
p = p.father;
R[p.y, p.x] = 3;
}
}
public void PrintMap()
{
for (int a = 0; a < 10; a++)
{
for (int b = 0; b < 10; b++)
{
if (R[a, b] == 1) Console.Write("█");
else if (R[a, b] == 3) Console.Write("★");
else if (R[a, b] == 4) Console.Write("○");
else Console.Write(" ");
}
Console.Write("\n");
}
}
}
class Point
{
public int y;
public int x;
public int G;
public int H;
public Point() { }
public Point(int x0, int y0, int G0, int H0, Point F)
{
x = x0;
y = y0;
G = G0;
H = H0;
father = F;
}
public Point father;
}
}
或者:
using System;
using System.Collections.Generic;
using System.Linq;
namespace Maze
{
class Program
{
static void Main(string[] args)
{
int[,] array = {
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
{ 1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},
{ 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
{ 1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},
{ 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},
{ 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},
{ 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},
{ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
};
Maze maze = new Maze(array);
Point start = new Point(1, 1);
Point end = new Point(6, 10);
var parent = maze.FindPath(start, end, false);
Console.WriteLine("Print path:");
while (parent != null)
{
Console.WriteLine(parent.X + ", " + parent.Y);
parent = parent.ParentPoint;
}
Console.ReadKey();
}
}
class Maze
{
public const int OBLIQUE = 14;
public const int STEP = 10;
public int[,] MazeArray { get; private set; }
List<Point> CloseList;
List<Point> OpenList;
public Maze(int[,] maze)
{
this.MazeArray = maze;
OpenList = new List<Point>(MazeArray.Length);
CloseList = new List<Point>(MazeArray.Length);
}
public Point FindPath(Point start, Point end, bool IsIgnoreCorner)
{
OpenList.Add(start);
while (OpenList.Count != 0)
{
//找出F值最小的点
var tempStart = OpenList.MinPoint();
OpenList.RemoveAt(0);
CloseList.Add(tempStart);
//找出它相邻的点
var surroundPoints = SurrroundPoints(tempStart, IsIgnoreCorner);
foreach (Point point in surroundPoints)
{
if (OpenList.Exists(point))
//计算G值, 如果比原来的大, 就什么都不做, 否则设置它的父节点为当前点,并更新G和F
FoundPoint(tempStart, point);
else
//如果它们不在开始列表里, 就加入, 并设置父节点,并计算GHF
NotFoundPoint(tempStart, end, point);
}
if (OpenList.Get(end) != null)
return OpenList.Get(end);
}
return OpenList.Get(end);
}
private void FoundPoint(Point tempStart, Point point)
{
var G = CalcG(tempStart, point);
if (G < point.G)
{
point.ParentPoint = tempStart;
point.G = G;
point.CalcF();
}
}
private void NotFoundPoint(Point tempStart, Point end, Point point)
{
point.ParentPoint = tempStart;
point.G = CalcG(tempStart, point);
point.H = CalcH(end, point);
point.CalcF();
OpenList.Add(point);
}
private int CalcG(Point start, Point point)
{
int G = (Math.Abs(point.X - start.X) + Math.Abs(point.Y - start.Y)) == 2 ? STEP : OBLIQUE;
int parentG = point.ParentPoint != null ? point.ParentPoint.G : 0;
return G + parentG;
}
private int CalcH(Point end, Point point)
{
int step = Math.Abs(point.X - end.X) + Math.Abs(point.Y - end.Y);
return step * STEP;
}
//获取某个点周围可以到达的点
public List<Point> SurrroundPoints(Point point, bool IsIgnoreCorner)
{
var surroundPoints = new List<Point>(9);
for (int x = point.X - 1; x <= point.X + 1; x++)
for (int y = point.Y - 1; y <= point.Y + 1; y++)
{
if (CanReach(point, x, y, IsIgnoreCorner))
surroundPoints.Add(x, y);
}
return surroundPoints;
}
//在二维数组对应的位置不为障碍物
private bool CanReach(int x, int y)
{
return MazeArray[x, y] == 0;
}
public bool CanReach(Point start, int x, int y, bool IsIgnoreCorner)
{
if (!CanReach(x, y) || CloseList.Exists(x, y))
return false;
else
{
if (Math.Abs(x - start.X) + Math.Abs(y - start.Y) == 1)
return true;
//如果是斜方向移动, 判断是否 "拌脚"
else
{
if (CanReach(Math.Abs(x - 1), y) && CanReach(x, Math.Abs(y - 1)))
return true;
else
return IsIgnoreCorner;
}
}
}
}
//Point 类型
public class Point
{
public Point ParentPoint { get; set; }
public int F { get; set; } //F=G+H
public int G { get; set; }
public int H { get; set; }
public int X { get; set; }
public int Y { get; set; }
public Point(int x, int y)
{
this.X = x;
this.Y = y;
}
public void CalcF()
{
this.F = this.G + this.H;
}
}
//对 List<Point> 的一些扩展方法
public static class ListHelper
{
public static bool Exists(this List<Point> points, Point point)
{
foreach (Point p in points)
if ((p.X == point.X) && (p.Y == point.Y))
return true;
return false;
}
public static bool Exists(this List<Point> points, int x, int y)
{
foreach (Point p in points)
if ((p.X == x) && (p.Y == y))
return true;
return false;
}
public static Point MinPoint(this List<Point> points)
{
points = points.OrderBy(p => p.F).ToList();
return points[0];
}
public static void Add(this List<Point> points, int x, int y)
{
Point point = new Point(x, y);
points.Add(point);
}
public static Point Get(this List<Point> points, Point point)
{
foreach (Point p in points)
if ((p.X == point.X) && (p.Y == point.Y))
return p;
return null;
}
public static void Remove(this List<Point> points, int x, int y)
{
foreach (Point point in points)
{
if (point.X == x && point.Y == y)
points.Remove(point);
}
}
}
}