public void AstarRun()
{
Console.WriteLine("A* starts!");
NavigateNode start = new NavigateNode(2, 2, NavigateNode.StateEnum.NAVIGABLE);
NavigateNode end = new NavigateNode(goalX, goalY, NavigateNode.StateEnum.NAVIGABLE);
PriorityQueue <NavigateNode> openSet = new PriorityQueue <NavigateNode>();
PriorityQueue <NavigateNode> closeSet = new PriorityQueue <NavigateNode>();
openSet.Add(start);
while (!openSet.Empty)
{
// get from open set
NavigateNode current = openSet.Pop();
// add to close set
closeSet.Add(current);
// goal found
if (current.IsSameLocation(end))
{
while (current.Parent != null)
{
mMap[current.X, current.Y].State = NavigateNode.StateEnum.PATH;
current = current.Parent;
}
return;
}
else
{
List <NavigateNode> neighbors = GetNeighbors(current);
foreach (NavigateNode n in neighbors)
{
if (closeSet.IsMember(n))
{
continue;
}
else
{
if (!openSet.IsMember(n))
{
n.Parent = current;
n.DirectCost = current.DirectCost + GetDirectCost(current, n);
n.HeuristicCost = GetHeuristicCost(n, end);
n.TotalCost = n.DirectCost + n.HeuristicCost;
// add to open set
openSet.Add(n);
}
else
{
double costFromThisPathToM = current.DirectCost + GetDirectCost(current, n);
// we found a better path
if (costFromThisPathToM < n.DirectCost)
{
n.Parent = current; // change parent to n
n.DirectCost = costFromThisPathToM; // recalculate direct cost
n.TotalCost = n.HeuristicCost + n.DirectCost; // recalculate total cost
}
}
}
}
}
}
Console.WriteLine("end here?");
}
