public List<Node> ExecuteSearch()
{
PriorityQueue openQ = new PriorityQueue();
Node startNode = new Node();
startNode.Location = startLoc;
startNode.CostFromStart = 0;
startNode.CostToGoal = FindHeuristic(startLoc, goalLoc);
startNode.Parent = null;
openQ.Enqueue(startNode);
open.Add(startNode.Location, startNode);
while (openQ.Count > 0)
{
Node node = openQ.Dequeue();
open.Remove(node.Location);
if (node.Location.Equals(goalLoc))
return GetSolution(node);
List<Node> neighbors = FindNeighbors(node);
foreach (Node newNode in neighbors)
{
Location nnLoc = newNode.Location;
if (closed.ContainsKey(nnLoc)) continue;
decimal newCostFromStart = node.CostFromStart + FindActualCost(node.Location, newNode.Location);
decimal newCostToGoal = FindHeuristic(newNode.Location, goalLoc);
decimal newTotalCost = newCostFromStart + newCostToGoal;
Node holderO = null;
open.TryGetValue(newNode.Location, out holderO);
if (holderO != null && holderO.TotalCost <= newTotalCost)
continue;
newNode.CostFromStart = newCostFromStart;
newNode.CostToGoal = newCostToGoal;
newNode.Parent = node;
if (holderO != null) //remove old location if it exists
{
openQ.Remove(holderO);
open.Remove(newNode.Location);
}
openQ.Enqueue(newNode);
open.Add(newNode.Location, newNode);
}
closed.Add(node.Location, node);
}
return null;
}
/// <summary>
/// This method adds the node to the list based on totalcost of the node
/// </summary>
/// <param name="node"></param>
public void Enqueue(Node node)
{
bool found = false;
for (int i = 0; i < nodes.Count; i++)
{
Node holder = nodes[i];
if (holder.TotalCost >= node.TotalCost)
{
nodes.Insert(i, node);
found = true;
break;
}
}
if (!found)
{
nodes.Add(node);
}
}
/// <summary>
///
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
private List<Node> FindNeighbors(Node node)
{
Location nodeLoc = node.Location;
var neighbors = new List<Node>();
AddConditional(neighbors, nodeLoc, -1, 0);
AddConditional(neighbors, nodeLoc, 0, -1);
AddConditional(neighbors, nodeLoc, 0, 1);
AddConditional(neighbors, nodeLoc, 1, 0);
return neighbors;
}
/// <summary>
///
/// </summary>
/// <param name="addTo"></param>
/// <param name="loc"></param>
/// <param name="x"></param>
/// <param name="y"></param>
private void AddConditional(IList<Node> addTo, Location loc, int x, int y)
{
int newX = loc.X + x, newY = loc.Y + y;
if (newX < 0 || newX >= grid.GetLength(0))
{
return;
}
if (newY < 0 || newY >= grid.GetLength(1))
{
return;
}
if (grid[newX, newY] == Constants.SOLID)
return;
var location = new Location(newX, newY);
var newNode = new Node() {Location = location};
addTo.Add(newNode);
}
/// <summary>
///
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
private static List<Node> GetSolution(Node node)
{
var pathList = new List<Node> {node};
while (node.Parent != null)
{
pathList.Insert(0, node.Parent);
node = node.Parent;
}
return pathList;
}
/// <summary>
/// This function removes the element from the Priority queue
/// </summary>
/// <param name="node"></param>
public void Remove(Node node)
{
nodes.Remove(node);
}
/// <summary>
/// This method checks whether the Priority queue contains the node or not
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public bool Contains(Node node)
{
return nodes.Contains(node);
}