public IEnumerable <Point> FindPath(IMap map, Point from, Point to) { var points = new AvlTree <AStarPoint>(); var path = new System.Collections.Generic.Dictionary <AStarPoint, AStarPoint>(); var start = new AStarPoint(from, to, null); var finish = new AStarPoint(to, to, null); points.Insert(start); path[start] = null; while (points.Count != 0) { var point = points.GetMinOrThrow(); if (point.Point.Equals(to)) { break; } var nextPoints = map.GetAreaOfPoint(point.Point) .Where(map.IsCouldVisited) .Select(p => new AStarPoint(p, to, point)) .Where(p => !path.ContainsKey(p)); foreach (var aStarPoint in nextPoints) { points.Insert(aStarPoint); path[aStarPoint] = point; } points.Remove(point); } foreach (var point in BuildPath(path, finish)) { yield return(point); } }
private static IEnumerable <Point> BuildPath( IReadOnlyDictionary <AStarPoint, AStarPoint> path, AStarPoint finish) { if (!path.ContainsKey(finish)) { yield break; } var result = new Stack <Point>(); result.Push(finish.Point); while (path[finish] != null) { result.Push((finish = path[finish])?.Point); } while (result.Count != 0) { yield return(result.Pop()); } }