//List<PathFindingNode> UpdateBottomLevelPath(PathFindingNode start, PathFindingNode end)
//{
// List<PathFindingNode> bottomLevelPath = AStar(start, end);
// pathVisaulDebugger.DrawLines(bottomLevelPath);
// for (int i = 0; i < bottomLevelPath.Count; i++)
// {
// pathSoFar.Enqueue(bottomLevelPath[i]);
// }
// return bottomLevelPath;
//}
//List<PathFindingNode> updateMidLevelPath(PathFindingNode start, PathFindingNode firstMidLevel, PathFindingNode end) {
// List<PathFindingNode> result = new List<PathFindingNode>();
// midLevelPath = AStar(firstMidLevel, end);
// List<PathFindingNode> bottomLevelPath = new List<PathFindingNode>();
// PathFindingNode bottomStart = start;
// for (int i = 1; i < midLevelPath.Count; i++)
// {
// if (bottomLevelPath.Count > 0)
// {
// bottomStart = bottomLevelPath[bottomLevelPath.Count - 1];
// }
// bottomLevelPath = UpdateBottomLevelPath(bottomStart, midLevelPath[i]);
// }
// return result;
//}
// do A star
// kick out if the parent changes or if you reach end
private AbstractNode AStar(AbstractNode start, AbstractNode end)
{
List <AbstractNode> path = new List <AbstractNode>();
// do A* from start to end but kick out the path if you enter a new region or sub region
start.CostSoFar = 0;
PriorityQueue priorityQueue = new PriorityQueue(start);
Dictionary <string, AbstractNode> lookUpTable = new Dictionary <string, AbstractNode>
{
{ start.GetName, start }
};
priorityQueue.Enqueue(start);
while (priorityQueue.Count() > 0)
{
AbstractNode parent = (AbstractNode)priorityQueue.Peek();
priorityQueue.Dequeue();
foreach (AbstractNode neighbor in parent.GetNeighbors())
{
float costFromParent = (float)Math.Sqrt(Math.Pow(neighbor.GetX - parent.GetX, 2) + Math.Pow(neighbor.GetY - parent.GetY, 2));
float costSoFar = parent.CostSoFar + costFromParent;
if (neighbor.GetName == end.GetName)
{
// TODO add hierarchy check to see if we have changed hierarchies. If we have then we should also kick out and return
// we made it to the end
neighbor.Parent = parent;
neighbor.CostSoFar = costSoFar;
path.Add(neighbor);
return(neighbor);
}
if (lookUpTable.ContainsKey(neighbor.GetName) && lookUpTable[neighbor.GetName].CostSoFar < costSoFar)
{
continue;
}
else if (!lookUpTable.ContainsKey(neighbor.GetName))
{
neighbor.Parent = parent;
neighbor.CostSoFar = costSoFar;
lookUpTable.Add(neighbor.GetName, neighbor);
priorityQueue.Enqueue(neighbor);
}
else
{
lookUpTable[neighbor.GetName].CostSoFar = costSoFar;
if (!priorityQueue.IsConsistent())
{
}
}
}
}
return(null);
}