// // CSC // void CalculatePathLengths_CSC() { bool isOuterTangent = false; bool isBottomTangent = false; int segments = 3; OneReedsSheppPath pathData = null; // //LSL and RSR is only working if the circles don't have the same position // //LSL if (!startLeftCircle.Equals(goalLeftCircle)) { isOuterTangent = true; //L+ S+ L+ isBottomTangent = true; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, true); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(false, false, false); Get_CSC_Length(startLeftCircle, goalLeftCircle, isOuterTangent, isBottomTangent, pathData); //L- S- L- isBottomTangent = false; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, true); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(true, true, true); Get_CSC_Length(startLeftCircle, goalLeftCircle, isOuterTangent, isBottomTangent, pathData); } //RSR if (!startRightCircle.Equals(goalRightCircle)) { isOuterTangent = true; //R+ S+ R+ isBottomTangent = false; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, false, false); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(false, false, false); Get_CSC_Length(startRightCircle, goalRightCircle, isOuterTangent, isBottomTangent, pathData); //R- S- R- isBottomTangent = true; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, false, false); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(true, true, true); Get_CSC_Length(startRightCircle, goalRightCircle, isOuterTangent, isBottomTangent, pathData); } // // LSR and RSL is only working of the circles don't intersect // float comparisonSqr = reedsSheppMath.TurningRadius * 2f * reedsSheppMath.TurningRadius * 2f; //LSR if ((startLeftCircle - goalRightCircle).sqrMagnitude > comparisonSqr) { isOuterTangent = false; //L+ S+ R+ isBottomTangent = true; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, false); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(false, false, false); Get_CSC_Length(startLeftCircle, goalRightCircle, isOuterTangent, isBottomTangent, pathData); //L- S- R- isBottomTangent = false; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, false); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(true, true, true); Get_CSC_Length(startLeftCircle, goalRightCircle, isOuterTangent, isBottomTangent, pathData); } //RSL if ((startRightCircle - goalLeftCircle).sqrMagnitude > comparisonSqr) { isOuterTangent = false; //R+ S+ L+ isBottomTangent = false; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, false, true); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(false, false, false); Get_CSC_Length(startRightCircle, goalLeftCircle, isOuterTangent, isBottomTangent, pathData); //R- S- L- isBottomTangent = true; pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, false, true); pathData.AddIfTurning(true, false, true); pathData.AddIfReversing(true, true, true); Get_CSC_Length(startRightCircle, goalLeftCircle, isOuterTangent, isBottomTangent, pathData); } }
// // CC turn CC // void CalculatePathLength_CC_turn_CC() { //Is only valid if the two circles intersect? float comparisonSqr = reedsSheppMath.TurningRadius * 2f * reedsSheppMath.TurningRadius * 2f; //Always 4 segments int segments = 4; bool isBottom = false; OneReedsSheppPath pathData = null; //RLRL if ((startRightCircle - goalLeftCircle).sqrMagnitude < comparisonSqr) { //R+ L+ R- L- pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, true, false, true); pathData.AddIfTurning(true, true, true, true); pathData.AddIfReversing(false, false, true, true); isBottom = false; Get_CC_turn_CC_Length(startRightCircle, goalLeftCircle, isBottom, pathData); //R- L- R+ L+ pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, true, false, true); pathData.AddIfTurning(true, true, true, true); pathData.AddIfReversing(true, true, false, false); isBottom = false; Get_CC_turn_CC_Length(startRightCircle, goalLeftCircle, isBottom, pathData); } //LRLR if ((startLeftCircle - goalRightCircle).sqrMagnitude < comparisonSqr) { //L+ R+ L- R- pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, true, false); pathData.AddIfTurning(true, true, true, true); pathData.AddIfReversing(false, false, true, true); isBottom = true; Get_CC_turn_CC_Length(startLeftCircle, goalRightCircle, isBottom, pathData); //L- R- L+ R+ pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, true, false); pathData.AddIfTurning(true, true, true, true); pathData.AddIfReversing(true, true, false, false); //Should maybe be false? isBottom = true; Get_CC_turn_CC_Length(startLeftCircle, goalRightCircle, isBottom, pathData); } }
// // CCC // void CalculatePathLengths_CCC() { // // With the CCC paths, the distance between the start and goal have to be less than 4 * r // float maxDistSqr = 4f * reedsSheppMath.TurningRadius * 4f * reedsSheppMath.TurningRadius; //The number of segments is always 3 int segments = 3; // // RLR // if ((startRightCircle - goalRightCircle).sqrMagnitude < maxDistSqr) { List <OneReedsSheppPath> tmpPathList = new List <OneReedsSheppPath>(); //Add all data that's the same for all 6 paths for (int i = 0; i < 5; i++) { OneReedsSheppPath pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(false, true, false); pathData.AddIfTurning(true, true, true); tmpPathList.Add(pathData); } //R+ L- R+ tmpPathList[0].AddIfReversing(false, true, false); //R- L+ R- Can be eliminated //tmpPathList[1].AddIfReversing(true, false, true); //R+ L+ R- tmpPathList[1].AddIfReversing(false, false, true); //R- L- R+ tmpPathList[2].AddIfReversing(true, true, false); //R+ L- R- tmpPathList[3].AddIfReversing(false, true, true); //R- L+ R+ tmpPathList[4].AddIfReversing(true, false, false); //Get all path lengths for (int i = 0; i < tmpPathList.Count; i++) { //Unsure if all should be true but gives better result because no have the same length if all are true Get_CCC_Length(startRightCircle, goalRightCircle, true, tmpPathList[i]); } } // // LRL // if ((startLeftCircle - goalLeftCircle).sqrMagnitude < maxDistSqr) { List <OneReedsSheppPath> tmpPathList = new List <OneReedsSheppPath>(); //Add all data that's the same for all 6 paths for (int i = 0; i < 5; i++) { OneReedsSheppPath pathData = new OneReedsSheppPath(segments); pathData.AddIfTurningLeft(true, false, true); pathData.AddIfTurning(true, true, true); tmpPathList.Add(pathData); } //L+ R- L+ tmpPathList[0].AddIfReversing(false, true, false); //L- R+ L- Can be eliminated //tmpPathList[1].AddIfReversing(true, false, true); //L+ R+ L- tmpPathList[1].AddIfReversing(false, false, true); //L- R- L+ tmpPathList[2].AddIfReversing(true, true, false); //L+ R- L- tmpPathList[3].AddIfReversing(false, true, true); //L- R+ L+ tmpPathList[4].AddIfReversing(true, false, false); for (int i = 0; i < tmpPathList.Count; i++) { Get_CCC_Length(startLeftCircle, goalLeftCircle, false, tmpPathList[i]); } } }