/// <summary>
/// Find a path from the endpoint to the startpoint.
/// This is done by looking at all points and selecting the points that are in the given target area.
/// Their orientation may only vary by the given AcceptableOrientationDeviation.
/// </summary>
/// <returns>The path to target.</returns>
public List <RRTPath> FindPathToTarget()
{
//Move area around the endpoint
Rectangle TranslatedTargetArea = new Rectangle(EndPoint.X - TargetArea.Width / 2, EndPoint.Y + TargetArea.Height / 2, TargetArea.Width, TargetArea.Height);
Graphics g = Graphics.FromImage(InternalMap.ImageMap);
g.DrawRectangle(Pens.Fuchsia, new Rectangle(InternalMap.ToMapCoordinates(TranslatedTargetArea.Location), TranslatedTargetArea.Size));
List <RRTNode> NodesInTargetArea = new List <RRTNode>();
List <RRTPath> Paths = new List <RRTPath>();
//Step through all nodes
foreach (var item in AllNodes)
{
//Check if the rectangle contains the point and check the orientation
if (Math.Abs(item.Orientation - EndRRTNode.Orientation) < AcceptableOrientationDeviation)
{
if ((Math.Abs(EndPoint.X - item.Position.X) < TargetArea.Width / 2) && (Math.Abs(EndPoint.Y - item.Position.Y) < TargetArea.Height / 2))
{
//Add to the list of found nodes.
NodesInTargetArea.Add(item);
Console.WriteLine("Found node in target area: " + item);
}
}
}
//In case no point was found return
if (NodesInTargetArea.Count == 0)
{
return(Paths);
}
//Some helpers for creating a nice color for the paths ;)
int B = 0;
int R = 255;
int Step = 255 / NodesInTargetArea.Count;
//Step through all found nodes
foreach (var item in NodesInTargetArea)
{
//Calculate length
double length = 0;
//Follow the Predecessor until their is none (this is the startpoint then)
RRTNode previous = item.Predecessor;
RRTNode end = null;
while (previous != null)
{
if (previous.Predecessor != null)
{
//TODO replace with RRTHelpers.CalculateDistance
length += RRTHelpers.CalculateDistance(previous, previous.Predecessor);
//length += Math.Sqrt (Math.Pow (previous.Position.X - previous.Predecessor.Position.X, 2) + Math.Pow (previous.Position.Y - previous.Predecessor.Position.Y, 2));
}
else
{
end = previous;
}
previous = previous.Predecessor;
}
//Create new path from start and end item
RRTPath path = new RRTPath(item, end);
Paths.Add(path);
path.Color = Color.FromArgb(R, 50, B);
path.DistanceToEnd = RRTHelpers.CalculateDistance(path.Start, EndRRTNode);
path.OrientationDeviation = path.Start.Orientation - EndRRTNode.Orientation;
B += Step;
R -= Step;
}
//Sort the list by the cost function of the given paths
Paths.Sort((RRTPath x, RRTPath y) => {
if (x.Cost() > y.Cost())
{
return(1);
}
else
{
return(-1);
}
});
//List<RRTPath> SortedList = Paths.AsParallel().OrderBy(o => o.Cost()).ToList();
foreach (var item in Paths)
{
Console.WriteLine("Length for path " + item.Color.ToString() + " : " + item.Length + " Distance to End: " + item.DistanceToEnd + " OrientationDif: " + item.OrientationDeviation);
}
return(Paths);
}
