public List<PathNode> Calculate(IGridShape start, IGridShape end, IGridManager gridManager)
{
var startNode = new PathNode {Position = start};
startNode = CalculateFGH(startNode, startNode, end);
_openList.Add(startNode);
while (true)
{
_openList.Sort();
if (_openList.Count == 0)
//_openList.Add(_closedList[0]);
return _closedList;
var current = _openList[0];
if (current.Position.Equals(end))
return _closedList;
_closedList.Add(current);
_openList.Remove(current);
var neighbours = gridManager.GetNeighbours(current.Position);
foreach (var g in neighbours)
{
if (g == null) continue;
var c1 = _closedList.Where(x => x.Position.Equals(g)).FirstOrDefault();
if (!g.Blocked && (c1 == null))
{
var newNode = new PathNode { Position = g, Parent = current };
newNode = CalculateFGH(current, newNode, end);
if (!_openList.Contains(newNode))
{
newNode.Parent = current;
newNode = CalculateFGH(current, newNode, end);
_openList.Add(newNode);
}
else
{
var existingNode = _openList.First(x => x.Position.Id == newNode.Position.Id);
if (newNode.G < existingNode.G)
{
_openList.Remove(existingNode);
existingNode.Parent = current;
existingNode = CalculateFGH(current, existingNode, end);
_openList.Add(existingNode);
_openList.Sort();
}
}
}
//if (!g.Blocked && (c1 != null))
//{
// var newNode = new PathNode { Position = g, Parent = current };
// newNode = CalculateFGH(current, newNode, end);
// if (newNode.G < c1.G)
// {
// _openList.Remove(c1);
// _openList.Add(newNode);
// }
//}
}
}
}