//Run the main loop
private void RunHybridAStar(List <Node> allExpandedNodes, CarData targetCarData)
{
//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
Vector3 startPos = carData.GetRearWheelPos();
IntVector2 startCellPos = PathfindingController.ConvertCoordinateToCellPos(startPos);
lowestCostForward[startCellPos.x, startCellPos.z] = 0f;
lowestCostReverse[startCellPos.x, startCellPos.z] = 0f;
//Create a new node
Node node = new Node();
//Add the initial car data to the start node
node.g = 0f;
node.h = HeuristicsController.heuristics[startCellPos.x, startCellPos.z];
node.cellPos = startCellPos;
node.carPos = startPos;
node.heading = carData.GetHeading() * Mathf.Deg2Rad;
node.steeringAngle = 0f;
node.isReversing = false;
openNodes.Add(node);
//Init the bad node
this.badNode = node;
//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 identify the best of the bad nodes
//bestDistance = Mathf.Infinity;
//To break out of the loop if it takes too long time
int iterations = 0;
while (!found && !resign)
{
if (iterations > 100000)
{
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
{
//Get the node with the lowest cost
Node nextNode = openNodes.RemoveFirst();
//Save it in case we can find an entire path if it has a lower cost
//Use heuristics to determine if this node is close to the goal than a previous node
if (nextNode.h < badNode.h)
{
this.badNode = nextNode;
}
//Close this cell
IntVector2 cellPos = nextNode.cellPos;
if (nextNode.isReversing)
{
closedCellsReverse[cellPos.x, cellPos.z] = true;
}
else
{
closedCellsForward[cellPos.x, cellPos.z] = true;
}
//Check if this is a goal node
//Use an accuracy of 1 m because we will not hit the exact target coordinate
float distanceSqrToGoal = (nextNode.carPos - targetCarData.GetRearWheelPos()).sqrMagnitude;
//But we also need to make sure the car has correct heading
float headingDifference = Mathf.Abs(targetCarData.GetHeading() - nextNode.heading * Mathf.Rad2Deg);
if (distanceSqrToGoal < 1f && headingDifference < 20f)
{
found = true;
Debug.Log("Found a path");
finalNode = nextNode;
//Make sure the end node has the same position as the target
finalNode.carPos.x = targetCarData.GetRearWheelPos().x;
finalNode.carPos.z = targetCarData.GetRearWheelPos().z;
}
//If we havent found the goal, then expand this node
else
{
float distSqr = (nextNode.carPos - targetCarData.GetRearWheelPos()).sqrMagnitude;
//Test if we can find the goal with a fixed path algorithm such as Dubins or Reeds-Shepp
List <Node> fixedPath = null;
//Don't try to find a fixed path each expansion, but try to find more fixed paths the close to the goal we are
if (
(allExpandedNodes.Count % 300 == 0) ||
(allExpandedNodes.Count % 20 == 0 && distSqr < 40f * 40f) ||
(distSqr < 20f * 20f))
{
fixedPath = GetShortestReedsSheppPath(nextNode, targetCarData.GetCarTransform(), carData);
}
//If a fixed path is possible
if (fixedPath != null)
{
//Stop looping - real Hybrid A* continues looping and just add this node as a node in the tree
found = true;
Debug.Log("Found a path with a fixed path algorithm");
//Generate nodes along this path until we reach the goal
Node previousNode = nextNode;
//Don't need the first coordinate because it is the same as the position from the tree (nextNode)
for (int i = 1; i < fixedPath.Count; i++)
{
fixedPath[i].previousNode = previousNode;
previousNode = fixedPath[i];
}
finalNode = previousNode;
//Make sure the end node has the same position as the target
finalNode.carPos.x = targetCarData.GetRearWheelPos().x;
finalNode.carPos.z = targetCarData.GetRearWheelPos().z;
}
else
{
ExpandNode(nextNode);
//For debugging
allExpandedNodes.Add(nextNode);
}
}
}
}
}
//Change the car's speed to slow down when curvy path or close to end of path
void ChangeSpeed()
{
//Slow down if we reach an endpoint or a if we are about to change driving direction
bool isGoingTooFast = false;
//The car's current speed
float carSpeed = carScript.GetCarSpeed();
//Change 1
//If we are driving forward, then change speed depending on angle of the path we are following
if (!wayPoints[currentWayPointIndex].isReversing)
{
//Change speed depending on the angle to the next waypoint
int lookAheadIndex = currentWayPointIndex + 4;
//As we get closer to the end we cant look ahead too many waypoints
lookAheadIndex = Mathf.Clamp(lookAheadIndex, 2, wayPoints.Count - 1);
Vector3 P2 = GetWayPointPos(lookAheadIndex);
Vector3 P1 = GetWayPointPos(currentWayPointIndex);
Vector3 carPos = carScript.transform.position;
//Angle is 0 degrees if P2 is infront of P1 in the direction of the car
float angle = Vector3.Angle(P1 - carPos, P2 - carPos);
//print(angle);
if (angle > 10f && carSpeed > 15f)
{
isGoingTooFast = true;
}
}
//Change 2
//Slow down the car as it's getting close to a change, like forward -> reverse
bool closeToChange = false;
bool currentDrivingDirection = wayPoints[currentWayPointIndex].isReversing;
bool isCloseToEnd = false;
//The number of nodes until change
int counter = 0;
//The max number of nodes until we care about a change
int maxNodesUntilChange = 8;
for (int i = currentWayPointIndex + 1; i < wayPoints.Count - 0; i++)
{
if (counter > maxNodesUntilChange)
{
break;
}
if (wayPoints[i].isReversing != currentDrivingDirection)
{
closeToChange = true;
//print("Close to change in driving direction");
break;
}
//Last waypoint
if (i == wayPoints.Count - 1)
{
closeToChange = true;
isCloseToEnd = true;
//print("Close to end of path");
}
counter += 1;
}
//print(counter);
//Now we need to slow down to something close to 0
if (closeToChange)
{
//The easiest way is to use an animation curve
float percentage = (float)counter / (float)maxNodesUntilChange;
//Has to be different or the car will drive very slowly when changing direction
float minCarSpeed = 5f;
if (isCloseToEnd)
{
minCarSpeed = 1f;
}
float maxCarSpeed = minCarSpeed + (slowDownCurve.Evaluate(percentage) * 20f);
//print(maxCarSpeed);
if (carSpeed > maxCarSpeed)
{
isGoingTooFast = true;
}
}
//Change 3
//Slow down the car if it is deviting too much from the path and heading
float CTE = Mathf.Abs(CalculateCTE());
float wantedHeading = wayPoints[currentWayPointIndex].heading * Mathf.Rad2Deg;
//This is already in degrees
float currentHeading = carData.GetHeading();
float headingDiff = Mathf.Abs(wantedHeading - currentHeading);
if (CTE > 0.2f && headingDiff > 5f)
{
isGoingTooFast = true;
}
//Stop the car if its going too fast
if (isGoingTooFast && carScript.GetCarSpeed() > 5f)
{
carScript.StopCar();
//print("Stop car");
}
else
{
//print("Drive car");
}
}
//Run the main loop
private void RunHybridAStar(CarData targetCarData, List <Node> allExpandedNodes)
{
//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
Vector3 startPos = carData.GetRearWheelPos();
IntVector2 startCellPos = PathfindingController.ConvertCoordinateToCellPos(startPos);
//Create a new node
Node node = new Node();
//Add the initial car data to the start node
node.g = 0f;
node.h = HeuristicsController.heuristics[startCellPos.x, startCellPos.z];
node.cellPos = startCellPos;
node.carPos = startPos;
node.heading = carData.GetHeading() * Mathf.Deg2Rad;
node.steeringAngle = 0f;
node.isReversing = false;
openNodes.Add(node);
//Init the bad node
this.badNode = node;
//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 identify the best of the bad nodes
//bestDistance = Mathf.Infinity;
//To break out of the loop if it takes too long time
int iterations = 0;
while (!found && !resign)
{
if (iterations > 100000)
{
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
{
//Get the node with the lowest cost
Node nextNode = openNodes.RemoveFirst();
//Save it in case we can find an entire path if it has a lower cost
//Use heuristics to determine if this node is vlose to the goal than a previous node
if (nextNode.h < badNode.h)
{
this.badNode = nextNode;
}
//Close this cell
IntVector2 cellPos = nextNode.cellPos;
int roundedAngle = RoundAngle(nextNode.heading * Mathf.Rad2Deg);
Dictionary <int, bool> currentAngles = closedCells[cellPos.x, cellPos.z];
//Close the cell with this angle
if (!currentAngles.ContainsKey(roundedAngle))
{
currentAngles.Add(roundedAngle, true);
}
else
{
//This is not costly so it souldnt be counted as an iteration
//Is needed because we are not removing nodes with higher cost but the same angle from the heap
iterations -= 1;
continue;
}
//Check if this is a goal node
//Use an accuracy of 1 m because we will not hit the exact target coordinate
float distanceSqrToGoal = (nextNode.carPos - targetCarData.GetRearWheelPos()).sqrMagnitude;
//But we also need to make sure the car has correct heading
float headingDifference = Mathf.Abs(targetCarData.GetHeading() - nextNode.heading * Mathf.Rad2Deg);
if (distanceSqrToGoal < 1f && headingDifference < 20f)
{
found = true;
Debug.Log("Found a path");
finalNode = nextNode;
//Make sure the end node has the same position as the target
finalNode.carPos.x = targetCarData.GetRearWheelPos().x;
finalNode.carPos.z = targetCarData.GetRearWheelPos().z;
}
//If we havent found the goal, then expand this node
else
{
//Test if we can find the goal with a fixed path algorithm such as Dubins or Reeds-Shepp
List <Node> fixedPath = null;
//Don't try to find a fixed path each expansion, but try to find more fixed paths the close to the goal we are
if (
(allExpandedNodes.Count % 300 == 0) ||
(allExpandedNodes.Count % 20 == 0 && distanceSqrToGoal < 40f * 40f)
)
{
fixedPath = GetShortestReedsSheppPath(nextNode, targetCarData.GetCarTransform(), carData);
//If a fixed path is possible
if (fixedPath != null)
{
//Add this node to the open list
//Not 0 because that's the node we are expanding from
Node fixedPathNode = fixedPath[1];
fixedPathNode.cellPos = PathfindingController.ConvertCoordinateToCellPos(fixedPathNode.carPos);
fixedPathNode.h = HeuristicsController.heuristics[fixedPathNode.cellPos.x, fixedPathNode.cellPos.z];
fixedPathNode.previousNode = nextNode;
//Add the other car data to the node
//This is not exactly true but almost true because this node does almost have the same steering angle as the last node
fixedPathNode.steeringAngle = 0f;
//Now we can calculate the cost to reach this node
fixedPathNode.g = CalculateCosts(fixedPathNode);
//Add the node to the list with open nodes
openNodes.Add(fixedPathNode);
}
}
ExpandNode(nextNode);
//For debugging
allExpandedNodes.Add(nextNode);
}
}
}
}
public float GetCurrentHeading()
{
return(carData.GetHeading());
}
