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