public void ProcessNeighbors(List <GridSquare> n, GridSquare p)
{
Debug.WriteLine("processing neighbor set...");
foreach (var g in n)
{
if (IsInClosed(g))
{
Debug.WriteLine("Node closed moving on...");
continue;
}
else if (g.IsBlocked)
{
continue;
}
else if (IsInOpen(g))
{
Debug.WriteLine("Comparing parents and re-prioritizing...");
if (g.CompareParentTo(p))
{
g.Parent = Nodes[p.CoordX, p.CoordY];
g.LowestHCost(Blob);
g.ComputeGCost();
g.ComputeFCost();
Open.UpdatePriority(g, g.FCost);
}
}
else if (g.Parent == null)
{
g.Parent = Nodes[p.CoordX, p.CoordY];
g.LowestHCost(Blob);
g.ComputeGCost();
g.ComputeFCost();
Debug.WriteLine("New Node enqueued: " + g.CoordX + ", " + g.CoordY);
Open.Enqueue(g, g.FCost);
}
else
{
throw new Exception("WTF you moron");
}
}
}
public IList <Node> Find(Node start, Node goal)
{
Start = start;
Goal = goal;
Parent.Add(start, start);
Open.Enqueue(Start, Heuristic(Start));
while (Open.Count > 0)
{
Node node = Open.Dequeue();
if (node.Id == Goal.Id)
{
return(BuildPath());
}
AddUpdate(Closed, node.Id, node);
foreach (ProposedStep step in Grid.Neighbours(node))
{
Node neighour = step.Node;
if (!Closed.ContainsKey(neighour.Id))
{
if (!Open.Contains(neighour))
{
AddUpdate(GHistory, neighour.Id, double.PositiveInfinity);
TryRemove(Parent, neighour);
}
double gOld = TryGetValue(GHistory, neighour.Id);
// Compute Cost
if (UseLineOfSight && LineOfSight(TryGetValue(Parent, node), neighour))
{
var sParent = TryGetValue(Parent, node);
var sParentCost = Cost(sParent, neighour);
if (TryGetValue(GHistory, sParent.Id) + sParentCost < TryGetValue(GHistory, neighour.Id))
{
AddUpdate(Parent, neighour, sParent);
AddUpdate(GHistory, neighour.Id, TryGetValue(GHistory, node.Id) + sParentCost);
}
}
else
{
if (TryGetValue(GHistory, node.Id) + step.Direction.Cost < TryGetValue(GHistory, neighour.Id))
{
AddUpdate(Parent, neighour, node);
AddUpdate(GHistory, neighour.Id, TryGetValue(GHistory, node.Id) + step.Direction.Cost);
}
}
// Compute Cost End
if (TryGetValue(GHistory, neighour.Id) < gOld)
{
if (Open.Contains(neighour))
{
Open.Remove(neighour);
}
Open.Enqueue(neighour, TryGetValue(GHistory, neighour.Id) + Heuristic(neighour));
}
}
}
}
return(new List <Node>());
}
public IEnumerable <Node> GetPath(Node start, Node goal)
{
this.Parent[start] = null;
this.Cost[start] = 0;
this.Open.Enqueue(start);
while (Open.Count > 0)
{
Node currentNode = Open.Dequeue();
if (currentNode.Equals(goal))
{
break;
}
List <Node> neighbours = new List <Node>
{
new Node(currentNode.Row + 1, currentNode.Col),
new Node(currentNode.Row - 1, currentNode.Col),
new Node(currentNode.Row, currentNode.Col + 1),
new Node(currentNode.Row, currentNode.Col - 1)
};
foreach (Node neighbour in neighbours)
{
if (neighbour.Row >= Map.GetLength(0) || (neighbour.Row < 0))
{
continue;
}
if (neighbour.Col >= Map.GetLength(1) || (neighbour.Col < 0))
{
continue;
}
if (Map[neighbour.Row, neighbour.Col] == 'W' || Map[neighbour.Row, neighbour.Col] == 'P')
{
continue;
}
int newCost = Cost[currentNode] + 1;
if (!Cost.ContainsKey(neighbour) || Cost[neighbour] > newCost)
{
Cost[neighbour] = newCost;
neighbour.F = newCost + GetH(neighbour, goal);
Open.Enqueue(neighbour);
Parent[neighbour] = currentNode;
}
}
}
List <Node> result = new List <Node>();
Node lastNode = goal;
if (!Parent.ContainsKey(lastNode))
{
result.Add(start);
}
else
{
result.Add(lastNode);
while (Parent[lastNode] != null)
{
result.Add(Parent[lastNode]);
lastNode = Parent[lastNode];
}
}
result.Reverse();
return(result);
//Node lastNode = goal;
//if (!Parent.ContainsKey(lastNode))
//{
// yield return start;
//}
//else
//{
// yield return lastNode;
// while (Parent[lastNode] != null)
// {
// yield return Parent[lastNode];
// lastNode = Parent[lastNode];
// }
//}
}
