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