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