void Start()
{
//Create the map with cell data we need
map = new Map(Parameters.mapWidth, Parameters.cellWidth);
//Generate obstacles
//Has to do it from this script or the obstacles might be created after this script has finished and mess things up
//Need the start car so we can avoid adding obstacles on it
//Car startCar = new Car(SimController.current.GetSelfDrivingCarTrans(), SimController.current.GetActiveCarData());
Vector3 startPos = SimController.current.GetSelfDrivingCarTrans().position;
int startTime = Environment.TickCount;
this.GetComponent <ObstaclesGenerator>().InitObstacles(map, startPos);
string timeText = DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Generate obstacles and Voronoi diagram");
Debug.Log(timeText);
//Create the textures showing various stuff, such as the distance to nearest obstacle
//debugController.DisplayCellObstacleIntersection(map);
displayController.GenerateTexture(map, DisplayController.TextureTypes.Flowfield_Obstacle);
displayController.GenerateTexture(map, DisplayController.TextureTypes.Voronoi_Field);
displayController.GenerateTexture(map, DisplayController.TextureTypes.Voronoi_Diagram);
}
//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);
}
//
// Generate a path with Hybrid A*
//
public static List <Node> GeneratePath(Car startCar, Car endCar, Map map, List <Node> allExpandedNodes, Car startTrailer)
{
//Reset timers
timer_selectLowestCostNode = 0;
timer_addNodeToHeap = 0;
timer_findChildren = 0;
timer_isCollidingWithObstacle = 0;
timer_ReedsSheppNode = 0;
timer_ReedsSheppHeuristics = 0;
timer_TrailerCollision = 0;
//Other data we want to track
//How many nodes did we prune?
int prunedNodes = 0;
//To track max number of nodes in the heap
int maxNodesInHeap = 0;
//Init the data structure we need
int mapWidth = map.MapWidth;
//Is faster to cache this than using map.cellData
Cell[,] cellData = map.cellData;
//Open nodes - the parameter is how many items can fit in the heap
//If we lower the heap size it will still find a path, which is more drunk
Heap <Node> openNodes = new Heap <Node>(200000);
//int in the dictionaries below is the rounded heading used to enter a cell
HashSet <int>[,] closedCells = new HashSet <int> [mapWidth, mapWidth];
//The node in the cell with the lowest g-cost at a certain angle
Dictionary <int, Node>[,] lowestCostNodes = new Dictionary <int, Node> [mapWidth, mapWidth];
//Trailer
//int in the dictionaries below is the rounded heading used to enter a cell
HashSet <int>[,] closedCellsTrailer = new HashSet <int> [mapWidth, mapWidth];
HashSet <int>[,] lowestCostNodesTrailer = new HashSet <int> [mapWidth, mapWidth];
for (int x = 0; x < mapWidth; x++)
{
for (int z = 0; z < mapWidth; z++)
{
closedCells[x, z] = new HashSet <int>();
lowestCostNodes[x, z] = new Dictionary <int, Node>();
//Trailer
closedCellsTrailer[x, z] = new HashSet <int>();
lowestCostNodesTrailer[x, z] = new HashSet <int>();
}
}
//Create the first node
//Why rear wheel? Because we need that position when simulating the "skeleton" car
//and then it's easier if everything is done from the rear wheel positions
IntVector2 startCellPos = map.ConvertWorldToCell(startCar.rearWheelPos);
Node node = new Node(
previousNode: null,
rearWheelPos: startCar.rearWheelPos,
heading: startCar.HeadingInRadians,
isReversing: false);
if (startTrailer != null)
{
node.TrailerHeadingInRadians = startTrailer.HeadingInRadians;
}
node.AddCosts(gCost: 0f, hCost: cellData[startCellPos.x, startCellPos.z].heuristics);
openNodes.Add(node);
//The end of the path, which we will return
Node finalNode = null;
//bools so we can break out of the main loop
//Set when search is complete
bool found = false;
//Set if we can't find a node to expand
bool resign = false;
//To break out of the loop if it takes too long time
int iterations = 0;
IntVector2 goalCellPos = map.ConvertWorldToCell(endCar.rearWheelPos);
while (!found && !resign)
{
if (iterations > 400000)
{
Debug.Log("Stuck in infinite loop");
break;
}
iterations += 1;
//If we don't have any nodes to expand
if (openNodes.Count == 0)
{
resign = true;
Debug.Log("Failed to find a path");
}
//We have nodes to expand
else
{
if (openNodes.Count > maxNodesInHeap)
{
maxNodesInHeap = openNodes.Count;
}
//Get the node with the lowest f cost
int timeBefore = Environment.TickCount;
Node nextNode = openNodes.RemoveFirst();
timer_selectLowestCostNode += Environment.TickCount - timeBefore;
//Close this cell
IntVector2 cellPos = map.ConvertWorldToCell(nextNode.rearWheelPos);
int roundedHeading = HelpStuff.RoundValue(nextNode.HeadingInDegrees, headingResolution);
HashSet <int> closedHeadingsInThisCell = closedCells[cellPos.x, cellPos.z];
bool haveAlreadyClosedCell = false;
//Close the cell, but we can still drive into this cell from another angle
if (!closedHeadingsInThisCell.Contains(roundedHeading))
{
closedHeadingsInThisCell.Add(roundedHeading);
}
else if (startTrailer == null)
{
haveAlreadyClosedCell = true;
}
if (startTrailer != null)
{
int roundedHeadingTrailer = HelpStuff.RoundValue(nextNode.TrailerHeadingInDegrees, headingResolutionTrailer);
HashSet <int> closedTrailerHeadingsInThisCell = closedCellsTrailer[cellPos.x, cellPos.z];
if (!closedTrailerHeadingsInThisCell.Contains(roundedHeadingTrailer))
{
closedTrailerHeadingsInThisCell.Add(roundedHeadingTrailer);
}
else
{
haveAlreadyClosedCell = true;
}
}
//We have already expanded a better node with the same heading so dont expand this node
if (haveAlreadyClosedCell)
{
iterations -= 1;
continue;
}
//For debugging
allExpandedNodes.Add(nextNode);
//Check if the vehicle has reached the target
float distanceSqrToGoal = (nextNode.rearWheelPos - endCar.rearWheelPos).sqrMagnitude;
//But we also need to make sure the vehiclke has correct heading
float headingDifference = Mathf.Abs(endCar.HeadingInDegrees - nextNode.HeadingInDegrees);
//If we end up in the same cell or is within a certain distance from the goal
if ((distanceSqrToGoal < posAccuracy * posAccuracy || (cellPos.x == goalCellPos.x && cellPos.z == goalCellPos.z)) &&
headingDifference < headingAccuracy)
{
found = true;
Debug.Log("Found a path");
finalNode = nextNode;
//Make sure the end node has the same position as the target
finalNode.rearWheelPos.x = endCar.rearWheelPos.x;
finalNode.rearWheelPos.z = endCar.rearWheelPos.z;
}
//If we havent found the goal, then expand this node
else
{
//Get all child nodes
timeBefore = Environment.TickCount;
List <Node> children = GetChildrenToNode(nextNode, map, cellData, startCar.carData, endCar, startTrailer);
timer_findChildren += Environment.TickCount - timeBefore;
//Should we add any of the child nodes to the open nodes?
foreach (Node child in children)
{
IntVector2 childCell = map.ConvertWorldToCell(child.rearWheelPos);
int roundedChildHeading = HelpStuff.RoundValue(child.HeadingInDegrees, headingResolution);
//Has this cell been closed with this heading?
//If so, it means we already have a path at this cell with this heading,
//and the existing node is cheaper because we have already expanded it
if (closedCells[childCell.x, childCell.z].Contains(roundedChildHeading) && startTrailer == null)
{
prunedNodes += 1;
continue;
}
//If we have a trailer
else if (closedCells[childCell.x, childCell.z].Contains(roundedChildHeading) && startTrailer != null)
{
int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);
if (closedCellsTrailer[childCell.x, childCell.z].Contains(roundedTrailerHeading))
{
prunedNodes += 1;
continue;
}
}
//Have we already expanded a node with lower cost in this cell at this heading?
float costSoFar = child.gCost;
//The dictionary with lowest cost nodes in this cell
Dictionary <int, Node> nodesWithLowestCost = lowestCostNodes[childCell.x, childCell.z];
//Have we expanded with this angle to the cell before?
if (nodesWithLowestCost.ContainsKey(roundedChildHeading) && startTrailer == null)
{
//If the open node has a large gCost then we need to update that node with data
//from the child node
if (costSoFar < nodesWithLowestCost[roundedChildHeading].gCost)
{
//If this child node is better then we should update the node in the open list
//which is faster than deleting the old node and adding this child node
Node existingNode = nodesWithLowestCost[roundedChildHeading];
child.StealDataFromThisNode(existingNode);
//Modify the heap-position of the node already in the open nodes
openNodes.UpdateItem(existingNode);
}
//If the open node has a smaller gCost, then we dont need this child node, so do nothing
prunedNodes += 1;
continue;
}
//We have a trailer
else if (nodesWithLowestCost.ContainsKey(roundedChildHeading) && startTrailer != null)
{
//Have we expanded to this node before with this trailer heading
int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);
if (lowestCostNodesTrailer[childCell.x, childCell.z].Contains(roundedTrailerHeading))
{
//If the open node has a large gCost then we need to update that node with data
//from the child node
if (costSoFar < nodesWithLowestCost[roundedChildHeading].gCost)
{
//If this child node is better then we should update the node in the open list
//which is faster than deleting the old node and adding this child node
Node existingNode = nodesWithLowestCost[roundedChildHeading];
child.StealDataFromThisNode(existingNode);
//Modify the heap-position of the node already in the open nodes
openNodes.UpdateItem(existingNode);
}
//If the open node has a smaller gCost, then we dont need this child node, so do nothing
prunedNodes += 1;
continue;
}
}
else
{
//Add the node to the cell with this angle
nodesWithLowestCost[roundedChildHeading] = child;
if (startTrailer != null)
{
int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);
lowestCostNodesTrailer[childCell.x, childCell.z].Add(roundedTrailerHeading);
}
}
//Dont add the node if its colliding with an obstacle or is outside of map
timeBefore = Environment.TickCount;
if (ObstaclesDetection.HasCarInvalidPosition(child.rearWheelPos, child.heading, startCar.carData, map))
{
prunedNodes += 1;
continue;
}
timer_isCollidingWithObstacle += Environment.TickCount - timeBefore;
//Trailer obstacle calculations
int startTrailerTimer = Environment.TickCount;
if (startTrailer != null)
{
//Now we need to check if this new position is valid by checking for collision with obstacles and the drag vehicle
//To do that we need the rear wheel pos of the trailer with this heading
//We know where the trailer is attached to the drag vehicle
Vector3 trailerAttachmentPoint = startCar.carData.GetTrailerAttachmentPoint(child.rearWheelPos, child.HeadingInRadians);
//Now we need the trailer's rear-wheel pos based on the new heading
Vector3 trailerRearWheelPos = startTrailer.carData.GetTrailerRearWheelPos(trailerAttachmentPoint, child.TrailerHeadingInRadians);
//Obstacle detection
//With the environment
if (ObstaclesDetection.HasCarInvalidPosition(trailerRearWheelPos, child.TrailerHeadingInRadians, startTrailer.carData, map))
{
prunedNodes += 1;
//Debug.Log("Semi trailer environment collision");
continue;
}
//With the drag vehicle
if (ObstaclesDetection.IsTrailerCollidingWithDragVehicle(
child.rearWheelPos, child.HeadingInRadians, startCar.carData,
trailerRearWheelPos, child.TrailerHeadingInRadians, startTrailer.carData))
{
prunedNodes += 1;
//Debug.Log("Semi trailer collision");
continue;
}
}
timer_TrailerCollision += Environment.TickCount - startTrailerTimer;
timeBefore = Environment.TickCount;
openNodes.Add(child);
timer_addNodeToHeap += Environment.TickCount - timeBefore;
}
}
}
}
//Generate the final path when Hybrid A* has found the goal
List <Node> finalPath = GenerateFinalPath(finalNode);
//Display how long time everything took
string display = DisplayController.GetDisplayTimeText(timer_selectLowestCostNode, "Select lowest cost node");
display += DisplayController.GetDisplayTimeText(timer_addNodeToHeap, "Add new node to heap");
display += DisplayController.GetDisplayTimeText(timer_findChildren, "Find children");
display += DisplayController.GetDisplayTimeText(timer_isCollidingWithObstacle, "Is node colliding");
display += DisplayController.GetDisplayTimeText(timer_ReedsSheppNode, "Reeds-Shepp Node");
display += DisplayController.GetDisplayTimeText(timer_ReedsSheppHeuristics, "Reeds-Shepp Heuristics");
display += DisplayController.GetDisplayTimeText(timer_TrailerCollision, "Trailer collision");
display += DisplayController.GetDisplayText("Max nodes in heap", maxNodesInHeap, ". ");
display += DisplayController.GetDisplayText("Expanded nodes", allExpandedNodes.Count, ". ");
display += DisplayController.GetDisplayText("Pruned nodes", prunedNodes, null);
Debug.Log(display);
//Display car positions along the final path
DisplayController.DisplayVehicleAlongPath(finalPath, startCar.carData, startTrailer);
return(finalPath);
}