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