private int reconstruct_move_depth(Dictionary <string, StorageState> cameFrom, StorageState current) { Console.WriteLine(); int result = 0; StorageState tempCurrent = current; // PrintState(tempCurrent); while (cameFrom.ContainsKey(tempCurrent.HashState())) { Console.WriteLine(); tempCurrent = cameFrom[tempCurrent.HashState()]; // PrintState(tempCurrent); result++; } return(result); }
public int ShortestPath() { int consoleY = Console.CursorTop; StorageState startState = CalcStartState(); // The set of moves already evaluated. HashSet <string> closedSet = new HashSet <string>(); // The set of currently discovered nodes still to be evaluated. // Initially, only the start node is known. List <StorageState> openSet = new List <StorageState>(); openSet.Add(startState); // For each node, which node it can most efficiently be reached from. // If a node can be reached from many nodes, cameFrom will eventually contain the // most efficient previous step. Dictionary <string, StorageState> cameFrom = new Dictionary <string, StorageState>(); // For each node, the cost of getting from the start node to that node. Dictionary <string, int> gScore = new Dictionary <string, int>(); // The cost of going from start to start is zero. gScore[startState.HashState()] = 0; // For each node, the total cost of getting from the start node to the goal // by passing by that node. That value is partly known, partly heuristic. Dictionary <string, int> fScore = new Dictionary <string, int>(); // For the first node, that value is completely heuristic. fScore[startState.HashState()] = SolveCostEstimate(startState); _lastDistanceFromOpen = 0; _lastDistanceFromRoot = 0; while (openSet.Count > 0) { Console.SetCursorPosition(0, consoleY); Console.WriteLine("Queue depth: " + openSet.Count + " Hashes processed: " + closedSet.Count + " Last Open Distance: " + _lastDistanceFromOpen.ToString() + " Last Distance from Solved: " + _lastDistanceFromRoot.ToString()); var v = from f in openSet join j in fScore on f.HashState() equals j.Key select new { score = j.Value, storageState = f }; var scores = v.ToList(); int minValue = scores.Min(o => o.score); // the node in openSet having the lowest fScore[] value StorageState current = scores.First(o => o.score == minValue).storageState; //Console.WriteLine("Chose this state as the best one: " + current.ID); //PrintState(current); //Console.ReadLine(); if (current.DesiredDataOnX == 0 && current.DesiredDataOnY == 0) { return(reconstruct_move_depth(cameFrom, current)); } openSet.Remove(current); closedSet.Add(current.HashState()); ConcurrentQueue <StorageState> neighbours = new ConcurrentQueue <StorageState>(); //int moveCount = 0; CalcAllPossibleValidMoves(current, neighbours); foreach (StorageState bm in neighbours) { //Console.WriteLine("Possible move #" + (++moveCount).ToString() + " id: " + bm.ID); //PrintState(bm); // Ignore the neighbor which is already evaluated. if (closedSet.Contains(bm.HashState())) { continue; } string hash = current.HashState(); // The distance from start to a neighbor int tentative_gScore = gScore[hash] + 1; // Discover a new node if (!openSet.Contains(bm)) { openSet.Add(bm); } else if (tentative_gScore >= gScore[bm.HashState()]) { continue; // This is not a better path. } // This path is the best until now. Record it! cameFrom[bm.HashState()] = current; gScore[bm.HashState()] = tentative_gScore; fScore[bm.HashState()] = gScore[bm.HashState()] + SolveCostEstimate(bm); } } return(-1); }