private AStarNode GetOpenHeapNode(int nodeX, int nodeY, BinaryHeap <AStarNode> OpenHeap, out int index)
{
index = -1;
int i;
for (i = 0; i < OpenHeap.Count; i++)
{
AStarNode n = OpenHeap[i];
if (n == null)
{
continue;
}
if (n.X == nodeX && n.Y == nodeY)
{
index = i;
return(n);
}
}
return(null);
}
public MapPath FindPath(int startX, int startY, int endX, int endY, bool useHeuristic = true)
{
if (field == null)
{
return(null);
}
UseHeuristic = useHeuristic;
List <AStarNode> Closed = new List <AStarNode>();
BinaryHeap <AStarNode> OpenHeap = new BinaryHeap <AStarNode>(fieldWidth * fieldHeight);
AStarNode Root;
Root = new AStarNode();
Root.parent = null;
Root.X = startX;
Root.Y = startY;
Root.G = 0;
int offX = Math.Abs(Root.X - endX);
int offY = Math.Abs(Root.Y - endY);
if (UseHeuristic == false)
{
Root.H = 0;
}
else
{
if (offX > offY)
{
Root.H = 14 * offY + 10 * (offX - offY);
}
else
{
Root.H = 14 * offX + 10 * (offY - offX);
}
}
OpenHeap.Add(Root.F, Root);
int res = ProcessLowestNode(endX, endY, Closed, OpenHeap);
while (res == 0)
{
res = ProcessLowestNode(endX, endY, Closed, OpenHeap);
}
if (res == -1)
{
return(null);
}
AStarNode node = null;
if (res == 1)
{
node = GetClosedNode(endX, endY, Closed);
}
else if (res == 2)
{
node = Closed[Closed.Count - 1];
}
MapPath result = new MapPath();
result.BuildFromFinalAStarNode(node);
return(result);
}
private sbyte ProcessLowestNode(int endX, int endY, List <AStarNode> Closed, BinaryHeap <AStarNode> OpenHeap)
{
AStarNode node = OpenHeap.Remove();
Closed.Add(node);
if (node == null)
{
return(-1);
}
if (node.X == endX && node.Y == endY)
{
return(1);
}
int x, y;
for (y = -1; y <= 1; y++)
{
for (x = -1; x <= 1; x++)
{
if (x == 0 && y == 0)
{
continue;
}
int newX = node.X + x;
int newY = node.Y + y;
if (newX < 0 || newX >= fieldWidth || newY < 0 || newY >= fieldHeight)
{
continue;
}
if (field[newX, newY] > 0 && newX == endX && newY == endY)
{
return(2);
}
if (field[newX, newY] > 0 || GetClosedNode(newX, newY, Closed) != null)
{
continue;
}
int index = 0;
AStarNode o_node = GetOpenHeapNode(newX, newY, OpenHeap, out index);
if (o_node != null)
{
// Node is already in the open list
int G = node.G + SQRT[x * x + y * y];
if (G < o_node.G)
{
o_node.parent = node;
o_node.G = G;
OpenHeap.ChangeItem(index, o_node.F, o_node);
}
}
else
{
// Node is NOT in the open list
AStarNode n = new AStarNode();
n.X = node.X + x;
n.Y = node.Y + y;
n.parent = node;
n.G = node.G + SQRT[x * x + y * y];
int offX = Math.Abs(n.X - endX);
int offY = Math.Abs(n.Y - endY);
if (UseHeuristic == false)
{
n.H = 0;
}
else
{
if (offX > offY)
{
n.H = 14 * offY + 10 * (offX - offY);
}
else
{
n.H = 14 * offX + 10 * (offY - offX);
}
n.H = (Math.Abs(n.X - endX) + Math.Abs(n.Y - endY)) * 10;
}
OpenHeap.Add(n.F, n);
}
}
}
return(0);
}