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