public static bool OnOneLine(Node sNode, Node fNode, NodeArray nodesArray)
{
var destination = FindDestination(sNode, fNode);
var finishPoint = new Vector2(Convert.ToInt32(fNode.Position.X), Convert.ToInt32(fNode.Position.Y));
var line = new StraightLine(Convert.ToInt32(sNode.Position.X), Convert.ToInt32(sNode.Position.Y),
destination, nodesArray);
if (line.Points.Exists(arg => arg == finishPoint))
{
for (var i = 1; i < line.Points.Count; ++i)
{
var pointX = Convert.ToInt32(line.Points[i].X);
var pointY = Convert.ToInt32(line.Points[i].Y);
if (nodesArray.Array[pointX, pointY].IsObstacle)
{
return(false);
}
if (line.Points[i] == finishPoint)
{
return(true);
}
if ((destination == Destinations.UpRight || destination == Destinations.DownRight) &&
!nodesArray.Array[pointX, pointY + 1].IsObstacle)
{
return(false);
}
if ((destination == Destinations.UpLeft || destination == Destinations.DownLeft) &&
!nodesArray.Array[pointX, pointY - 1].IsObstacle)
{
return(false);
}
}
}
return(false);
}
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);
}