public static List <Node> AStar(Node from, Node to) { var finalPath = new List <Node>(); if (from.IsObstacle || to.IsObstacle) { Console.WriteLine("Can't run A*! Enter proper from and to, one of them is an obsttacle!"); return(finalPath); } var current = new Node(from) { DistanceToGoal = Node.MetricsAStar(@from, to), Visited = NodeState.Processed }; var observed = new List <Node> { current }; while (current.Position != to.Position) { var query = NodesArray.Neighbours(current); query = query.Where(node => !node.IsObstacle).ToList(); foreach (var node in query) { if (!observed.Exists(arg => arg.Position == node.Position)) { node.DistanceToGoal = Node.MetricsAStar(node, to); node.Visited = NodeState.Discovered; observed.Add(node); } } observed = observed.OrderBy(arg => arg.Visited).ThenBy(arg => arg.DistanceToGoal).ToList(); if (observed[0].Visited != NodeState.Processed) { current = observed[0]; observed[0].Visited = NodeState.Processed; } else { Console.WriteLine("No path was found"); return(finalPath); } } observed = observed.Where(informer => informer.Visited == NodeState.Processed).ToList(); finalPath.Add(observed[0]); for (var i = 0; i < observed.Count; ++i) { var maxIndex = i; for (var j = i + 1; j < observed.Count; ++j) { if (StraightLine.OnOneLine(observed[i], observed[j], NodesArray)) { maxIndex = j; } } if (maxIndex != i) { var points = StraightLine.FindMiddlePoints(observed[i], observed[maxIndex]); points.RemoveAt(0); foreach (var point in points) { var coordinatesX = Convert.ToInt32(point.X); var coordinatesY = Convert.ToInt32(point.Y); finalPath.Add(NodesArray.Array[coordinatesX, coordinatesY]); } i = maxIndex - 1; } } finalPath.Reverse(); return(finalPath); }