class Node//结点
{
public bool isOb = false;
public Node Parent;
public int F;
public int G;
public int H;
public int X;
public int Y;
public bool isLine = false;
}
class Map//地图
{
public Node[][] nodes;
public int XLength;
public int YLength;
public void Clear()
{
for (int i = 0; i < XLength; i++)
{
for (int j = 0; j < YLength; j++)
{
Node node = nodes[i][j];
node.F = 0;
node.G = 0;
node.H = 0;
node.isLine = false;
node.Parent = null;
}
}
}
//输出地图和寻路结果信息
public void PrintMap()
{
for (int i = 0; i < YLength; i++)
{
if (i == 0)
{
Console.Write(" " + " ");
for (int k = 0; k < XLength; k++)
{
Console.Write(k + " ");
}
Console.WriteLine();
}
for (int j = 0; j < XLength; j++)
{
if (j == 0)
{
Console.Write(i + " ");
}
Node node = nodes[j][i];
if (node.isLine)
{
Console.ForegroundColor = ConsoleColor.Red;
}
else
{
Console.ForegroundColor = ConsoleColor.Green;
}
if (node.isOb)
{
Console.ForegroundColor = ConsoleColor.Yellow;
Console.Write("#" + " ");
}
else
{
Console.Write("*" + " ");
}
Console.ForegroundColor = ConsoleColor.White;
}
Console.WriteLine();
}
}
}
class AXin//寻找
{
private List<Node> openList;
private List<Node> closeList;
private Map map;
private Node startNode;
private Node endNode;
private Node preNode;
public AXin()
{
openList = new List<Node>();
closeList = new List<Node>();
}
public void Search(int startX, int startY,int endX,int endY)
{
if (map == null)
{
Console.WriteLine("请设置地图");
return;
}
openList.Clear();
closeList.Clear();
map.Clear();
startNode = map.nodes[startX][startY];
if (startNode.isOb)
{
Console.WriteLine("开始位置为障碍物！");
return;
}
endNode = map.nodes[endX][endY];
openList.Add(startNode);
Search();
}
private void Search()
{
bool isSearch = false;
Node tempNode = null;
while (openList.Count > 0 && !isSearch)
{
tempNode = GetLowerFNode();
openList.Remove(tempNode);
closeList.Add(tempNode);
preNode = tempNode; //用来计算其他的结点的F
int x = tempNode.X, y = tempNode.Y;
//遍历周边结点8个 障碍和在close的 忽略
for (int i = -1; i < 2; i++)//x
{
for (int j = -1; j < 2; j++)//y
{
x = tempNode.X + i;
y = tempNode.Y + j;
if (x < 0 || x >= map.XLength || y < 0 || y >= map.YLength)
{
//越界了
continue;
}
if (i == 0 && j == 0) continue;//中心点
Node node = map.nodes[x][y];
//障碍跳过
if (node.isOb) continue;
//在close里面的跳过
if (isInClose(node)) continue;
//检查对角线 两边有障碍物的不能过
if (i != 0 && j != 0)
{
Node oneNode = map.nodes[tempNode.X][y];
if (oneNode.isOb) continue;
oneNode = map.nodes[x][tempNode.Y];
if (oneNode.isOb) continue;
}
//已经在openList
if (isInOpen(node))
{
//判断是否需要重新算 F
int offsetX = Math.Abs(node.X - preNode.X);
int offsetY = Math.Abs(node.Y - preNode.Y);
int len = 10;
if (offsetX + offsetY == 2)
{
len = 14;
}
if (preNode.G + len < node.G)
{
//重新算
node.G = preNode.G + len;
node.F = node.G + node.H;
node.Parent = preNode;
}
}
else {
//计算F 加进去 设置父物体
CalF(node);
openList.Add(node);
node.Parent = preNode;
if (node == endNode)
{
isSearch = true;
}
}
}
}
}
if (isSearch)
{
Node go = endNode;
//输出结果
while (go != startNode)
{
//Console.WriteLine("X：" + go.X +"||" + "Y：" + go.Y);
//Console.WriteLine("权重F="+go.F +" G="+go.G + " H="+go.H);
go.isLine = true;//标志路径
go = go.Parent;
}
go.isLine = true;
PrintMap();
}
else {
Console.WriteLine("没有找到！");
}
}
public void SetMap(Map m)
{
map = m;
}
//计算F的值
private void CalF(Node node)
{
int offsetX = Math.Abs(node.X - preNode.X);
int offsetY = Math.Abs(node.Y - preNode.Y);
if (offsetX + offsetY == 2)
{
node.G += 14;
}
else {
node.G += 10;
}
//计算方向
int DirectX = Math.Sign(endNode.X - node.X);
int DirectY = Math.Sign(endNode.Y - node.Y);
//计算H的X
int indexX = node.X;
while (indexX != endNode.X)
{
if (map.nodes[indexX][node.Y].isOb == false)
{
node.H += 10;
}
indexX += DirectX;
}
//计算H的Y
int indexY = node.Y;
while (indexY != endNode.Y)
{
if (map.nodes[node.X][indexY].isOb == false)
{
node.H += 10;
}
indexY += DirectY;
}
node.F = node.G + node.H;
}
private bool isInClose(Node node)
{
//可优化 在节点中保存信息
bool isIn = false;
foreach (var i in closeList)
{
if (node == i)
isIn = true;
}
return isIn;
}
private bool isInOpen(Node node)
{
//可优化 在节点中保存信息
bool isIn = false;
foreach (var i in openList)
{
if (node == i)
isIn = true;
}
return isIn;
}
private Node GetLowerFNode()
{
//可优化，用最小堆
if (openList.Count < 1) {
Console.WriteLine("怎么会没有了呢!");
return null;
}
Node node = openList[0];
for (int i = 1; i < openList.Count; i++)
{
if (openList[i].F < node.F)
{
node = openList[i];
}
}
return node;
}
public void PrintMap()
{
map.PrintMap();
}
}首先是将寻路区域分作一个个小方格（我这里是），这是基础
还需要一个评估节点承前启后的消耗，也就是F的计算，G是来到这个节点需要的消耗，H是预计到达目标点的消耗，F是这两个值的和
一个openList记录需要检查周边的节点，closeList记录已经去过的节点（就不用再访问了）
逻辑是这样子的，从openList取出F值最小的加入到closeList中，一开始起点结点会放到openList中
然后遍历这个节点周边的8个节点，障碍物的、在closeList的（访问过的）、斜角边附近有障碍物的都不能通过，直接不做处理；其他的做处理，如果在openList的，要检查当前路径到达这个节点的消耗是否优于以前的，是的话就更换一下F和G值，如果不是，新节点没访问过的，计算一下FGH，加入进去openList。
一直进行下去，直到找到了目标，它要加入到openList，或则openList里面一个元素都没有了。
G - 一个横 一个竖 都是10消耗，一个斜线14消耗（勾股定理）
H - 距离目标横竖偏移和*10（除开障碍物）
F = G + H