//LSR
void Get_LSR_Length(Vector3 startCircle, Vector3 goalCircle, List <OneDubinsPath> finalPathList)
{
//Find both tangent positions
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
dubinsMath.RSLorLSR(startCircle, goalCircle, true, out startTangent, out goalTangent);
//Calculate lengths
float length1 = dubinsMath.GetArcLength(startCircle, startPos, startTangent, true);
float length2 = (startTangent - goalTangent).magnitude;
float length3 = dubinsMath.GetArcLength(goalCircle, goalTangent, goalObj.position, 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.path2Turning = false;
//LSR
pathData.SetIfTurningRight(false, false, true);
//Add the path to the collection of all paths
finalPathList.Add(pathData);
}
//LRL
void Get_LRL_Length(Vector3 startCircle, Vector3 goalCircle, bool isLRL, List <OneDubinsPath> finalPathList)
{
//Find both tangent positions
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
//Center of the 3rd circle
Vector3 middleCircle = Vector3.zero;
//Calculate the positions of the 3 circles
dubinsMath.GetRLRorLRLTangents(
startCircle,
goalCircle,
isLRL,
out startTangent,
out goalTangent,
out middleCircle);
//Calculate the total length of this path
float length1 = dubinsMath.GetArcLength(startCircle, startPos, startTangent, true);
float length2 = dubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false);
float length3 = dubinsMath.GetArcLength(goalCircle, goalTangent, goalObj.position, 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.path2Turning = true;
//LRL
pathData.SetIfTurningRight(false, true, false);
//Add the path to the collection of all paths
finalPathList.Add(pathData);
}
