//LRL
private static OneDubinsPath Get_LRL_Path()
{
//Find both tangent positions and the position of the 3rd circle
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
//Center of the 3rd circle
Vector3 middleCircle = Vector3.zero;
float turningRadius = startCar.turningRadius;
DubinsMath.GetRLRorLRLTangents(
startLeftCircle,
goalLeftCircle,
true,
out startTangent,
out goalTangent,
out middleCircle,
turningRadius);
//Calculate the total length of this path
float length1 = DubinsMath.GetArcLength(startLeftCircle, startCar.pos, startTangent, true, turningRadius);
float length2 = DubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false, turningRadius);
float length3 = DubinsMath.GetArcLength(goalLeftCircle, goalTangent, goalCar.pos, true, turningRadius);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LRL);
return(pathData);
}
//LSR
private static OneDubinsPath Get_LSR_Path()
{
//Find both tangent positions
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
float turningRadius = startCar.turningRadius;
DubinsMath.RSLorLSR(startLeftCircle, goalRightCircle, true, out startTangent, out goalTangent, turningRadius);
//Calculate lengths
float length1 = DubinsMath.GetArcLength(startLeftCircle, startCar.pos, startTangent, true, turningRadius);
float length2 = (startTangent - goalTangent).magnitude;
float length3 = DubinsMath.GetArcLength(goalRightCircle, goalTangent, goalCar.pos, false, turningRadius);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LSR);
return(pathData);
}