//One path CCC void Get_CCC_Length(Vector3 startCircle, Vector3 goalCircle, bool isRightToRight, OneReedsSheppPath pathData) { //Find both tangent positions and the position of the 3rd circles reedsSheppMath.Get_CCC_Tangents( startCircle, goalCircle, isRightToRight, pathData); //Calculate the total length of this path float length1 = reedsSheppMath.GetArcLength( startCircle, startPos, pathData.startTangent, pathData.segmentsList[0]); float length2 = reedsSheppMath.GetArcLength( pathData.middleCircleCoordinate, pathData.startTangent, pathData.goalTangent, pathData.segmentsList[1]); float length3 = reedsSheppMath.GetArcLength( goalCircle, pathData.goalTangent, goalPos, pathData.segmentsList[2]); //Save the data pathData.AddPathLengths(length1, length2, length3); //Add the path to the collection of all paths allReedsSheppPaths.Add(pathData); }
//One path CC turn CC void Get_CC_turn_CC_Length(Vector3 startCircle, Vector3 goalCircle, bool isBottom, OneReedsSheppPath pathData) { //Find the 3 tangent points and the 2 middle circle positions reedsSheppMath.CC_turn_CC( startCircle, goalCircle, isBottom, pathData); //Calculate the total length of each path float length1 = reedsSheppMath.GetArcLength( startCircle, startPos, pathData.startTangent, pathData.segmentsList[0]); float length2 = reedsSheppMath.GetArcLength( pathData.middleCircleCoordinate, pathData.startTangent, pathData.middleTangent, pathData.segmentsList[1]); float length3 = reedsSheppMath.GetArcLength( pathData.middleCircleCoordinate2, pathData.middleTangent, pathData.goalTangent, pathData.segmentsList[2]); float length4 = reedsSheppMath.GetArcLength( goalCircle, pathData.goalTangent, goalPos, pathData.segmentsList[3]); //Save the lengths pathData.AddPathLengths(length1, length2, length3, length4); //Add the final path allReedsSheppPaths.Add(pathData); }
//One path CSC void Get_CSC_Length( Vector3 startCircle, Vector3 goalCircle, bool isOuterTangent, bool isBottomTangent, OneReedsSheppPath pathData) { //Find both tangent positions if (isOuterTangent) { reedsSheppMath.LSLorRSR(startCircle, goalCircle, isBottomTangent, pathData); } else { reedsSheppMath.RSLorLSR(startCircle, goalCircle, isBottomTangent, pathData); } //Calculate the total length of this path float length1 = reedsSheppMath.GetArcLength( startCircle, startPos, pathData.startTangent, pathData.segmentsList[0]); float length2 = (pathData.startTangent - pathData.goalTangent).magnitude; float length3 = reedsSheppMath.GetArcLength( goalCircle, pathData.goalTangent, goalPos, pathData.segmentsList[2]); //Save the data pathData.AddPathLengths(length1, length2, length3); //Add the path to the collection of all paths allReedsSheppPaths.Add(pathData); }