//LRL - Find both tangent positions and the position of the 3rd circle
private void Get_LRL_Length()
{
vec2 startTangent = new vec2(0, 0);
vec2 goalTangent = new vec2(0, 0);
vec2 middleCircle = new vec2(0, 0);
DubinsMath.GetRLRorLRLTangents(
startLeftCircle,
goalLeftCircle,
true,
out startTangent,
out goalTangent,
out middleCircle);
//Calculate the total length of this path
double length1 = DubinsMath.GetArcLength(startLeftCircle, startPos, startTangent, true);
double length2 = DubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false);
double length3 = DubinsMath.GetArcLength(goalLeftCircle, goalTangent, goalPos, true);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LRL);
//We also need this data to simplify when generating the final path
pathData.segment2Turning = true;
//LRL
pathData.SetIfTurningRight(false, true, false);
//Add the path to the collection of all paths
pathDataList.Add(pathData);
}
//LSR
private void Get_LSR_Length()
{
//Find both tangent positions
vec2 startTangent = new vec2(0, 0);
vec2 goalTangent = new vec2(0, 0);
DubinsMath.RSLorLSR(startLeftCircle, goalRightCircle, true, out startTangent, out goalTangent);
//Calculate lengths
double length1 = DubinsMath.GetArcLength(startLeftCircle, startPos, startTangent, true);
double length2 = (startTangent - goalTangent).GetLength();
double length3 = DubinsMath.GetArcLength(goalRightCircle, goalTangent, goalPos, false);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LSR);
//We also need this data to simplify when generating the final path
pathData.segment2Turning = false;
//LSR
pathData.SetIfTurningRight(false, false, true);
//Add the path to the collection of all paths
pathDataList.Add(pathData);
}
//RLR - Find both tangent positions and the position of the 3rd circle
private void Get_RLR_Length()
{
DubinsMath.GetRLRorLRLTangents(
startRightCircle,
goalRightCircle,
false,
out vec2 startTangent,
out vec2 goalTangent,
out vec2 middleCircle);
//Calculate lengths
double length1 = DubinsMath.GetArcLength(startRightCircle, startPos, startTangent, false);
double length2 = DubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, true);
double length3 = DubinsMath.GetArcLength(goalRightCircle, goalTangent, goalPos, false);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.RLR)
{
//We also need this data to simplify when generating the final path
segment2Turning = true
};
//RLR
pathData.SetIfTurningRight(true, false, true);
//Add the path to the collection of all paths
pathDataList.Add(pathData);
}
//Find the coordinates of the entire path from the 2 tangents and length of each segment
private void GetTotalPath(OneDubinsPath pathData)
{
//Store the waypoints of the final path here
List <vec2> finalPath = new List <vec2>();
//Start position of the car
vec2 currentPos = startPos;
//Start heading of the car
double theta = startHeading;
//We always have to add the first position manually
finalPath.Add(currentPos);
//How many line segments can we fit into this part of the path
//First
int segments = (int)Math.Floor(pathData.length1 / CDubins.driveDistance);
DubinsMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
segments,
true,
pathData.segment1TurningRight);
//Second
segments = (int)Math.Floor(pathData.length2 / CDubins.driveDistance);
DubinsMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
segments,
pathData.segment2Turning,
pathData.segment2TurningRight);
//Third
segments = (int)Math.Floor(pathData.length3 / CDubins.driveDistance);
DubinsMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
segments,
true,
pathData.segment3TurningRight);
//Add the final goal coordinate
finalPath.Add(new vec2(goalPos.easting, goalPos.northing));
//Save the final path in the path data
pathData.pathCoordinates = finalPath;
}
//Position the left and right circles that are to the left/right of the target and the car
private void PositionLeftRightCircles()
{
//Goal pos
goalRightCircle = DubinsMath.GetRightCircleCenterPos(goalPos, goalHeading);
goalLeftCircle = DubinsMath.GetLeftCircleCenterPos(goalPos, goalHeading);
//Start pos
startRightCircle = DubinsMath.GetRightCircleCenterPos(startPos, startHeading);
startLeftCircle = DubinsMath.GetLeftCircleCenterPos(startPos, startHeading);
}