protected void DoANode(int x, int y, int ex, int ey, float NG, AStarNode PNode, List<AStarNode> OpenList, List<AStarNode> CloseList)
{
if (Data[x, y] != 0)
return;
AStarNode NNode = new AStarNode(x, y);
NNode.BuildGHF(NG, ex, ey);
int Index;
// 查找OpenList
Index = FindIndex(OpenList, x, y);
if (Index > 0)
{
AStarNode NodeInOpenList = OpenList[Index];
// 如果OpenList中的路径还比当前大，那更新他
if (NNode.F < NodeInOpenList.F)
{
Console.WriteLine("F**k This");
NodeInOpenList.F = NNode.F;
NodeInOpenList.H = NNode.H;
NodeInOpenList.G = NNode.G;
AStarNode OldParentNode = NodeInOpenList.ParentNode;
NodeInOpenList.ParentNode = PNode;
PNode.ChildrenNode.Add(NodeInOpenList);
if (OldParentNode != null)
{
Index = FindIndex(OldParentNode.ChildrenNode, x, y);
OldParentNode.ChildrenNode.RemoveAt(Index);
}
NodeInOpenList.UpdateG();
}
return;
}
// 查找CloseList,如果在CloseList中，则不管他
Index = FindIndex(CloseList, x, y);
if (Index > 0)
{
// Just do nothing.
return;
AStarNode NodeInCloseList = CloseList[Index];
if (NNode.F <= NodeInCloseList.F)
{
NodeInCloseList.F = NNode.F;
NodeInCloseList.H = NNode.H;
NodeInCloseList.G = NNode.G;
AStarNode OldParentNode = NodeInCloseList.ParentNode;
NodeInCloseList.ParentNode = PNode;
PNode.ChildrenNode.Add(NodeInCloseList);
CloseList.RemoveAt(Index);
OpenList.Add(NodeInCloseList);
if (OldParentNode != null)
{
Index = FindIndex(OldParentNode.ChildrenNode, x, y);
OldParentNode.ChildrenNode.RemoveAt(Index);
}
NodeInCloseList.UpdateG();
return;
}
}
NNode.ParentNode = PNode;
PNode.ChildrenNode.Add(NNode);
OpenList.Add(NNode);
}
public List<__Point> GetPath(int x0, int y0, int x1, int y1)
{
List<__Point> Path = new List<__Point>();
List<AStarNode> OpenList = new List<AStarNode>();
List<AStarNode> CloseList = new List<AStarNode>();
AStarNode FirstNode = new AStarNode(x0, y0);
FirstNode.BuildGHF(0.0f, x1, y1);
OpenList.Add(FirstNode);
while (OpenList.Count > 0)
{
int MinIndex = 0;
float MinF = float.MaxValue;
AStarNode MinNode = null;
int i = 0;
foreach (var Node in OpenList)
{
if (Node.F < MinF)
{
MinNode = Node;
MinIndex = i;
MinF = Node.F;
}
i++;
}
int x = MinNode.x;
int y = MinNode.y;
if (x == x1 && y == y1)
{
while (MinNode != null)
{
__Point pp = new __Point();
pp.x = MinNode.x;
pp.y = MinNode.y;
Path.Add(pp);
MinNode = MinNode.ParentNode;
}
Console.WriteLine("Find.");
return Path;
}
OpenList.RemoveAt(MinIndex);
CloseList.Add(MinNode);
if (x - 1 >= 0)
DoANode(x - 1, y, x1, y1, MinNode.G + 1.0f, MinNode, OpenList, CloseList);
if (x + 1 < 50)
DoANode(x + 1, y, x1, y1, MinNode.G + 1.0f, MinNode, OpenList, CloseList);
if (y - 1 >= 0)
DoANode(x, y - 1, x1, y1, MinNode.G + 1.0f, MinNode, OpenList, CloseList);
if (y + 1 < 50)
DoANode(x, y + 1, x1, y1, MinNode.G + 1.0f, MinNode, OpenList, CloseList);
}
Console.WriteLine("Can\'t Find.");
return Path;
}
