public static List <Node> AStarBuTheBook(Node start, Node goal)
{
;
if (start.IsObstacle || goal.IsObstacle)
{
Console.WriteLine("Can't run A*! Enter proper from and to, one of them is an obsttacle!");
return(null);
}
// The set of nodes already evaluated
var closedSet = new List <Node>();
// The set of currently discovered nodes that are not evaluated yet.
// Initially, only the start node is known.
var openSet = new List <Node> {
start
};
// 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.
var cameFrom = new List <Node>();
while (openSet.Count != 0)
{
openSet = openSet.OrderBy(arg => arg.DistanceToGoal).ToList();
var current = openSet[0];
if (current.Position == goal.Position)
{
return(reconstructPath(cameFrom, current));
}
var neighbours = NodesArray.Neighbours(current);
openSet.Remove(current);
closedSet.Add(current);
foreach (var neighbour in neighbours)
{
if (closedSet.Exists(arg => arg.Position == neighbour.Position))
{
continue; // Ignore the neighbor which is already evaluated.
}
if (!openSet.Exists(arg => arg.Position == neighbour.Position))
{
openSet.Add(neighbour); // Discover a new node
}
var tentativeDistance = current.DistanceToStart + Node.MetricsAStar(current, neighbour);
if (tentativeDistance > neighbour.DistanceToStart)
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
if (!cameFrom.Exists(arg => arg.Position == current.Position))
{
cameFrom.Add(current);
}
neighbour.DistanceToStart = tentativeDistance;
neighbour.DistanceToGoal = neighbour.DistanceToStart + Node.MetricsAStar(neighbour, goal);
}
}
return(null);
}
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);
}