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