private void SmoothThisPath(Map map)
{
//Generate the Reeds-Shepp path with some turning radius
//Get the shortest path
Vector3 startPos = startTrans.position;
Vector3 endPos = endTrans.position;
float startRot = startTrans.rotation.eulerAngles.y * Mathf.Deg2Rad;
float endRot = endTrans.rotation.eulerAngles.y * Mathf.Deg2Rad;
//Make sure the start and end are within the map
if (!(map.IsPosWithinGrid(startPos) && map.IsPosWithinGrid(endPos)))
{
return;
}
//Get the shortest path
List <RSCar> shortestPath = ReedsShepp.GetShortestPath(startPos, startRot, endPos, endRot, 12f, 1f, generateOneWp: false);
//Make it unsmooth to easier see the difference
Random.InitState(0);
for (int i = 1; i < shortestPath.Count - 1; i++)
{
Vector3 p = shortestPath[i].pos;
float dist = 0.4f;
p.x += Random.Range(-dist, dist);
p.z += Random.Range(-dist, dist);
shortestPath[i].pos = p;
}
//To Node data formart
List <Node> nodes = new List <Node>();
for (int i = 1; i < shortestPath.Count - 1; i++)
{
Node previousNode = null;
Vector3 pos = shortestPath[i].pos;
float heading = shortestPath[i].HeadingInRad;
bool isReversing = shortestPath[i].gear == RSCar.Gear.Back ? true : false;
Node node = new Node(previousNode, pos, heading, isReversing);
nodes.Add(node);
}
//Smooth the path and push it away from obstacles
List <Node> smoothPath = ModifyPath.SmoothPath(nodes, map, false, isDebugOn: true);
//Display
DisplayPath(nodes, Color.white);
DisplayPathNodes(nodes, Color.black);
DisplayPath(smoothPath, Color.blue);
//DisplayPathNodes(smoothPath, Color.black);
}
//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);
}
