//LRL
private static OneDubinsPath Get_LRL_Path()
{
//Find both tangent positions and the position of the 3rd circle
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
//Center of the 3rd circle
Vector3 middleCircle = Vector3.zero;
float turningRadius = startCar.turningRadius;
DubinsMath.GetRLRorLRLTangents(
startLeftCircle,
goalLeftCircle,
true,
out startTangent,
out goalTangent,
out middleCircle,
turningRadius);
//Calculate the total length of this path
float length1 = DubinsMath.GetArcLength(startLeftCircle, startCar.pos, startTangent, true, turningRadius);
float length2 = DubinsMath.GetArcLength(middleCircle, startTangent, goalTangent, false, turningRadius);
float length3 = DubinsMath.GetArcLength(goalLeftCircle, goalTangent, goalCar.pos, true, turningRadius);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LRL);
return(pathData);
}
//Get the shortest path
public static OneDubinsPath GetShortestPath(Vector3 startPos, float startHeading, Vector3 goalPos, float goalHeading, float turningRadius)
{
startCar = new Car(startPos, startHeading, turningRadius);
goalCar = new Car(goalPos, goalHeading, turningRadius);
List <OneDubinsPath> allPaths = new List <OneDubinsPath>();
//Position the circles that are to the left/right of the cars
PositionLeftRightCircles();
//Find the length of each path with tangent coordinates
CalculateDubinsPathsLengths(allPaths);
//If we have paths
if (allPaths.Count > 0)
{
//Sort the list with paths so the shortest path is first
allPaths.Sort((x, y) => x.totalLength.CompareTo(y.totalLength));
//Get the shortest path
OneDubinsPath shortestPath = allPaths[0];
//Generate the final coordinates of the path from tangent points and segment lengths
GeneratePathWaypoints(shortestPath);
return(shortestPath);
}
//No paths could be found
return(null);
}
//
// Generate the final waypoints we need for navigation
//
private static void GeneratePathWaypoints(OneDubinsPath pathData)
{
//Store the waypoints of the final path here
List <Vector3> waypoints = new List <Vector3>();
//The car that will be simulated along the path
Car car = startCar;
//We always have to add the first position manually = the position of the car
waypoints.Add(car.pos);
//To be able to get evenly spaced waypoints
//Will be reset when we reach a waypoint
float distanceTravelled = 0f;
//The steering wheel positions of each segment
//Tturning left (-1), right (1) or driving forward (0)
int[] steeringWheelPos = pathData.GetSteeringWheelPositions();
//Each path consists of 3 segments
for (int i = 0; i < pathData.segmentLengths.Length; i++)
{
AddCoordinatesToSegment(
ref car,
waypoints,
pathData.segmentLengths[i],
steeringWheelPos[i],
driveStepDistance,
distanceBetweenWaypoints,
ref distanceTravelled);
}
//Make sure the goal endpoint is at the goal position
waypoints[waypoints.Count - 1] = goalCar.pos;
//Save the final path in the path data
pathData.waypoints = waypoints;
}
//LSR
private static OneDubinsPath Get_LSR_Path()
{
//Find both tangent positions
Vector3 startTangent = Vector3.zero;
Vector3 goalTangent = Vector3.zero;
float turningRadius = startCar.turningRadius;
DubinsMath.RSLorLSR(startLeftCircle, goalRightCircle, true, out startTangent, out goalTangent, turningRadius);
//Calculate lengths
float length1 = DubinsMath.GetArcLength(startLeftCircle, startCar.pos, startTangent, true, turningRadius);
float length2 = (startTangent - goalTangent).magnitude;
float length3 = DubinsMath.GetArcLength(goalRightCircle, goalTangent, goalCar.pos, false, turningRadius);
//Save the data
OneDubinsPath pathData = new OneDubinsPath(length1, length2, length3, startTangent, goalTangent, PathType.LSR);
return(pathData);
}