public void InitMap(int[,] map)
{
this.mapWidth = map.GetLength(0);
this.mapHeight = map.GetLength(1);
nodes = new AstarNode[mapWidth, mapHeight];
for (int i = 0; i < mapWidth; i++)
{
for (int j = 0; j < mapHeight; j++)
{
AstarNode node = new AstarNode(i, j, map[i, j] == 0 ? NodeType.CanWalk : NodeType.Obstacle);
nodes[i, j] = node;
}
}
}
private void FindNearlyNodeToOpenList(int x, int y, double g, AstarNode preNode)
{
if (!IsPointInMap(x, y))
{
return;
}
AstarNode tempNode = nodes[x, y];
if (tempNode == null || tempNode.NodeType == NodeType.Obstacle ||
closeList.Contains(tempNode))
{
return;
}
if (openList.Contains(tempNode))
{
//修正g 及前驱节点
var gOld = tempNode.G;
var gNew = preNode.G + g;
if (gNew < gOld)
{
tempNode.PreNode = preNode;
tempNode.G = preNode.G + g;
tempNode.H = AstarNode.GetManhattanDistance(tempNode, end);
//openList.Add(tempNode);//已经在openList中了
return;
}
else
{
//不需要修正，保留
return;
}
//return;
}
else
{
tempNode.PreNode = preNode;
tempNode.G = preNode.G + g;
//启发函数
tempNode.H = AstarNode.GetManhattanDistance(tempNode, end);
openList.Add(tempNode);
}
}
public static double GetManhattanDistance(AstarNode node1, AstarNode node2)
{
return(Math.Abs(node1.x - node2.x) + Math.Abs(node1.y - node2.y));
}