//Generate a path and send it to the car
//We have to do it over some time to avoid a sudden stop in the simulation
IEnumerator GeneratePath(Car goalCar)
{
//Get the start positions
//The self-driving car
Car startCar = new Car(SimController.current.GetSelfDrivingCarTrans(), SimController.current.GetActiveCarData());
//The trailer (if any)
Car startTrailer = null;
Transform trailerTrans = SimController.current.TryGetTrailerTrans();
if (trailerTrans != null)
{
startTrailer = new Car(trailerTrans, SimController.current.TryGetTrailerData());
}
//First we have to check if the self-driving car is inside of the grid
if (!map.IsPosWithinGrid(startCar.rearWheelPos))
{
Debug.Log("The car is outside of the grid");
yield break;
}
//Which cell do we want to reach? We have already checked that this cell is valid
IntVector2 targetCell = map.ConvertWorldToCell(goalCar.rearWheelPos);
//To measure time, is measured in tick counts
int startTime = 0;
//To display how long time each part took
string timeText = "";
//
// Calculate Heuristics
//
//Calculate euclidean distance heuristic
startTime = Environment.TickCount;
HeuristicsController.EuclideanDistance(map, targetCell);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Euclidean Distance");
yield return(new WaitForSeconds(0.05f));
//Calculate dynamic programing = flow field
startTime = Environment.TickCount;
HeuristicsController.DynamicProgramming(map, targetCell);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Dynamic Programming");
yield return(new WaitForSeconds(0.05f));
//Calculate the final heuristics
HeuristicsController.GenerateFinalHeuristics(map);
//
// Generate the shortest path with Hybrid A*
//
//List with all expanded nodes for debugging, so we can display the search tree
List <Node> expandedNodes = new List <Node>();
startTime = Environment.TickCount;
//The output is finalPath and expandedNodes
List <Node> finalPath = HybridAStar.GeneratePath(startCar, goalCar, map, expandedNodes, startTrailer);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Hybrid A Star");
if (finalPath == null || finalPath.Count == 0)
{
UIController.current.SetFoundPathText("Failed to find a path!");
}
else
{
UIController.current.SetFoundPathText("Found a path!");
}
//
// Smooth the path and send it to the car
//
//If we have found a path
List <Node> smoothPath = null;
if (finalPath != null && finalPath.Count > 0)
{
//Modify the path to make it easier for the vehicle to follow it
//Step 1. Hybrid A* is using the rear wheel axle to generate the path, but it's easier for the car to follow it
//if we also know where the path should have been if we had used the front axle
Vector3 vehicleStartDir = SimController.current.GetSelfDrivingCarTrans().forward;
Vector3 vehicleEndDir = SimController.current.GetCarShowingEndPosTrans().forward;
ModifyPath.CalculateFrontAxlePositions(finalPath, startCar.carData, vehicleStartDir, vehicleEndDir, isMirrored: false);
//When reversing we should track a path which is a path that goes along the front axle
//but the front axle is mirrored along the rear axle
ModifyPath.CalculateFrontAxlePositions(finalPath, startCar.carData, vehicleStartDir, vehicleEndDir, isMirrored: true);
//Smooth the path by making it smoother and adding waypoints to make it easier for the car to follow the path
startTime = Environment.TickCount;
smoothPath = ModifyPath.SmoothPath(finalPath, map, isCircular: false, isDebugOn: true);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Smooth path");
//The car will immediatelly start following the path
SimController.current.SendPathToActiveCar(smoothPath, isCircular: false);
}
//
// Display the results
//
//Display how long time the different parts took
Debug.Log(timeText);
//Reset display
displayController.ResetGUI();
//Always display the search tree even if we havent found a path to the goal
displayController.DisplaySearchTree(expandedNodes);
//Generate the flow field heuristic texture
displayController.GenerateTexture(map, DisplayController.TextureTypes.Flowfield_Target);
//Display the different paths
displayController.DisplayFinalPath(finalPath, smoothPath);
yield return(null);
}