////////////////////////////////////////////////////////////////////////// private void _ImplementPathfinding(int checks) { // do while has variants while (checks-- > 0) { // not found if (OpenSet.Count == 0) { Complete(FindPathResult.NotReachable); return; } // limit achieved if (ClosedSet.Count >= Options.CheckLimit) { Complete(FindPathResult.Overwhelm); return; } // get next node var currentNode = OpenSet.First(); // close current, move from open to close set OpenSet.Remove(currentNode); ClosedSet.Add(currentNode.Master); ClosedNodes.AddLast(currentNode); // goal check if (Goal.Any(n => n.Equals(currentNode.Master))) { getPath(currentNode, Path); Complete(FindPathResult.Found); return; } // proceed connections foreach (var neighborNode in Explorer.GetNeighbours(currentNode.Master)) { // skip if not passable if (neighborNode == null || Explorer.Passable(neighborNode) == false) { continue; } // IsClosed, skip if already checked if (ClosedSet.Contains(neighborNode)) { continue; } var pathCost = currentNode.PathCost + Explorer.GetPathCost(currentNode.Master, neighborNode); var openNode = OpenSet.FirstOrDefault(pathNode => pathNode.Master.Equals(neighborNode)); if (openNode != null) { // if presented and part is shorter, then reset his parent and cost if (openNode.PathCost > pathCost) { openNode.СameFrom = currentNode; openNode.PathCost = pathCost; // update priority openNode.Cost = openNode.PathCostEstimated + openNode.PathCost; OpenSet.UpdatePriority(openNode, openNode.Cost); } } else { // if not presented, add as variant var pathNode = new PathNode <T>(neighborNode); pathNode.СameFrom = currentNode; pathNode.PathCost = pathCost; if (Options.Heuristic) { pathNode.PathCostEstimated = getShortestPath(Explorer, pathNode.Master, Goal); pathNode.Cost = pathNode.PathCostEstimated + pathNode.PathCost; } else { pathNode.Cost = pathNode.PathCost; } OpenSet.Enqueue(pathNode, pathNode.Cost); } } } ///////////////////////////////////// float getShortestPath(iExplorer <T> explorer, T start, IEnumerable <T> goal) { var shortestPath = float.MaxValue; // ReSharper disable once ForeachCanBeConvertedToQueryUsingAnotherGetEnumerator foreach (var n in goal) { var currentShortestPath = explorer.GetShortestPath(start, n); if (shortestPath > currentShortestPath) { shortestPath = currentShortestPath; } } return(shortestPath); } void getPath(PathNode <T> pathNode, LinkedList <T> path) { for (var currentNode = pathNode; currentNode != null; currentNode = currentNode.СameFrom) { path.AddFirst(currentNode.Master); } } }
public void FindPath(Object obj) { try { KeyValuePair <Point, Spot> FirstPair = Spots.First(); FirstPair.Value.GScore = 0; FirstPair.Value.HScore = PointFunctions.DistanceTo(FirstPair.Value.Point, End); OpenSet.Add(FirstPair); while (OpenSet.Count > 0) { Spot Current = OpenSet.ToList().OrderBy(p => p.Value.FScore).First().Value; Console.WriteLine("Current point is: " + Current.Point.ToString()); Invalidate(); if (Current.Point == End) { Current.Color = Color.Blue; bool Searching = true; while (Searching) { if (Current.CameFrom.Value != null) { Current.CameFrom.Value.Color = Color.Blue; Current.CameFrom.Value.OnBestPath = true; Current = Current.CameFrom.Value; } else { Searching = false; } } Spots.ToList().ForEach(s => { if (!s.Value.OnBestPath && !s.Value.IsObstacle) { s.Value.Color = Color.White; Invalidate(); } }); Invalidate(); Console.WriteLine("Done!"); break; } ClosedSet.Add(Current.Point, Current); OpenSet.Remove(Current.Point); int tempG = 0; foreach (KeyValuePair <Point, Spot> neighbour in Current.Neibhours) { if (ClosedSet.Contains(neighbour) || neighbour.Value.IsObstacle) { continue; } tempG = Current.GScore + PointFunctions.DistanceTo(Current.Point, neighbour.Key); if (!OpenSet.Contains(neighbour)) { OpenSet.Add(neighbour); } else if (tempG >= neighbour.Value.GScore) { continue; } neighbour.Value.GScore = tempG; neighbour.Value.HScore = PointFunctions.DistanceTo(neighbour.Key, End); neighbour.Value.CameFrom = new KeyValuePair <Point, Spot>(Current.Point, Current); } Invalidate(); } } catch (Exception ex) { Console.WriteLine(ex.StackTrace); System.Diagnostics.Debugger.Break(); throw; } }