/// <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>
/// 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>
/// 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());
}
