public List <GridSquare> FindPath(Controllers.HomeController.GData d)
{
ProcessData(d);
GridSquare next;
List <GridSquare> path = new List <GridSquare>();
List <GridSquare> neighbors;
Debug.WriteLine("Target is: " + TargetNode.CoordX + ", " + TargetNode.CoordY);
GridSquare current = Nodes[TargetNode.CoordX, TargetNode.CoordY];
while (!IsInBlob(current))
{
neighbors = current.VisitNeighbors(Width, Height, this);
ProcessNeighbors(neighbors, current);
next = Open.Dequeue();
Closed.Add(Nodes[current.CoordX, current.CoordY]);
current = Nodes[next.CoordX, next.CoordY];
}
path = BacktrackFrom(current);
return(path);
}
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];
// }
//}
}