//
// Length calculations for the different paths
//
//Basic idea (from "Optimal paths for a car that goes both forwards and backwards"):
//In each formula the objective is to move from (0, 0, 0) to (x, y, phi)
//We write (r, theta) = R(x, y) for the polar transform:
// r * cos(theta) = x
// r * sin(theta) = y
// r >= 0
// -pi <= theta < pi
//So R(x, y) means that we translate x and y to polar coordinates from the cartesian
//https://en.wikipedia.org/wiki/Polar_coordinate_system
//We write phi = M(theta) if phi ≡ theta mod(2*pi) and -pi <= phi < pi, so M(theta) means modulus 2*pi
//L is the overall length. We say L = ∞ if there's no solution
//t, u, v are the unknown segment lengths
//A Reeds-Shepp path consists of 3-5 segments, but only 3 segments have unpredetermined lengths
//The other segments have each a length of a curve where we drive a length of pi/2
//or the same length as another segment in the same path
//The unit of length for a straight segment is whatever we want. For a circular arc it is in radians.
//8.1: CSC, same turn
public static float Lf_Sf_Lf(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//On page 24 in the Reeds-Shepp report, it says:
//(u, t) = R(x - sin(phi), y - 1 + cos(phi))
//v = M(phi - t)
//L = |t| + |u| + |v|
//This path can't be optimal if t or v is outside [0, pi]
R(x - Mathf.Sin(phi), y - 1f + Mathf.Cos(phi), out u, out t);
v = M(phi - t);
//Debug.Log(t + " " + u + " " + v);
//Dont need to check u because its straight
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + v;
return(totalLength);
}
//Loop through all paths and find the shortest one (if one can be found)
private static float FindShortestPathLength(RSCar carEndMod, out PathSegmentLengths bestPathLengths, out PathWords bestWord)
{
//How many paths are we going to check
int numPathWords = Enum.GetNames(typeof(PathWords)).Length;
//Better than using float.MaxValue because we can use float.IsPositiveInfinity(bestPathLength) to test if its infinity
float shortestPathLength = float.PositiveInfinity;
bestWord = 0;
//Will keep track of the length of the best path
//Some Reeds-Shepp segments have 5 lengths, but 2 of those are known, so we only need 3 to find the shortest path
bestPathLengths = new PathSegmentLengths(0f, 0f, 0f);
//Loop through all paths that are enums to find the shortest
for (int w = 0; w < numPathWords; w++)
{
PathWords word = (PathWords)w;
PathSegmentLengths pathSegmentLengths;
float pathLength = PathLengthMath.GetLength(carEndMod, word, out pathSegmentLengths);
if (pathLength < shortestPathLength)
{
shortestPathLength = pathLength;
bestWord = word;
bestPathLengths = pathSegmentLengths;
}
}
return(shortestPathLength);
}
//Find the shortest Reeds-Shepp path and generate waypoints to follow that path
private static List <RSCar> FindShortestPath(
RSCar carStart, RSCar carEnd, RSCar carEndMod, float wpDistance, float turningRadius, bool generateOneWp)
{
PathSegmentLengths bestPathLengths;
PathWords bestWord;
float shortestPathLength = FindShortestPathLength(carEndMod, out bestPathLengths, out bestWord);
//No path could be found
if (float.IsPositiveInfinity(shortestPathLength))
{
Debug.Log("Cant find a Reeds-Shepp path");
return(null);
}
//else
//{
// Debug.Log(bestWord);
//}
//Calculate the waypoints to complete this path
//Use the car's original start position because we no longer need the normalized version
List <RSCar> shortestPath = AddWaypoints(bestWord, bestPathLengths, carStart, carEnd, wpDistance, turningRadius, generateOneWp);
return(shortestPath);
}
//Same as above but we just want the shortest distance
public static float GetShortestDistance(Vector3 startPos, float startHeading, Vector3 endPos, float endHeading, float turningRadius)
{
RSCar carStart = new RSCar(startPos, startHeading);
RSCar carEnd = new RSCar(endPos, endHeading);
//The formulas assume we move from(0, 0, 0) to(x, y, theta), and that the turning radius is 1
//This means we have to move and rotate the goal's position and heading as if the start's position and heading had been at (0,0,0)
RSCar carEndMod = NormalizeGoalCar(carStart, carEnd, turningRadius);
PathSegmentLengths bestPathLengths;
PathWords bestWord;
float shortestPathLength = FindShortestPathLength(carEndMod, out bestPathLengths, out bestWord);
//No path could be found
if (float.IsPositiveInfinity(shortestPathLength))
{
Debug.Log("Cant find a Reeds-Shepp path");
return(shortestPathLength);
}
//Convert back to the actual diistance by using the turning radius
shortestPathLength *= turningRadius;
return(shortestPathLength);
}
//Copy data from car to this car
public RSCar(RSCar car)
{
this.pos = car.pos;
this.heading = car.heading;
this.gear = car.gear;
this.steering = car.steering;
}
//8.9: C|C(pi/2)SC, same turn
public static float Lf_Rbpi2_Sb_Lb(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//Uses a modified formula adapted from the c_c2sca function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
float xi = x - Mathf.Sin(phi);
float eta = y - 1f + Mathf.Cos(phi);
float u1, theta;
R(xi, eta, out u1, out theta);
float u1Squared = u1 * u1;
if (u1Squared < 4f)
{
return(float.PositiveInfinity);
}
//This is the part thats different from the original report:
float va1 = 1.25f - ((u1 * u1) / 16f);
if (va1 < 0f || va1 > 1f)
{
return(float.PositiveInfinity);
}
u = Mathf.Sqrt(u1Squared - 4f) - 2f;
if (u < 0f)
{
return(float.PositiveInfinity);
}
float alpha = Mathf.Atan2(2f, u + 2f);
t = M(HALF_PI + theta + alpha);
v = M(t + HALF_PI - phi);
//Check all 2 curves (pi/2 is always a valid curve)
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + HALF_PI + u + v;
return(totalLength);
}
//8.8: C|CuCu|C
public static float Lf_Rub_Lub_Rf(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//Uses a modified formula adapted from the c_cucu_c function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
float xi = x + Mathf.Sin(phi);
float eta = y - 1f - Mathf.Cos(phi);
float u1, theta;
R(xi, eta, out u1, out theta);
if (u1 > 6f)
{
return(float.PositiveInfinity);
}
//This is the part thats different from the original report:
float va1 = 1.25f - ((u1 * u1) / 16f);
if (va1 < 0f || va1 > 1f)
{
return(float.PositiveInfinity);
}
u = Mathf.Acos(va1);
float va2 = Mathf.Sin(u);
float alpha = Mathf.Asin((2f * va2) / u1);
t = M(HALF_PI + theta + alpha);
v = M(t - phi);
//Check all 3 curves
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + u + v;
return(totalLength);
}
//8.7: CCu|CuC
public static float Lf_Ruf_Lub_Rb(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//Uses a modified formula adapted from the ccu_cuc function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
float xi = x + Mathf.Sin(phi);
float eta = y - 1f - Mathf.Cos(phi);
float u1, theta;
R(xi, eta, out u1, out theta);
if (u1 > 4f)
{
return(float.PositiveInfinity);
}
if (u1 > 2f)
{
float alpha = Mathf.Acos((u1 / 4f) - 0.5f);
t = M(HALF_PI + theta - alpha);
u = M(PI - alpha);
v = M(phi - t + 2f * u);
}
else
{
float alpha = Mathf.Acos((u1 / 4f) + 0.5f);
t = M(HALF_PI + theta + alpha);
u = M(alpha);
v = M(phi - t + 2f * u);
}
//Check all 3 curves
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + u + v;
return(totalLength);
}
//8.4: CC|C
public static float Lf_Rf_Lb(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//Uses a modified formula adapted from the cc_c function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
float xi = x - Mathf.Sin(phi);
float eta = y - 1f + Mathf.Cos(phi);
float u1, theta;
R(xi, eta, out u1, out theta);
if (u1 > 4f)
{
return(float.PositiveInfinity);
}
u = Mathf.Acos((8f - (u1 * u1)) / 8f);
float va = Mathf.Sin(u);
float alpha = Mathf.Asin(2f * va / u1);
t = M(HALF_PI - alpha + theta);
v = M(t - u - phi);
//Check all 3 curves
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + v;
return(totalLength);
}
//8.2: CSC, different turn
public static float Lf_Sf_Rf(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
float u1, t1;
R(x + Mathf.Sin(phi), y - 1f - Mathf.Cos(phi), out u1, out t1);
if (u1 * u1 < 4f)
{
return(float.PositiveInfinity);
}
u = Mathf.Sqrt((u1 * u1) - 4f);
float T, theta;
R(u, 2f, out T, out theta);
t = M(t1 + theta);
v = M(t - phi);
//Debug.Log(t + " " + u + " " + v);
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + v;
return(totalLength);
}
//The formulas assume we move from(0, 0, 0) to (x, y, theta), and that the turning radius is 1
//This means we have to move and rotate the goal's position and heading as if the start's position and heading had been at (0,0,0)
//But we are using Unity, so the rotation of the start car has to be along the x-axis and not z-axis which is Unity's zero-rotation
public static RSCar NormalizeGoalCar(RSCar carStart, RSCar carEnd, float turningRadius)
{
//Change the position and rotation of the goal car
Vector3 posDiff = carEnd.pos - carStart.pos;
//Turning radius is 1
posDiff /= turningRadius;
//Unitys coordinate is not the same as the one used in the pdf so we have to make som strange translations
float headingDiff = PathLengthMath.WrapAngleInRadians((2f * Mathf.PI) - (carEnd.HeadingInRad - carStart.HeadingInRad));
//Rotate the vector between the cars
//Add 90 degrees because of unitys coordinate system
Vector3 newEndPos = Quaternion.Euler(0f, -carStart.HeadingInDegrees + 90f, 0f) * posDiff;
RSCar carEndMod = new RSCar(newEndPos, headingDiff);
return(carEndMod);
}
//8.4: C|CC
public static float Lf_Rb_Lb(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//Uses a modified formula adapted from the c_cc function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
float xi = x - Mathf.Sin(phi);
float eta = y - 1f + Mathf.Cos(phi);
float u1, theta;
R(xi, eta, out u1, out theta);
if (u1 > 4f)
{
return(float.PositiveInfinity);
}
float alpha = Mathf.Acos(u1 / 4f);
t = M(HALF_PI + alpha + theta);
u = M(PI - 2f * alpha);
//This part is the only thing thats different from 8.3
v = M(t + u - phi);
//Check all 3 curves
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(u) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + v;
return(totalLength);
}
/// <summary>
/// Generate the shortest Reeds-Shepp path
/// </summary>
/// <param name="startPos">The position of the car where the path starts</param>
/// <param name="startHeading">The heading of the car where the path starts [rad]</param>
/// <param name="endPos">The position of the car where the path ends</param>
/// <param name="endHeading">The heading of the car where the path ends [rad]</param>
/// <param name="turningRadius">The truning radius of the car [m]</param>
/// <param name="wpDistance">The distance between the waypoints that make up the final path</param>
/// <param name="generateOneWp">Should we generate just 1 waypoint and not all</param>
/// <returns></returns>
public static List <RSCar> GetShortestPath(Vector3 startPos, float startHeading, Vector3 endPos, float endHeading, float turningRadius, float wpDistance, bool generateOneWp)
{
RSCar carStart = new RSCar(startPos, startHeading);
RSCar carEnd = new RSCar(endPos, endHeading);
//The formulas assume we move from(0, 0, 0) to(x, y, theta), and that the turning radius is 1
//This means we have to move and rotate the goal's position and heading as if the start's position and heading had been at (0,0,0)
RSCar carEndMod = NormalizeGoalCar(carStart, carEnd, turningRadius);
//int startTime = Environment.TickCount;
List <RSCar> shortestPath = FindShortestPath(carStart, carEnd, carEndMod, wpDistance, turningRadius, generateOneWp);
//float timeInSeconds = (float)(Environment.TickCount - startTime) / 1000f;
//Debug.Log("Ticks to generate <b>Reeds-Shepp:</b> " + (Environment.TickCount - startTime));
return(shortestPath);
}
//8.10 (reversed): CSC(pi/2)|C, different turn
public static float Lf_Sf_Lfpi2_Rb(RSCar goal, out PathSegmentLengths path)
{
float t = 0f; float u = 0f; float v = 0f; path = new PathSegmentLengths(0f, 0f, 0f);
float x = goal.pos.x;
float y = goal.pos.z;
float phi = goal.HeadingInRad;
//Calculations
//Uses a modified formula adapted from the csc2_cb function from http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c
float xi = x + Mathf.Sin(phi);
float eta = y - 1f - Mathf.Cos(phi);
float u1, theta;
R(xi, eta, out u1, out theta);
if (u1 < 2f)
{
return(float.PositiveInfinity);
}
//This is the part thats different from the original report:
t = M(theta);
u = u1 - 2f;
v = M(-t - HALF_PI + phi);
//Check all 2 curves
if (IsCurveSegmentInvalid(t) || IsCurveSegmentInvalid(v))
{
return(float.PositiveInfinity);
}
path.t = t; path.u = u; path.v = v;
float totalLength = t + u + HALF_PI + v;
return(totalLength);
}
//Add waypoints to a given path
private static List <RSCar> AddWaypoints(
PathWords word, PathSegmentLengths pathSegmentLengths, RSCar carStart, RSCar carEnd, float wpDistance, float turningRadius, bool generateOneWp)
{
//Find the car settings we need to drive through the path
List <SegmentSettings> pathSettings = PathSettings.GetSettings(word, pathSegmentLengths);
if (pathSettings == null)
{
Debug.Log("Cant find settings for a path");
return(null);
}
//Generate the waypoints
//Data used when generating the path
//The pos and heading we will move along the path
Vector3 pos = carStart.pos;
float heading = carStart.HeadingInRad;
//The distance between each step we take when generating the path, the smaller the better, but is also slower
float stepDistance = 0.05f;
//To generate waypoints with a certain distance between them we need to know how far we have driven since the last wp
float driveDistance = 0f;
//The waypoints
List <RSCar> waypoints = new List <RSCar>();
//Add the first wp
waypoints.Add(new RSCar(pos, heading, pathSettings[0].gear, pathSettings[0].steering));
//Loop through all path 3-5 path segments
for (int i = 0; i < pathSettings.Count; i++)
{
SegmentSettings segmentSettings = pathSettings[i];
//How many steps will we take to generate this segment
//Will always be at least 2 no matter the stepDistance
int n = (int)Math.Ceiling((segmentSettings.length * turningRadius) / stepDistance);
//How far will we move each step?
float stepLength = (segmentSettings.length * turningRadius) / n;
//Change stuff depending on in which direction we are moving
float steeringWheelPos = 1f;
if (segmentSettings.steering == RSCar.Steering.Left)
{
steeringWheelPos = -1f;
}
//Invert steering if we are reversing
if (segmentSettings.gear == RSCar.Gear.Back)
{
steeringWheelPos *= -1f;
}
//Drive through this segment in small steps
for (int j = 0; j < n; j++)
{
//Update position
float dx = stepLength * Mathf.Sin(heading);
float dz = stepLength * Mathf.Cos(heading);
if (segmentSettings.gear == RSCar.Gear.Back)
{
dx = -dx;
dz = -dz;
}
pos = new Vector3(pos.x + dx, pos.y, pos.z + dz);
//Update heading if we are turning
if (segmentSettings.steering != RSCar.Steering.Straight)
{
heading = heading + (stepLength / turningRadius) * steeringWheelPos;
}
//Should we generate a new wp?
driveDistance += stepLength;
if (driveDistance > wpDistance)
{
waypoints.Add(new RSCar(pos, heading, segmentSettings.gear, segmentSettings.steering));
driveDistance = driveDistance - wpDistance;
if (generateOneWp)
{
return(waypoints);
}
}
}
//We also need to add the last pos of this segment as waypoint or the path will not be the same
//if we for example are ignoring the waypoint where we change direction
waypoints.Add(new RSCar(pos, heading, segmentSettings.gear, segmentSettings.steering));
}
//Move the last wp pos to the position of the goal car
//When we generate waypoints, the accuracy depends on the stepDistance, so is not always hitting the goal exactly
waypoints[waypoints.Count - 1].pos = carEnd.pos;
return(waypoints);
}
//Could maybe optimize sin(phi) and cos(phi) because they are always the same
public static float GetLength(RSCar car, PathWords word, out PathSegmentLengths pathLengths)
{
switch (word)
{
//8.1: CSC, same turn
case PathWords.Lf_Sf_Lf: return(Lf_Sf_Lf(car, out pathLengths));
case PathWords.Lb_Sb_Lb: return(Lf_Sf_Lf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Rf: return(Lf_Sf_Lf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Rb: return(Lf_Sf_Lf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.2: CSC, different turn
case PathWords.Lf_Sf_Rf: return(Lf_Sf_Rf(car, out pathLengths));
case PathWords.Lb_Sb_Rb: return(Lf_Sf_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Lf: return(Lf_Sf_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Lb: return(Lf_Sf_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.3: C|C|C
case PathWords.Lf_Rb_Lf: return(Lf_Rb_Lf(car, out pathLengths));
case PathWords.Lb_Rf_Lb: return(Lf_Rb_Lf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lb_Rf: return(Lf_Rb_Lf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lf_Rb: return(Lf_Rb_Lf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.4: C|CC
case PathWords.Lf_Rb_Lb: return(Lf_Rb_Lb(car, out pathLengths));
case PathWords.Lb_Rf_Lf: return(Lf_Rb_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lb_Rb: return(Lf_Rb_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lf_Rf: return(Lf_Rb_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.4: CC|C
case PathWords.Lf_Rf_Lb: return(Lf_Rf_Lb(car, out pathLengths));
case PathWords.Lb_Rb_Lf: return(Lf_Rf_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lf_Rb: return(Lf_Rf_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lb_Rf: return(Lf_Rf_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.7: CCu|CuC
case PathWords.Lf_Ruf_Lub_Rb: return(Lf_Ruf_Lub_Rb(car, out pathLengths));
case PathWords.Lb_Rub_Luf_Rf: return(Lf_Ruf_Lub_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Luf_Rub_Lb: return(Lf_Ruf_Lub_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lub_Ruf_Lf: return(Lf_Ruf_Lub_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.8: C|CuCu|C
case PathWords.Lf_Rub_Lub_Rf: return(Lf_Rub_Lub_Rf(car, out pathLengths));
case PathWords.Lb_Ruf_Luf_Rb: return(Lf_Rub_Lub_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lub_Rub_Lf: return(Lf_Rub_Lub_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Luf_Ruf_Lb: return(Lf_Rub_Lub_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.9: C|C(pi/2)SC, same turn
case PathWords.Lf_Rbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Lb(car, out pathLengths));
case PathWords.Lb_Rfpi2_Sf_Lf: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lfpi2_Sf_Rf: return(Lf_Rbpi2_Sb_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.10: C|C(pi/2)SC, different turn
case PathWords.Lf_Rbpi2_Sb_Rb: return(Lf_Rbpi2_Sb_Rb(car, out pathLengths));
case PathWords.Lb_Rfpi2_Sf_Rf: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lbpi2_Sb_Lb: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lfpi2_Sf_Lf: return(Lf_Rbpi2_Sb_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.9(reversed): CSC(pi / 2) | C, same turn
case PathWords.Lf_Sf_Rfpi2_Lb: return(Lf_Sf_Rfpi2_Lb(car, out pathLengths));
case PathWords.Lb_Sb_Rbpi2_Lf: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Lfpi2_Rb: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Lbpi2_Rf: return(Lf_Sf_Rfpi2_Lb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.10 (reversed): CSC(pi/2)|C, different turn
case PathWords.Lf_Sf_Lfpi2_Rb: return(Lf_Sf_Lfpi2_Rb(car, out pathLengths));
case PathWords.Lb_Sb_Lbpi2_Rf: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Sf_Rfpi2_Lb: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Sb_Rbpi2_Lf: return(Lf_Sf_Lfpi2_Rb(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
//8.11: C | C(pi / 2)SC(pi / 2) | C
case PathWords.Lf_Rbpi2_Sb_Lbpi2_Rf: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car, out pathLengths));
case PathWords.Lb_Rfpi2_Sf_Lfpi2_Rb: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(-car.X, car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rf_Lbpi2_Sb_Rbpi2_Lf: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(car.X, -car.Z, -car.HeadingInRad), out pathLengths));
case PathWords.Rb_Lfpi2_Sf_Rfpi2_Lb: return(Lf_Rbpi2_Sb_Lbpi2_Rf(car.ChangeData(-car.X, -car.Z, car.HeadingInRad), out pathLengths));
}
pathLengths = new PathSegmentLengths(0f, 0f, 0f);
return(float.PositiveInfinity);
}
//Change car data
public RSCar ChangeData(float newXPos, float newZPos, float newHeading)
{
RSCar carCopy = new RSCar(new Vector3(newXPos, pos.y, newZPos), newHeading);
return(carCopy);
}