/// <summary>
/// Cleans the path. Removes all not needed entries from the Path.
/// The returned path is a copy of the orignal nodes!
/// </summary>
/// <returns>The path.</returns>
/// <param name="Path">Path.</param>
public static RRTPath CleanPath(RRTPath Path)
{
List <RRTNode> nodes = Path.ToList();
//Clone all nodes
List <RRTNode> clonedNodes = new List <RRTNode>();
foreach (var item in nodes)
{
clonedNodes.Add(item.Clone());
}
//Reconnect the nodes
//First item is end
//Last item is start
RRTNode start = clonedNodes[0];
RRTNode end = clonedNodes[clonedNodes.Count - 1];
for (int i = 0; i < clonedNodes.Count - 1; i++)
{
clonedNodes [i].Predecessor = clonedNodes [i + 1];
clonedNodes[i + 1].AddSucessor(clonedNodes[i]);
}
RRTPath cleanedPath = new RRTPath(start, end);
cleanedPath.Color = Path.Color;
cleanedPath.DistanceToEnd = Path.DistanceToEnd;
cleanedPath.OrientationDeviation = Path.OrientationDeviation;
return(cleanedPath);
}
/// <summary>
/// Draws the given Node + its orientation.
/// </summary>
/// <param name="Base">Base.</param>
/// <param name="_Map">Map.</param>
/// <param name="additional">Additional.</param>
/// <param name="col">Col.</param>
public static void DrawImportantNode(RRTNode Base, Map _Map, int additional, Color col)
{
Point position = _Map.ToMapCoordinates(Base.Position);
for (int x = position.X - additional; x < position.X + additional; x++)
{
for (int y = position.Y - additional; y < position.Y + additional; y++)
{
_Map.DrawPixelOnBitmap(new Point(x, y), col);
}
}
for (int i = 0; i < 15; i++)
{
if (!Base.Inverted)
{
int x = Base.Position.X + (int)(i * Math.Cos(Base.Orientation * ToRadians));
int y = Base.Position.Y + (int)(i * Math.Sin(Base.Orientation * ToRadians));
_Map.DrawPixelOnBitmap(_Map.ToMapCoordinates(new Point(x, y)), Color.DarkRed);
}
else
{
int x = Base.Position.X + (int)(i * Math.Cos(Base.Orientation * ToRadians));
int y = Base.Position.Y + (int)(i * Math.Sin(Base.Orientation * ToRadians));
_Map.DrawPixelOnBitmap(_Map.ToMapCoordinates(new Point(x, y)), Color.DarkOliveGreen);
}
}
}
/// <summary>
/// Gets a RandomNode starting from a BaseNode in a random distance and random drift angle
/// </summary>
/// <returns>The random straight point.</returns>
/// <param name="BaseNode">Base node.</param>
/// <param name="MaximumDrift">Maximum drift.</param>
public static RRTNode GetRandomStraightPoint(RRTNode BaseNode, double MaximumDrift, int InvertProbability, int MaximumDistance = 400)
{
double Distance = Randomizer.NextDouble() * MaximumDistance;
double Angle = (Randomizer.NextDouble() - 0.5) * 2 * MaximumDrift;
double NewAngle;
double Orientation = BaseNode.Orientation;
bool Inverted = BooleanRandom(InvertProbability);
if (Inverted)
{
NewAngle = InvertOrientation(BaseNode.Orientation) + Angle;
}
else
{
NewAngle = BaseNode.Orientation + Angle;
}
int NewX = (int)(BaseNode.Position.X + (Distance * Math.Cos(NewAngle * ToRadians)));
int NewY = (int)(BaseNode.Position.Y + (Distance * Math.Sin(NewAngle * ToRadians)));
Point NewPos = new Point(NewX, NewY);
RRTNode NewNode = new RRTNode(NewPos, Orientation, null);
NewNode.Inverted = Inverted;
return(NewNode);
}
/// <summary>
/// Clone this instance without copying predecessors and sucessors.
/// </summary>
public RRTNode Clone()
{
RRTNode temp = new RRTNode(this.Position, this.Orientation, null);
temp.Inverted = this.Inverted;
return(temp);
}
public RRTNode(Point _Position, double _Orientation, RRTNode _Predecessor)
{
this.Position = _Position;
this.Orientation = _Orientation;
this.Predecessor = _Predecessor;
this.Successors = new List <RRTNode> ();
}
public RRTPath(RRTNode _Start, RRTNode _End)
{
this.Start = _Start;
this.End = _End;
//Calculate length and amount of nodes
CalculateLength();
}
/// <summary>
/// Helper method for iterating through the tree.
/// </summary>
/// <param name="Base">Base.</param>
/// <param name="_Action">Action.</param>
private static void StepThroughTree(RRTNode Base, Action <RRTNode> _Action)
{
foreach (var item in Base.Successors)
{
StepThroughTree(item, _Action);
_Action(item);
}
}
private void SelectPoints(Random random, int distance, out RRTNode node1, out RRTNode node2)
{
//Select two random points
int indexNode1 = random.Next(distance + 1, Path.CountNodes - 1);
node1 = Path.SelectNode(indexNode1);
node2 = Path.SelectNode(random.Next(1, indexNode1 - distance));
}
/// <summary>
/// Take the nodes in this path and put them into a list. Used for easier iteration.
/// </summary>
/// <returns>The list.</returns>
public List <RRTNode> ToList()
{
List <RRTNode> nodes = new List <RRTNode> ();
RRTNode node = Start;
while (node != null)
{
nodes.Add(node);
node = node.Predecessor;
}
return(nodes);
}
/// <summary>
/// Steps to node.
/// Takes a start and an end node and the argument wether to go straight or in a circle.
/// Steps into this direction
/// </summary>
/// <param name="Start">Start.</param>
/// <param name="End">End.</param>
private void StepToNodeStraight(RRTNode Start, RRTNode End)
{
double distance = RRTHelpers.CalculateDistance(Start, End);
double angle = RRTHelpers.CalculateAngle(Start, End);
double cosArgument = Math.Cos(angle);
double sinArgument = Math.Sin(angle);
RRTNode lastFoundNode = null;
//Lambda function that calculates a new point from a given x value
//Checks if the node is valid
//Adds it into the list of nodes.
//Returns false if point not valid
Func <double, bool> CalculateNewPoint = (double x) =>
{
int NewX = Start.Position.X + (int)((double)x * cosArgument);
int NewY = Start.Position.Y + (int)((double)x * sinArgument);
if (!PointValid(new Point(NewX, NewY)))
{
if (lastFoundNode == null)
{
RRTNode BetweenNode = new RRTNode(new Point(NewX, NewY), Start.Orientation, Start);
Start.AddSucessor(BetweenNode);
lastFoundNode = BetweenNode;
BetweenNode.Inverted = End.Inverted;
this.AllNodes.Add(BetweenNode);
}
else
{
RRTNode BetweenNode = new RRTNode(new Point(NewX, NewY), lastFoundNode.Orientation, lastFoundNode);
lastFoundNode.AddSucessor(BetweenNode);
lastFoundNode = BetweenNode;
BetweenNode.Inverted = End.Inverted;
this.AllNodes.Add(BetweenNode);
}
return(true);
}
else
{
return(false);
}
};
for (double x = 0; x < distance; x += StepWidth)
{
if (!CalculateNewPoint(x))
{
break;
}
}
}
/// <summary>
/// Initializes a new instance of the <see cref="BRRT.PathOptimizer"/> class.
/// </summary>
/// <param name="_Path">Path.</param>
/// <param name="_Map">Map.</param>
/// <param name="_EndPoint">End point.</param>
public PathOptimizer(RRTPath _Path, Map _Map, RRTNode _EndPoint)
{
this.InternalMap = _Map;
this.Path = _Path;
this.Iterations = 30000;
this.MaximumDriftAngle = 10;
this.MinimumRadius = 20;
this.AllowedOrientationDeviation = 1;
this.StepWidthStraight = 15;
this.EndPoint = _EndPoint;
this.StepWidthEnd = 4;
this.StepWidthCurve = 10;
}
/// <summary>
/// Draws the given RRTPath.
/// </summary>
/// <param name="Path">Path.</param>
/// <param name="_Map">Map.</param>
/// <param name="PathPen">Path pen.</param>
public static void DrawPath(RRTPath Path, Map _Map, Pen PathPen)
{
RRTNode previous = Path.Start;
Graphics g = Graphics.FromImage(_Map.ImageMap);
while (previous != null)
{
RRTHelpers.DrawImportantNode(previous, _Map, 2, Path.Color);
if (previous.Predecessor != null)
{
g.DrawLine(PathPen, _Map.ToMapCoordinates(previous.Position), _Map.ToMapCoordinates(previous.Predecessor.Position));
}
previous = previous.Predecessor;
}
}
/// <summary>
/// Draws the tree from the startpoint.
/// </summary>
/// <param name="Base">Base.</param>
/// <param name="_Map">Map.</param>
public static void DrawTree(RRTNode Base, Map _Map)
{
DrawImportantNode(Base, _Map, 5, Color.Red);
Action <RRTNode> DrawAction = null;
DrawAction = (RRTNode node) => {
DrawImportantNode(node, _Map, 2, Color.Blue);
foreach (var item in node.Successors)
{
Point position = _Map.ToMapCoordinates(node.Position);
Point sucPosition = _Map.ToMapCoordinates(item.Position);
Graphics g = Graphics.FromImage(_Map.ImageMap);
g.DrawLine(Pens.Black, position, sucPosition);
}
};
StepThroughTree(Base, DrawAction);
}
/// <summary>
/// Do a step into the right direction
/// </summary>
private void DoStep()
{
bool Curve = RRTHelpers.BooleanRandom(this.PreferStraight);
//Select a random base node from the list of all nodes
RRTNode RandomNode = RRTHelpers.SelectRandomNode(AllNodes);
//large value -> don' take nearest so often
bool SelectNearest = RRTHelpers.BooleanRandom(130);
if (SelectNearest)
{
//Take from 10 nodes the node that is the nearest to the endpoint
double bestDistance = RRTHelpers.CalculateDistance(RandomNode, EndRRTNode);
for (int i = 0; i < 10; i++)
{
RRTNode NewNode = RRTHelpers.SelectRandomNode(AllNodes);
double distance = RRTHelpers.CalculateDistance(NewNode, EndRRTNode);
if (distance < bestDistance)
{
bestDistance = distance;
RandomNode = NewNode;
}
}
}
if (!Curve)
{
//First go straight
//Get a new straight or drift random node
RRTNode NewStraightNode = RRTHelpers.GetRandomStraightPoint(RandomNode, this.MaximumDrift, this.StraightInvertProbability);
//Now step to the new node
StepToNodeStraight(RandomNode, NewStraightNode);
}
else
{
//Second go curve
double Distance = 0;
double Angle = 0;
double BaseAngle = 0;
bool Left = false;
Point Middle = new Point();
RRTNode NewCurveNode = RRTHelpers.GetRandomCurvePoint(RandomNode, this.MinumumRadius, ref Distance, ref Angle, ref BaseAngle, ref Middle, ref Left);
//RRTHelpers.DrawImportantNode(NewCurveNode, InternalMap, 4, Color.CornflowerBlue);
StepToNodeCurve(RandomNode, NewCurveNode, Distance, Angle, BaseAngle, Middle, Left);
}
}
/// <summary>
/// Calculates the length and the amount of nodes in the path.
/// </summary>
public void CalculateLength()
{
Length = 0;
CountNodes = 0;
RRTNode previous = Start;
while (previous != null)
{
if (previous.Predecessor != null)
{
Length += RRTHelpers.CalculateDistance(previous, previous.Predecessor);
}
previous = previous.Predecessor;
CountNodes++;
}
//Put them into the list.
NodesList = ToList();
}
/// <summary>
/// Gets the random curve point.
/// </summary>
/// <returns>The random curve point.</returns>
/// <param name="BaseNode">Base node.</param>
/// <param name="MinimumRadius">Minimum radius.</param>
/// <param name="_Distance">Distance.</param>
/// <param name="_Angle">Angle.</param>
public static RRTNode GetRandomCurvePoint(RRTNode BaseNode, double MinimumRadius, ref double _Distance, ref double _Angle, ref double _BaseAngle, ref Point _Middle, ref bool Left, int MaximumCurveDistance = 300)
{
//Decide whether we want to have right or left turn
//True = left , Right = false
bool LeftOrRight = BooleanRandom(256 / 2);
Left = LeftOrRight;
//Get Random value for the distance between or choosen point and the middle of the circle.
double Distance = Randomizer.NextDouble() * MaximumCurveDistance + MinimumRadius;
// Angle between 0 and 360.
double Angle = (Randomizer.NextDouble()) * 360;
//Angle to our middle point (orthogonal to orientation)
double AngleToMiddle = BaseNode.Orientation;
if (Left)
{
AngleToMiddle += 90;
}
else
{
AngleToMiddle -= 90;
}
AngleToMiddle = SanatizeAngle(AngleToMiddle);
//Calculate center point
double MiddleX = BaseNode.Position.X + Math.Cos(AngleToMiddle * ToRadians) * Distance;
double MiddleY = BaseNode.Position.Y + Math.Sin(AngleToMiddle * ToRadians) * Distance;
Point Middle = new Point((int)MiddleX, (int)MiddleY);
double BaseAngle = 0;
BaseAngle = Math.Atan2(BaseNode.Position.Y - Middle.Y, BaseNode.Position.X - Middle.X) * ToDegree;
BaseAngle = SanatizeAngle(BaseAngle);
//Calculate new point
int NewX = 0;
int NewY = 0;
NewX = Middle.X + (int)((double)Distance * Math.Cos((Angle) * ToRadians));
NewY = Middle.Y + (int)((double)Distance * Math.Sin((Angle) * ToRadians));
_Angle = Angle;
//We drive backwarts if Angle > Baseangle
bool Inverted = (InvertOrientation(BaseAngle) > Angle) && (Angle > BaseAngle); //^ BaseNode.Inverted;
double NewOrientation = 0;
NewOrientation = BaseNode.Orientation - (BaseAngle - Angle);
NewOrientation = SanatizeAngle(NewOrientation);
//Console.WriteLine("Orientation: " + NewOrientation);
_Distance = Distance;
_Middle = Middle;
_BaseAngle = BaseAngle;
RRTNode Node = new RRTNode(new Point(NewX, NewY), NewOrientation, null);
Node.Inverted = Inverted;
//Console.WriteLine(Node);
//Console.WriteLine();
return(Node);
}
private void GenerateStartLine()
{
//TODO calculate distance
double Distance = 1000;
for (int offset = (int)-MaximumDrift; offset < (int)MaximumDrift; offset++)
{
RRTNode lastFound = null;
for (int i = 0; i < Distance; i = i + StepWidth)
{
int NewX = StartRRTNode.Position.X + (int)((double)i * Math.Cos((StartRRTNode.Orientation + offset) * RRTHelpers.ToRadians));
int NewY = StartRRTNode.Position.Y + (int)((double)i * Math.Sin((StartRRTNode.Orientation + offset) * RRTHelpers.ToRadians));
if (!PointValid(new Point((int)NewX, (int)NewY)))
{
if (lastFound == null)
{
RRTNode NewNode = new RRTNode(new Point(NewX, NewY), StartRRTNode.Orientation + offset, StartRRTNode);
StartRRTNode.AddSucessor(NewNode);
this.AllNodes.Add(NewNode);
lastFound = NewNode;
}
else
{
RRTNode NewNode = new RRTNode(new Point(NewX, NewY), StartRRTNode.Orientation + offset, lastFound);
lastFound.AddSucessor(NewNode);
this.AllNodes.Add(NewNode);
lastFound = NewNode;
}
}
else
{
break;
}
}
lastFound = null;
for (int i = 0; i < Distance; i = i + StepWidth)
{
int NewX = StartRRTNode.Position.X + (int)((double)i * Math.Cos(RRTHelpers.InvertOrientation(StartRRTNode.Orientation + offset) * RRTHelpers.ToRadians));
int NewY = StartRRTNode.Position.Y + (int)((double)i * Math.Sin(RRTHelpers.InvertOrientation(StartRRTNode.Orientation + offset) * RRTHelpers.ToRadians));
if (!PointValid(new Point((int)NewX, (int)NewY)))
{
if (lastFound == null)
{
RRTNode NewNode = new RRTNode(new Point(NewX, NewY), StartRRTNode.Orientation + offset, StartRRTNode);
NewNode.Inverted = true;
StartRRTNode.AddSucessor(NewNode);
this.AllNodes.Add(NewNode);
lastFound = NewNode;
}
else
{
RRTNode NewNode = new RRTNode(new Point(NewX, NewY), StartRRTNode.Orientation + offset, lastFound);
lastFound.AddSucessor(NewNode);
this.AllNodes.Add(NewNode);
lastFound = NewNode;
NewNode.Inverted = true;
}
}
else
{
break;
}
}
}
}
/// <summary>
/// Calculates the angle from a to b. (a center of coordinate system)
/// </summary>
/// <returns>The angle.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
public static double CalculateAngle(RRTNode a, RRTNode b)
{
return(Math.Atan2(b.Position.Y - a.Position.Y, b.Position.X - a.Position.X));
}
/// <summary>
/// Calculates the distance between the two nodes.
/// </summary>
/// <returns>The distance.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
public static double CalculateDistance(RRTNode a, RRTNode b)
{
return(Math.Sqrt(Math.Pow(a.Position.X - b.Position.X, 2) + Math.Pow(a.Position.Y - b.Position.Y, 2)));
}
/// <summary>
/// Steps to random node in a curve.
/// </summary>
/// <param name="Start">Start.</param>
/// <param name="End">End.</param>
/// <param name="Distance">Distance.</param>
/// <param name="Angle">Angle.</param>
/// <param name="BaseAngle">Base angle.</param>
/// <param name="Middle">Middle.</param>
/// <param name="Left">If set to <c>true</c> left.</param>
private void StepToNodeCurve(RRTNode Start, RRTNode End, double Distance, double Angle, double BaseAngle, Point Middle, bool Left)
{
RRTNode lastFoundNode = null;
//RRTHelpers.DrawImportantNode(new RRTNode(Middle, BaseAngle,null), InternalMap, 5, Color.Coral);
Func <double, bool> CalculateNewPoint = (double x) =>
{
//We interpret the random angle as the angle in a polar coordinate system
int NewX = Middle.X + (int)((double)Distance * Math.Cos((x) * RRTHelpers.ToRadians));
int NewY = Middle.Y + (int)((double)Distance * Math.Sin((x) * RRTHelpers.ToRadians));
double Orientation = Start.Orientation - (BaseAngle - x);
Orientation = RRTHelpers.SanatizeAngle(Orientation);
if (!PointValid(new Point((int)NewX, (int)NewY)))
{
if (lastFoundNode == null)
{
RRTNode BetweenNode = new RRTNode(new Point((int)NewX, (int)NewY), Orientation, Start);
Start.AddSucessor(BetweenNode);
lastFoundNode = BetweenNode;
BetweenNode.Inverted = End.Inverted;
this.AllNodes.Add(BetweenNode);
}
else
{
RRTNode BetweenNode = new RRTNode(new Point((int)NewX, (int)NewY), Orientation, lastFoundNode);
lastFoundNode.AddSucessor(BetweenNode);
lastFoundNode = BetweenNode;
BetweenNode.Inverted = End.Inverted;
this.AllNodes.Add(BetweenNode);
}
return(true);
}
else
{
return(false);
}
};
double AdaptedStepWidth = (CircleStepWidth * 360.0) / (2 * Math.PI * Distance);
if (Left)
{
for (double x = (BaseAngle) + AdaptedStepWidth; x < Angle; x += AdaptedStepWidth)
{
if (!CalculateNewPoint(x)) //Break if a not valid point was stepped into
{
break;
}
}
}
else
{
for (double x = (BaseAngle) - AdaptedStepWidth; x > Angle; x -= AdaptedStepWidth)
{
if (!CalculateNewPoint(x)) //Break if a not valid point was stepped into
{
break;
}
}
}
}
/// <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);
}
/// <summary>
/// Optimize our path so we hit the endpoint
/// </summary>
public void OptimizeForEndPoint()
{
//Go along from then nearest point to the endpoint
RRTNode previous = Path.Start;
Console.WriteLine("Path length before optimization for endpoint: " + Path.Length + " Count: " + Path.CountNodes + " Cost: " + Path.Cost());
Console.WriteLine();
while (previous != null)
{
if (previous == null)
{
break;
}
//Calculate angle delta (angle between orientations) (in degrees)
double delta = previous.Orientation - EndPoint.Orientation;
double angle = RRTHelpers.CalculateAngle(previous, EndPoint);
//Check if the orientation of the selected point is nearly the same as the orientation of the endpoint
//if (Math.Abs(previous.Orientation - EndPoint.Orientation) < AllowedOrientationDeviation * 5 || 180 -Math.Abs(previous.Orientation - EndPoint.Orientation) < AllowedOrientationDeviation * 5 )
if (AnglesAreClose(delta, 0, AllowedOrientationDeviation) && (AnglesAreClose(EndPoint.Orientation, angle * RRTHelpers.ToDegree, MaximumDriftAngle)))
{
//Okey connect them
RRTNode selectedNode = previous;
RRTNode lastNode = null;
//Create a clone we can work on
RRTNode start = selectedNode.Clone();
double Distance = RRTHelpers.CalculateDistance(selectedNode, EndPoint);
if (Math.Abs(RRTHelpers.SanatizeAngle(angle * RRTHelpers.ToDegree)) > this.MaximumDriftAngle)
{
previous = previous.Predecessor;
continue;
}
bool success = true;
//Connect them
for (double i = 0; i <= Distance; i += StepWidthEnd)
{
//Create new point
int NewX = (int)(selectedNode.Position.X + i * Math.Cos(angle));
int NewY = (int)(selectedNode.Position.Y + i * Math.Sin(angle));
//Check if this point is occupied
if (InternalMap.IsOccupied(NewX, NewY))
{
success = false;
break;
}
RRTNode newNode = null;
if (lastNode == null)
{
newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), start.Orientation, start);
start.Successors.Add(newNode);
}
else
{
newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), start.Orientation, lastNode);
lastNode.Successors.Add(newNode);
}
lastNode = newNode;
}
if (lastNode == null)
{
success = false;
}
if (success)
{
Path.Start = lastNode;
//Replace the selectNode with our start node.
start.Predecessor = selectedNode.Predecessor;
selectedNode.Predecessor.Successors.Clear();
selectedNode.Predecessor.AddSucessor(start);
selectedNode.Predecessor = null;
selectedNode.Successors.Clear();
previous = start;
}
}
previous = previous.Predecessor;
}
Path.CalculateLength();
Console.WriteLine("Path length after optimization for endpoint: " + Path.Length + " Count: " + Path.CountNodes + " Cost: " + Path.Cost());
}
/// <summary>
/// Optimize two points taken from the path by trying to connect them via a curve.
/// </summary>
public bool OptimizeCurves(RRTNode start, RRTNode end)
{
//Calculate distance
double distance = RRTHelpers.CalculateDistance(start, end);
//Distance is too small that it makes sense to optimize it
if (distance < 10)
{
return(false);
}
//Calculate angle delta (angle between orientations) (in degrees)
double delta = end.Orientation - start.Orientation;
//Calculate angle between points (in radians)
double angle = RRTHelpers.CalculateAngle(start, end);
//We can't go from inverted to not inverted (not from forward to backwards or the other way round)
//NOTE Mit Kurvenfahrt ist das kein Problem, kann aber vermehrt zu Wendeschleifen führen
if (start.Inverted != end.Inverted)
{
return(false);
}
//Now decide if going straight is way to go
//NOTE delta ist entweder sehr klein oder sehr groß (fast 360°, siehe Hilfsfunktion "anglesAreClose" in pseudocode)
//NOTE Im zweiten Check prüfe, ob Orientierung und Fahrtwinkel näher zusammenliegen als MaximumDriftAngle
if (AnglesAreClose(delta, 0, AllowedOrientationDeviation) && (AnglesAreClose(start.Orientation, angle * RRTHelpers.ToDegree, MaximumDriftAngle)))
//if (Math.Abs (delta) < AllowedOrientationDeviation*5 && Math.Abs(angle*RRTHelpers.ToDegree) < MaximumDriftAngle)
{
//The deviation in the orientation is small enough we can accept going straight (or drift)
//And the angle between the points is smaller than the maximum drift we can do
//Step straight or in a drift. This function may manipulate the path
bool ret = StepStraight(start, end, distance, angle);
return(ret);
}
else
{
//We try a curve
double theta = RRTHelpers.SanatizeAngle(angle * RRTHelpers.ToDegree + Math.Sign(delta) * (180 - Math.Abs(delta)) / 2) * RRTHelpers.ToRadians;
double radius = Math.Abs(distance / (2 * Math.Sin(delta * RRTHelpers.ToRadians / 2)));
//Check if the radius is > minimum radius
if (radius < MinimumRadius)
{
return(false);
}
//Calculate middle point (theta is in radians)
double midX = start.Position.X + Math.Cos(theta) * radius;
double midY = start.Position.Y + Math.Sin(theta) * radius;
//RRTHelpers.DrawImportantNode (new RRTNode (new System.Drawing.Point ((int)midX, (int)midY), theta, null), InternalMap, 5, System.Drawing.Color.DarkMagenta);
//Theta in radians, delta in degrees -> gamma in degrees
double gamma = start.Orientation - RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree - Math.Sign(delta) * 90);
double driftAngle = gamma; //In degree
if (Math.Abs(driftAngle) / MaximumDriftAngle + MinimumRadius / radius >= 1)
{
return(false);
}
bool ret = StepCurve(start, end, delta, new System.Drawing.Point((int)midX, (int)midY), radius, angle, theta);
return(ret);
}
}
/// <summary>
/// Adds a sucessor to the list of successors
/// </summary>
/// <param name="Node">Node.</param>
public void AddSucessor(RRTNode Node)
{
Successors.Add(Node);
}
private bool StepCurve(RRTNode node1, RRTNode node2, double delta, System.Drawing.Point middle, double radius, double angle, double theta)
{
double outerlength = Math.Abs(delta * RRTHelpers.ToRadians * radius);
double steps = outerlength / StepWidthCurve;
double angleStep = delta / steps;
RRTNode start = node1.Clone();
RRTNode end = node2.Clone();
double beforeLength = 0;
RRTNode temp = node1;
while (temp.Position != end.Position)
{
if (temp == null || temp.Predecessor == null)
{
break;
}
beforeLength += RRTHelpers.CalculateDistance(temp, temp.Predecessor);
temp = temp.Predecessor;
}
RRTNode lastNode = null;
bool success = true;
for (int i = 0; i < (int)steps; i++)
{
int NewX = (int)(middle.X - Math.Cos(RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree + i * angleStep) * RRTHelpers.ToRadians) * radius);
int NewY = (int)(middle.Y - Math.Sin(RRTHelpers.SanatizeAngle(theta * RRTHelpers.ToDegree + i * angleStep) * RRTHelpers.ToRadians) * radius);
double NewOrientation = RRTHelpers.SanatizeAngle(node1.Orientation + i * angleStep);
if (InternalMap.IsOccupied(NewX, NewY))
{
success = false;
break;
}
RRTNode newNode = null;
if (lastNode == null)
{
newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), NewOrientation, start);
newNode.Inverted = start.Inverted;
start.Successors.Add(newNode);
}
else
{
newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), NewOrientation, lastNode);
lastNode.Successors.Add(newNode);
newNode.Inverted = lastNode.Inverted;
//RRTHelpers.DrawImportantNode (newNode, InternalMap, 5, Color.DarkOrange);
}
lastNode = newNode;
}
if (lastNode == null)
{
success = false;
}
//We successfully connected them
if (success)
{
double afterLength = 0;
temp = lastNode;
while (temp.Position != end.Position)
{
if (temp == null || temp.Predecessor == null)
{
break;
}
afterLength += RRTHelpers.CalculateDistance(temp, temp.Predecessor);
temp = temp.Predecessor;
}
if (afterLength > beforeLength)
{
return(false);
}
end.Predecessor = lastNode;
lastNode.AddSucessor(end);
if (node1.Predecessor != null)
{
node1.Predecessor.Successors.Clear();
node1.Predecessor.AddSucessor(start);
start.Predecessor = node1.Predecessor;
}
else
{
Console.WriteLine("Node1.Predecessor was null");
}
if (node2.Successors.Count > 0)
{
end.AddSucessor(node2.Successors[0]);
node2.Successors[0].Predecessor = end;
node2.Predecessor = null;
node2.Successors.Clear();
}
else
{
Console.WriteLine("Node2.Successor[0] was null");
}
node1.Successors.Clear();
node2.Successors.Clear();
node2.Predecessor = null;
node1.Predecessor = null;
Path.CalculateLength();
return(true);
}
return(false);
}
private bool StepStraight(RRTNode node1, RRTNode node2, double Distance, double Angle)
{
RRTNode start = node1.Clone();
RRTNode end = node2.Clone();
RRTNode lastNode = null;
bool success = true;
//Connect them
for (double i = 0; i <= Distance; i += StepWidthStraight)
{
int NewX = (int)(start.Position.X + i * Math.Cos(Angle));
int NewY = (int)(start.Position.Y + i * Math.Sin(Angle));
if (InternalMap.IsOccupied(NewX, NewY))
{
success = false;
break;
}
RRTNode newNode = null;
if (lastNode == null)
{
newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), node1.Orientation, start);
newNode.Inverted = start.Inverted;
start.Successors.Add(newNode);
}
else
{
newNode = new RRTNode(new System.Drawing.Point(NewX, NewY), node1.Orientation, lastNode);
lastNode.Successors.Add(newNode);
newNode.Inverted = lastNode.Inverted;
}
lastNode = newNode;
}
if (lastNode == null)
{
success = false;
}
//We successfully connected them
if (success)
{
end.Predecessor = lastNode;
lastNode.AddSucessor(end);
if (node1.Predecessor != null)
{
node1.Predecessor.Successors.Clear();
node1.Predecessor.AddSucessor(start);
start.Predecessor = node1.Predecessor;
}
else
{
Console.WriteLine("Node1.Predecessor was null");
}
if (node2.Successors.Count > 0)
{
end.AddSucessor(node2.Successors[0]);
node2.Successors[0].Predecessor = end;
node2.Predecessor = null;
node2.Successors.Clear();
}
else
{
Console.WriteLine("Node2.Successor[0] was null");
}
node1.Successors.Clear();
node2.Successors.Clear();
node2.Predecessor = null;
node1.Predecessor = null;
Path.CalculateLength();
return(true);
}
return(false);
}
