void Update()
{
//Check if the target position has changed
if (targetCarTrans.position != egoCarInterface.GetTargetPosition())
{
targetCarTrans.position = egoCarInterface.GetTargetPosition();
targetCarTrans.rotation = egoCarInterface.GetCurrentTransform().rotation; //temporary
float heading = egoCarInterface.GetCurrentHeading(); //temporary
// temporary testing to see where a new CarData can be created only from position and rotation
// Tried similar instantiation as GenerateHybridAStarPath() from HybridAStarAngle.cs
Debug.Log(string.Format("Target car position: {0}", targetCarData.GetCarTransform().position));
Debug.Log(string.Format("Target car rear wheel: {0}", targetCarData.GetRearWheelPos()));
//Check if the target car has a valid position
if (HasTargetCarValidPosition(targetCarTrans.position, heading, targetCarData))
{
//Stop the car
SimController.current.StopCar();
//Wait for the car to stop before trying to find a path
StartCoroutine(WaitForCarToStop());
}
}
}
void Start()
{
//for testing
InvokeRepeating("ResetObstacles", 0f, 10f);
//Generate obstacles
//Has to do it from this script or the obstacles might be created after this script has
//finished and mess things up
GetComponent <ObstaclesController>().InitObstacles();
hybridAStar = new HybridAStar();
hybridAStarAngle = new HybridAStarAngle();
//Create the textures showing various stuff
debugController.DisplayCellObstacleIntersection();
debugController.DisplayObstacleFlowField();
targetCarTrans.position = egoCarData.GetRearWheelPos();
targetCarTrans.rotation = egoCarData.GetCarTransform().rotation;
targetCarData = targetCarTrans.GetComponent <CarData>();
Debug.Log(string.Format("Target car position: {0}", targetCarData.GetCarTransform().position));
Debug.Log(string.Format("Target car rear wheel: {0}", targetCarData.GetRearWheelPos()));
}
//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);
}
}
}
}
}
//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 Transform GetCurrentTransform()
{
return(carData.GetCarTransform());
}
