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