public IList<Node> CreatePath(Node startPoint, Node endPoint)
{
var temp = new List<Node>();
// Calculate startPoints Heuristic and Final Cost
startPoint.HeuristicCost = GetHeuristic(startPoint, endPoint);
temp.Add(startPoint);
var current = startPoint;
while (temp.Where(x => x.State == NodeState.Open).Any() && (current.X != endPoint.X || current.Y != endPoint.Y))
{
foreach (var item in GetNeighbors(current))
{
if (!temp.Where(j => j.X == item.X && j.Y == item.Y).Any())
{
item.HeuristicCost = GetHeuristic(item, endPoint);
temp.Add(item);
}
else if (temp.Where(j => j.X == item.X && j.Y == item.Y && j.GivenCost > item.GivenCost).Any())
{
temp.Remove(temp.Where(j => j.X == item.X && j.Y == item.Y && j.GivenCost > item.GivenCost).First());
item.HeuristicCost = GetHeuristic(item, endPoint);
temp.Add(item);
}
}
temp.Where(x => x.State == NodeState.Open && x.X == current.X && x.Y == current.Y).First().State = NodeState.Closed;
current = temp.Where(x => x.State == NodeState.Open).OrderBy(x => x.HeuristicCost).FirstOrDefault();
if (Obstructions != null && Obstructions.Where(x => x.X == endPoint.X && x.Y == endPoint.Y).Any() && WithinRange(endPoint, current, Range))
{
endPoint = current;
break;
}
}
var result = new List<Node>();
result = DeterminePath(temp, endPoint);
result.Reverse();
return result.ToList();
}
private bool WithinRange(Node target, Node current, int range)
{
var result = false;
// North/South
if (current.X == target.X && Math.Abs(current.Y - target.Y) <= range)
result = true;
// East/West
if (Math.Abs(target.X - current.X) <= range && current.Y == target.Y)
result = true;
return result;
}
private int GetHeuristic(Node start, Node end)
{
int result = 0;
result = (int)Math.Sqrt(((start.X - end.X) * (start.X - end.X)) + ((start.Y - end.Y) * (start.Y - end.Y)));
return result * 10;
}
private List<Node> DeterminePath(List<Node> nodes, Node target, List<Node> path = null)
{
if (path == null)
path = new List<Node>();
if (nodes.Where(j => j.X == target.X && j.Y == target.Y).FirstOrDefault() == null)
{
var newTarget = nodes.OrderBy(x => x.HeuristicCost).Take(4).OrderBy(x => x.FinalCost).First();
return DeterminePath(nodes, newTarget, path);
}
var item = nodes.Where(j => j.X == target.X && j.Y == target.Y).First();
path.Add(item);
if (item.Parent == null)
return path;
return DeterminePath(nodes, item.Parent, path);
}
protected IList<Node> GetNeighbors(Node parent)
{
var result = new List<Node>();
// North
if ((Obstructions == null || !Obstructions.Where(x => x.X == parent.X && x.Y == parent.Y - 1).Any()) && WithinLimits(parent.X, parent.Y - 1))
{
var north = new Node(parent.X, parent.Y - 1, parent);
result.Add(north);
}
// South
if ((Obstructions == null || !Obstructions.Where(x => x.X == parent.X && x.Y == parent.Y + 1).Any()) && WithinLimits(parent.X, parent.Y + 1))
{
var south = new Node(parent.X, parent.Y + 1, parent);
result.Add(south);
}
// East
if ((Obstructions == null || !Obstructions.Where(x => x.X == parent.X + 1 && x.Y == parent.Y).Any()) && WithinLimits(parent.X + 1, parent.Y))
{
var east = new Node(parent.X + 1, parent.Y, parent);
result.Add(east);
}
// West
if ((Obstructions == null || !Obstructions.Where(x => x.X == parent.X - 1 && x.Y == parent.Y).Any()) && WithinLimits(parent.X - 1, parent.Y))
{
var west = new Node(parent.X - 1, parent.Y, parent);
result.Add(west);
}
return result;
}
public Node(int x, int y, Node parent)
: this(x, y)
{
this.Parent = parent;
this.GivenCost = this.Parent.GivenCost + 10;
}
