//Get one Reeds-Shepp path and display it
private void GetOnePath()
{
//Deactivate all line renderers
DeactivateAllLineRenderers();
//Get one path
Vector3 startPos = startCarObj.transform.position;
float startHeading = startCarObj.transform.eulerAngles.y * Mathf.Deg2Rad;
Vector3 goalPos = goalCarObj.transform.position;
float goalHeading = goalCarObj.transform.eulerAngles.y * Mathf.Deg2Rad;
OneReedsSheppPath oneReedsSheppPath = generateReedsShepp.GetOneReedSheppPath(startPos, startHeading, goalPos, goalHeading);
if (oneReedsSheppPath != null && oneReedsSheppPath.pathCoordinates.Count > 0)
{
//Display the path with a line renderer
DisplayOneReedsSheppPath(oneReedsSheppPath.pathCoordinates);
//Display the tangent spheres
//DisplayTangentSpheres(oneReedsSheppPath);
}
}
//Outer tangent (LSL and RSR)
public void LSLorRSR(
Vector3 startCircle,
Vector3 goalCircle,
bool isBottom,
OneReedsSheppPath pathData)
{
//The angle to the first tangent coordinate is always 90 degrees if the both circles have the same radius
float theta = 90f * Mathf.Deg2Rad;
//Need to modify theta if the circles are not on the same height (z)
theta += Mathf.Atan2(goalCircle.z - startCircle.z, goalCircle.x - startCircle.x);
//Add pi to get the "bottom" coordinate which is on the opposite side (180 degrees = pi)
if (isBottom)
{
theta += Mathf.PI;
}
//The coordinates of the first tangent points
float xT1 = startCircle.x + turningRadius * Mathf.Cos(theta);
float zT1 = startCircle.z + turningRadius * Mathf.Sin(theta);
//To get the second coordinate we need a direction
//This direction is the same as the direction between the center pos of the circles
Vector3 dirVec = goalCircle - startCircle;
float xT2 = xT1 + dirVec.x;
float zT2 = zT1 + dirVec.z;
//The final coordinates of the tangent lines
pathData.startTangent = new Vector3(xT1, 0.1f, zT1);
pathData.goalTangent = new Vector3(xT2, 0.1f, zT2);
}
//CC|CC
public void CC_turn_CC(
Vector3 startCircle,
Vector3 goalCircle,
bool isBottom,
OneReedsSheppPath pathData)
{
//The distance between the circles
float D = (startCircle - goalCircle).magnitude;
float r = turningRadius;
float a = Mathf.Sqrt((2f * r * 2f * r) - ((r - (D * 0.5f)) * (r - (D * 0.5f))));
//The angle we need to find the first circle center
float theta = Mathf.Acos(a / (2f * r)) + (90f * Mathf.Deg2Rad);
//Need to modify theta if the circles are not on the same height (z)
float atan2 = Mathf.Atan2(goalCircle.z - startCircle.z, goalCircle.x - startCircle.x);
if (isBottom)
{
theta = atan2 - theta;
}
else
{
theta = atan2 + theta;
}
//Center of the circle A
float Ax = startCircle.x + 2f * r * Mathf.Cos(theta);
float Az = startCircle.z + 2f * r * Mathf.Sin(theta);
Vector3 circleAPos = new Vector3(Ax, 0f, Az);
//The direction between the start circle and the goal circle
//is the same as the direction between the outer circles
Vector3 dirVec = (goalCircle - startCircle).normalized;
//And the distance between the outer circles is 2r
//So the position of the second circle is
Vector3 circleBPos = circleAPos + (dirVec * 2f * r);
//Now we can calculate the 3 tangent positions
Vector3 dirVecA = (startCircle - circleAPos).normalized;
Vector3 dirVecB = (goalCircle - circleBPos).normalized;
Vector3 startTangent = circleAPos + (dirVecA * r);
Vector3 middleTangent = circleAPos + (dirVec * r);
Vector3 goalTangent = circleBPos + (dirVecB * r);
//Save everything
pathData.startTangent = startTangent;
pathData.middleTangent = middleTangent;
pathData.goalTangent = goalTangent;
pathData.middleCircleCoordinate = circleAPos;
pathData.middleCircleCoordinate2 = circleBPos;
}
//Display the tangent spheres
void DisplayTangentSpheres(OneReedsSheppPath oneReedsSheppPath)
{
tangentSphere1.gameObject.SetActive(true);
tangentSphere2.gameObject.SetActive(true);
tangentSphere1.position = oneReedsSheppPath.startTangent;
tangentSphere2.position = oneReedsSheppPath.goalTangent;
}
//
// Calculate the start and end positions of the tangent lines
//
//Get the CCC tangent points
public void Get_CCC_Tangents(
Vector3 startCircle,
Vector3 goalCircle,
bool isRLR,
OneReedsSheppPath pathData)
{
//The distance between the circles
float D = (startCircle - goalCircle).magnitude;
//The angle between the goal and the new circle we create
float theta = Mathf.Acos(D / (4f * turningRadius));
//But we need to modify the angle theta if the circles are not on the same line
Vector3 V1 = goalCircle - startCircle;
//Different depending on if we calculate LRL or RLR
if (!isRLR)
{
theta = Mathf.Atan2(V1.z, V1.x) + theta;
}
else
{
theta = Mathf.Atan2(V1.z, V1.x) - theta;
}
//Calculate the position of the third circle
float x = startCircle.x + 2f * turningRadius * Mathf.Cos(theta);
float y = startCircle.y;
float z = startCircle.z + 2f * turningRadius * Mathf.Sin(theta);
Vector3 middleCircleCenter = new Vector3(x, y, z);
//Calculate the tangent points
Vector3 V2 = (startCircle - middleCircleCenter).normalized;
Vector3 V3 = (goalCircle - middleCircleCenter).normalized;
Vector3 startTangent = middleCircleCenter + V2 * turningRadius;
Vector3 goalTangent = middleCircleCenter + V3 * turningRadius;
//Save everything
pathData.middleCircleCoordinate = middleCircleCenter;
pathData.startTangent = startTangent;
pathData.goalTangent = goalTangent;
}
//Inner tangent (RSL and LSR)
public void RSLorLSR(
Vector3 startCircle,
Vector3 goalCircle,
bool isBottom,
OneReedsSheppPath pathData)
{
//Find the distance between the circles
float D = (startCircle - goalCircle).magnitude;
float R = turningRadius;
//If the circles have the same radius we can use cosine and not the law of cosines
//to calculate the angle to the first tangent coordinate
float theta = Mathf.Acos((2f * R) / D);
//If the circles is LSR, then the first tangent pos is on the other side of the center line
if (isBottom)
{
theta *= -1f;
}
//Need to modify theta if the circles are not on the same height
theta += Mathf.Atan2(goalCircle.z - startCircle.z, goalCircle.x - startCircle.x);
//The coordinates of the first tangent point
float xT1 = startCircle.x + turningRadius * Mathf.Cos(theta);
float zT1 = startCircle.z + turningRadius * Mathf.Sin(theta);
//To get the second tangent coordinate we need the direction of the tangent
//To get the direction we move up 2 circle radius and end up at this coordinate
float xT1_tmp = startCircle.x + 2f * turningRadius * Mathf.Cos(theta);
float zT1_tmp = startCircle.z + 2f * turningRadius * Mathf.Sin(theta);
//The direction is between the new coordinate and the center of the target circle
Vector3 dirVec = goalCircle - new Vector3(xT1_tmp, 0f, zT1_tmp);
//The coordinates of the second tangent point is the
float xT2 = xT1 + dirVec.x;
float zT2 = zT1 + dirVec.z;
//The final coordinates of the tangent lines
pathData.startTangent = new Vector3(xT1, 0.1f, zT1);
pathData.goalTangent = new Vector3(xT2, 0.1f, zT2);
}
//Get the shortest Reeds-Shepp path and display it
private void DisplayShortestPath(Transform startCarTrans, Transform goalCarTrans)
{
Vector3 startPos = startCarTrans.position;
float startHeading = startCarTrans.eulerAngles.y * Mathf.Deg2Rad;
Vector3 goalPos = goalCarTrans.position;
float goalHeading = goalCarTrans.eulerAngles.y * Mathf.Deg2Rad;
//Get the path
OneReedsSheppPath oneReedsSheppPath = generateReedsShepp.GetOneReedSheppPath(startPos, startHeading, goalPos, goalHeading);
//If we found a path
if (oneReedsSheppPath != null && oneReedsSheppPath.pathCoordinates.Count > 0)
{
//Display the path with line renderers
//DisplayPath(oneReedsSheppPath.pathCoordinates);
}
}
private void GetOnePathDebug()
{
//Deactivate all line renderers
DeactivateAllLineRenderers();
//Get one path
Vector3 startPos = startCarObj.transform.position;
float startHeading = startCarObj.transform.eulerAngles.y * Mathf.Deg2Rad;
Vector3 goalPos = goalCarObj.transform.position;
float goalHeading = goalCarObj.transform.eulerAngles.y * Mathf.Deg2Rad;
OneReedsSheppPath oneReedsSheppPath = generateReedsShepp.GetOneReedSheppPath(startPos, startHeading, goalPos, goalHeading);
if (oneReedsSheppPath != null)
{
//Display the path with lines
//DisplayOneReedsSheppPath(oneReedsSheppPath.pathCoordinates);
//Display the tangent spheres
//DisplayTangentSpheres(oneReedsSheppPath);
middleCircle1.gameObject.SetActive(true);
middleCircle1.position = oneReedsSheppPath.middleCircleCoordinate;
middleCircle2.gameObject.SetActive(true);
middleCircle2.position = oneReedsSheppPath.middleCircleCoordinate2;
tangentSphere1.gameObject.SetActive(true);
tangentSphere2.gameObject.SetActive(true);
tangentSphere3.gameObject.SetActive(true);
tangentSphere1.position = oneReedsSheppPath.startTangent;
tangentSphere2.position = oneReedsSheppPath.middleTangent;
tangentSphere3.position = oneReedsSheppPath.goalTangent;
}
}
//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);
}
//Find the coordinates of the entire path from the 2 tangents
void GeneratePathCoordinates(OneReedsSheppPath pathData)
{
//Store the waypoints of the final path here
List <Node> finalPath = new List <Node>();
//Start position of the car
Vector3 currentPos = startPos;
//Start heading of the car
float theta = startHeading;
//Loop through all segments and generate the waypoints
for (int i = 0; i < pathData.segmentsList.Count; i++)
{
reedsSheppMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
pathData.segmentsList[i]);
}
//Add the final goal coordinate
Vector3 finalPos = new Vector3(goalPos.x, currentPos.y, goalPos.z);
Node newNode = new Node();
newNode.carPos = finalPos;
newNode.heading = goalHeading;
if (pathData.segmentsList[pathData.segmentsList.Count - 1].isReversing)
{
newNode.isReversing = true;
}
finalPath.Add(newNode);
//Save the final path in the path data
pathData.pathCoordinates = finalPath;
}
//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);
}
//
// 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);
}
}
//
// 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]);
}
}
}
