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