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