//LSR
void Get_LSR_Length(Vector3 startCircle, Vector3 goalCircle, List <OneDubinsPath> finalPathList)
{
//Find both tangent positions
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
dubinsMath.RSLorLSR(startCircle, goalCircle, true, out startTangent, out goalTangent);
//Calculate lengths
float length1 = dubinsMath.GetArcLength(startCircle, startPos, startTangent, true);
float length2 = (startTangent - goalTangent).magnitude;
float length3 = dubinsMath.GetArcLength(goalCircle, goalTangent, goalObj.position, false);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LSR);
//We also need this data to simplify when generating the final path
pathData.path2Turning = false;
//LSR
pathData.SetIfTurningRight(false, false, true);
//Add the path to the collection of all paths
finalPathList.Add(pathData);
}
//Find the coordinates of the entire path from the 2 tangents
void GetTotalPath(OneDubinsPath pathData, bool isReversing)
{
//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 * Mathf.Deg2Rad;
//First
dubinsMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
pathData.length1,
true,
pathData.path1TurningRight,
isReversing);
//Second
dubinsMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
pathData.length2,
pathData.path2Turning,
pathData.path2TurningRight,
isReversing);
//Third
dubinsMath.AddCoordinatesToPath(
ref currentPos,
ref theta,
finalPath,
pathData.length3,
true,
pathData.path3TurningRight,
isReversing);
//Add the final goal coordinate of the real car and not the coordinateb we ended up with
//Because of numerical methods they may be different
Node newNode = new Node();
newNode.carPos = new Vector3(goalObj.position.x, currentPos.y, goalObj.position.z);
newNode.heading = theta;
if (isReversing)
{
newNode.isReversing = true;
}
finalPath.Add(newNode);
//Save the final path in the path data
pathData.pathCoordinates = finalPath;
}
//Find the shortest path from the set of curves and display it - Forward
void FindShortestDubinPathForward()
{
//Get the current position and heading of the car
//In Hybrid A* we can't use the car, but the current position and heading in search tree
Vector3 startPos = startPosObj.position;
float startHeading = startPosObj.eulerAngles.y;
OneDubinsPath shortestPath = dubinsPathGenerator.GetShortestDubinPath(startPos, startHeading);
//If we found a path
if (shortestPath != null)
{
//Display the shortest path with a special line
DisplayPath(shortestPath, lineFinalPath);
}
}
//Display a path with a line renderer
void DisplayPath(OneDubinsPath pathData, LineRenderer lineRenderer)
{
//Activate the line renderer
lineRenderer.gameObject.SetActive(true);
//The coordinates of the final path
List <Node> finalPath = pathData.pathCoordinates;
//Display the final line
//lineRenderer.SetVertexCount(finalPath.Count);
lineRenderer.positionCount = finalPath.Count;
for (int i = 0; i < finalPath.Count; i++)
{
lineRenderer.SetPosition(i, finalPath[i].carPos);
}
}
//LRL
void Get_LRL_Length(Vector3 startCircle, Vector3 goalCircle, bool isLRL, List <OneDubinsPath> finalPathList)
{
//Find both tangent positions
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
//Center of the 3rd circle
Vector3 middleCircle = Vector3.zero;
//Calculate the positions of the 3 circles
dubinsMath.GetRLRorLRLTangents(
startCircle,
goalCircle,
isLRL,
out startTangent,
out goalTangent,
out middleCircle);
//Calculate the total length of this path
float length1 = dubinsMath.GetArcLength(startCircle, startPos, startTangent, true);
float length2 = dubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false);
float length3 = dubinsMath.GetArcLength(goalCircle, goalTangent, goalObj.position, true);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LRL);
//We also need this data to simplify when generating the final path
pathData.path2Turning = true;
//LRL
pathData.SetIfTurningRight(false, true, false);
//Add the path to the collection of all paths
finalPathList.Add(pathData);
}
//Display balls where the tangent coordinates are
void DisplayTangentBalls(OneDubinsPath shortestPath)
{
debugSpheres[0].transform.position = shortestPath.tangent1;
debugSpheres[1].transform.position = shortestPath.tangent2;
}