//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); }