public void ExploreFrom(Coordinate from) { if (from.X < 0 || from.X > _width - 1 || from.Y < 0 || from.Y > _height - 1) { throw new ArgumentOutOfRangeException("from", "Outside bounds of map"); } Source = from; State = ExplorerState.Working; ClearSolution(); var root = new PathfinderNode(); root.Heuristic = 0; root.ParentX = from.X; root.ParentY = from.Y; root.X = from.X; root.Y = from.Y; root.Cost = 0; root.Heuristic = 0; root.Value = root.Cost + root.Heuristic; _workingQueue.Add(root); }
private void ClearSolution() { for (int x = 0; x < _solution.GetLength(0); x++) { for (int y = 0; y < _solution.GetLength(1); y++) { _solution[x, y] = new PathfinderNode() { Explored = false }; } } _workingQueue.Clear(); }
public void Tick(int ticks) { // Break if we are not working if (State != ExplorerState.Working) { return; } for (int i = 0; i < ticks; i++) { // Completed - if the queue is empty if (_workingQueue.Count == 0) { State = ExplorerState.Completed; return; } // Dequeue var current = _workingQueue.Remove(); // Discard if we already have a better solution var better = HasBetterExistingSolutionOnMap(current.X, current.Y, current.Cost); if (better) { i -= 1; continue; } // Store current solution _solution[current.X, current.Y] = current; // If the heuristic is complete if (HeuristicCalculator.Complete(current.X, current.Y)) { State = ExplorerState.Completed; return; } // Explore Neighbours for (int j = 0; j < _moveVectors.Length; j++) { var moveVector = _moveVectors[j]; // Calculate new coordinates var newX = current.X + moveVector.X; var newY = current.Y + moveVector.Y; // Discard if the cell is not open / available if (!CostCalculator.OpenCheck(current.X, current.Y, newX, newY, moveVector.X, moveVector.Y)) { continue; } // Evaluate Node int cost = current.Cost + CostCalculator.CalculateCost(current.X, current.Y, newX, newY, moveVector.X, moveVector.Y); int heuristic = HeuristicCalculator.Calculate(newX, newY); var value = heuristic + cost; // Discard if we have exceeded our maximum cost if (MaxCost.HasValue && cost > MaxCost.Value) { continue; } // Discard if we already have a better solution on map or in queue better = HasBetterExistingSolutionOnMap(newX, newY, cost); if (better) { continue; } better = HasBetterExistingSolutionInQueue(newX, newY, cost); if (better) { continue; } // Enqueue New Tile var newNode = new PathfinderNode(); newNode.X = newX; newNode.Y = newY; newNode.ParentX = current.X; newNode.ParentY = current.Y; newNode.Cost = cost; newNode.Heuristic = heuristic; newNode.Value = value; newNode.Explored = true; // Store solution in map _workingQueue.Add(newNode); } } }