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