private void FindPath(ref List <List <Node> > nodes, Node start, Node end)
{
List <Node> open = new List <Node>();
List <Node> close = new List <Node>();
open.Add(start);
while (open.Count > 0)
{
close.Add(open[0]);
Node pendingNode = open[0];
open.RemoveAt(0);
for (int i = 0; i < pendingNode.Count; i++)
{
Node current = pendingNode[i];
if (current == null || current.Equals(start) || current.isWall)
{
continue;
}
int h;
int g;
int f;
if (i % 2 == 0)
{
//Up Right Down Left
g = pendingNode.G + 10;
}
else
{
// URight DRight DLeft ULeft
//check for walls
//|X
//|_
if (pendingNode[(i + 1) % 8].isWall || pendingNode[i - 1].isWall)
{
continue;
}
g = pendingNode.G + 14;
}
h = (Math.Abs(end.Pos.X - current.Pos.X) + Math.Abs(end.Pos.Y - current.Pos.Y)) * 10;
f = h + g;
if (f < current.F || current.F == 0)
{
current.G = g;
current.H = h;
current.F = f;
current.Parent = pendingNode;
open.Add(current);
}
if (current.Equals(end))
{
List <Node> paths = new List <Node>();
Node path = end;
while (path.Parent != null)
{
paths.Add(path);
path = path.Parent;
}
open.Clear();
VisualizePath(paths);
break;
}
}
open = open.OrderBy(item => item.F).ToList();
}
}
public static List <Direction8Way> Search8WayNode(Node start, Node end, IMovable grid, out int distance)
{
Console.Error.WriteLine("Finding from {0} to {1} by A*", start, end);
distance = -1;
List <Direction8Way> pathWay = new List <Direction8Way>();
if (start.Equals(end))
{
distance = 0;
return(pathWay);
}
List <SearchNode> frontier = new List <SearchNode>();
List <SearchNode> explored = new List <SearchNode>();
SearchNode startNode = new SearchNode(start, null, 0, PathFinding.GetManhattanHeuristic(start, end));
frontier.Add(startNode);
bool found = false;
while (frontier.Count > 0)
{
SearchNode current = frontier[0];
frontier.RemoveAt(0);
explored.Add(current);
if (current.Pos.Equals(end))
{
distance = current.CostSoFar + current.CostToEnd;
SearchNode parent = current;
while (parent != null && parent.Pos.Equals(start) == false)
{
Direction8Way dir = PathFinding.Get8WayDirection(parent.Parent.Pos, parent.Pos);
pathWay.Add(dir);
parent = parent.Parent;
}
found = true;
break;
}
List <SearchNode> neighbors = PathFinding.Get8WayNeighbors(current, grid);
foreach (SearchNode node in neighbors)
{
if (explored.Contains(node))
{
continue;
}
node.CostSoFar = current.CostSoFar + 1;
node.CostToEnd = PathFinding.GetManhattanHeuristic(node.Pos, end);
int index = frontier.IndexOf(node);
if (index > 0)
{
if (node.CostSoFar < frontier[index].CostSoFar)
{
// if found better way
frontier[index].Parent = current;
frontier[index].CostSoFar = node.CostSoFar;
frontier[index].CostToEnd = node.CostToEnd;
}
}
else
{
frontier.Add(node);
}
}
frontier.Sort((item1, item2) => (item1.CostSoFar + item1.CostToEnd) - (item2.CostSoFar + item2.CostToEnd));
//Console.Error.WriteLine( "frontier = {0}", frontier.ToDebugString() );
}
pathWay.Reverse();
if (found)
{
Console.Error.WriteLine("Found : {0} to {1} : {2} of distance {3}", start, end, pathWay.ToDebugString(), distance);
}
else
{
Console.Error.WriteLine("No Way! : {0} to {1}", start, end);
}
return(pathWay);
}