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