//Check if the car outside of map (MATT)
public static bool TargetPositionWithinTrack(Vector3 carRearWheelPos, float heading, CarData carData)
{
bool withinTrack = true;
//Make the car bigger than it is to be on the safe side
float marginOfSafety = 0.5f;
float carLength = carData.GetLength() + marginOfSafety;
float carWidth = carData.GetWidth() + marginOfSafety;
//Find the center pos of the car (carPos is at the rearWheels)
float distCenterToRearWheels = carData.GetDistanceToRearWheels();
float xCenter = carRearWheelPos.x + distCenterToRearWheels * Mathf.Sin(heading);
float zCenter = carRearWheelPos.z + distCenterToRearWheels * Mathf.Cos(heading);
Vector3 carPos = new Vector3(xCenter, carRearWheelPos.y, zCenter);
//Find all corners of the car
Rectangle carCornerPos = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);
//Detect if any of the corners is outside of the map
if (
!PathfindingController.IsPositionWithinGrid(carCornerPos.FL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.FR) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BR))
{
//At least one of the corners is outside of the map
withinTrack = false;
}
return(withinTrack);
}
//Method 2 - Find which cells the car is intersecting with and return the obstacles that intersect with those cells
private static List <ObstacleData> FindCloseObstaclesCell(Vector3 carPos, Rectangle carCorners)
{
//The list with close obstacles
List <ObstacleData> closeObstacles = new List <ObstacleData>();
IntVector2 carCellPos = PathfindingController.ConvertCoordinateToCellPos(carPos);
//Check an area of cells around the car's cell for obstacles
//The car is 5 m long so search 3 m to each side?
int searchArea = 3;
for (int x = -searchArea; x <= searchArea; x++)
{
for (int z = -searchArea; z <= searchArea; z++)
{
IntVector2 cellPos = new IntVector2(carCellPos.x + x, carCellPos.z + z);
//Is this cell within the map?
if (PathfindingController.IsCellWithinGrid(cellPos))
{
//Add all obstacles from this list to the list of close obstacles
List <ObstacleData> obstaclesInCell = ObstaclesController.obstaclesInCell[cellPos.x, cellPos.z];
if (obstaclesInCell != null)
{
for (int i = 0; i < obstaclesInCell.Count; i++)
{
//Might add the same obstacle more than one time, but maybe that's not a big problem?
closeObstacles.Add(obstaclesInCell[i]);
}
}
}
}
}
return(closeObstacles);
}
//Expand one node
private void ExpandNode(Node currentNode)
{
//To be able to expand we need the simulated car's heading and position
float heading = currentNode.heading;
//Save which cells are expanded so we can close them after we have expanded all directions
expandedCellsForward.Clear();
expandedCellsReverse.Clear();
//Expand both forward and reverse
for (int j = 0; j < driveDistances.Length; j++)
{
float driveDistance = driveDistances[j];
//Expand by looping through all steering angles
for (int i = 0; i < steeringAngles.Count; i++)
{
//Steering angle
float alpha = steeringAngles[i];
//Turning angle
float beta = (driveDistance / carData.GetWheelBase()) * Mathf.Tan(alpha);
//Simulate the skeleton car
Vector3 newCarPos = SkeletonCar.CalculateNewPosition(heading, beta, driveDistance, currentNode.carPos);
float newHeading = SkeletonCar.CalculateNewHeading(heading, beta);
//Get the cell pos of the new position
IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(newCarPos);
//Detect if the car is colliding with obstacle or is outside of map
if (!ObstaclesDetection.TargetPositionWithinTrack(newCarPos, newHeading, carData))
//if (ObstaclesDetection.HasCarInvalidPosition(newCarPos, newHeading, carData)) (MATT)
{
continue;
}
//Is this node closed? Important this is after obstacle/outside of map detection or may be out of range
else if (
(driveDistance < 0f && closedCellsReverse[cellPos.x, cellPos.z]) ||
(driveDistance > 0f && closedCellsForward[cellPos.x, cellPos.z]))
{
continue;
}
//We can create a new node because this is a valid position
else
{
//
//Calculate costs
//
//The cost it took to get to this node
float cost = Mathf.Abs(driveDistance);
//Add cost for turning if we are not having the same steering angle as previously
if (alpha != currentNode.steeringAngle)
{
cost += turningCost;
}
//Add a cost if we are close to an obstacle, its better to drive around them than close to them
//We can use the flow map to check this
/* (MATT)
* if (ObstaclesController.distanceToClosestObstacle[cellPos.x, cellPos.z] < 6)
* {
* cost += obstacleCost;
* }
*/
//Add cost for reversing
if (driveDistance < 0f)
{
cost += reverseCost;
}
//Add a cost if we are switching from reverse -> forward or the opposite
bool isReversing = driveDistance < 0f ? true : false;
if ((isReversing && !currentNode.isReversing) || (!isReversing && currentNode.isReversing))
{
cost += switchingDirectionOfMovementCost;
}
//The cost to reach this node
float g2 = currentNode.g + cost;
//Is this cost lower than it was?
if (
(driveDistance > 0f && g2 < lowestCostForward[cellPos.x, cellPos.z]) ||
(driveDistance < 0f && g2 < lowestCostReverse[cellPos.x, cellPos.z]))
{
//We have found a better path
if (driveDistance > 0f)
{
//lowestCostForward[cellPos.x, cellPos.z] = g2;
expandedCellsForward.Add(new ExpandedCellsStorage(cellPos, g2));
}
if (driveDistance < 0f)
{
//lowestCostReverse[cellPos.x, cellPos.z] = g2;
expandedCellsReverse.Add(new ExpandedCellsStorage(cellPos, g2));
}
//
//Create a new node
//
Node nextNode = new Node();
nextNode.g = g2;
nextNode.h = HeuristicsController.heuristics[cellPos.x, cellPos.z];
nextNode.cellPos = cellPos;
nextNode.previousNode = currentNode;
//Add the car data to the node
nextNode.carPos = newCarPos;
nextNode.heading = newHeading;
nextNode.steeringAngle = steeringAngles[i];
//Are we reversing?
nextNode.isReversing = driveDistance < 0f ? true : false;
//Add the node to the list with open nodes
openNodes.Add(nextNode);
}
}
}
}
//Close all cells we expanded to from this node so we cant reach them again from another node
if (expandedCellsForward.Count > 0)
{
for (int k = 0; k < expandedCellsForward.Count; k++)
{
//closedArrayForward[expandedCellsForward[k].x, expandedCellsForward[k].z] = true;
IntVector2 cellPos = expandedCellsForward[k].cellPos;
if (expandedCellsForward[k].cost < lowestCostForward[cellPos.x, cellPos.z])
{
lowestCostForward[cellPos.x, cellPos.z] = expandedCellsForward[k].cost;
}
}
}
if (expandedCellsReverse.Count > 0)
{
for (int k = 0; k < expandedCellsReverse.Count; k++)
{
//closedArrayReverse[expandedCellsReverse[k].x, expandedCellsReverse[k].z] = true;
IntVector2 cellPos = expandedCellsReverse[k].cellPos;
if (expandedCellsReverse[k].cost < lowestCostReverse[cellPos.x, cellPos.z])
{
lowestCostReverse[cellPos.x, cellPos.z] = expandedCellsReverse[k].cost;
}
}
}
}
//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);
}
}
}
}
}
//Check if the car is colliding with an obstacle or is outside of map
public static bool HasCarInvalidPosition(Vector3 carRearWheelPos, float heading, CarData carData)
{
bool hasInvalidPosition = false;
//Make the car bigger than it is to be on the safe side
float marginOfSafety = 0.5f;
float carLength = carData.GetLength() + marginOfSafety;
float carWidth = carData.GetWidth() + marginOfSafety;
//Find the center pos of the car (carPos is at the rearWheels)
float distCenterToRearWheels = carData.GetDistanceToRearWheels();
float xCenter = carRearWheelPos.x + distCenterToRearWheels * Mathf.Sin(heading);
float zCenter = carRearWheelPos.z + distCenterToRearWheels * Mathf.Cos(heading);
Vector3 carPos = new Vector3(xCenter, carRearWheelPos.y, zCenter);
//
// Step 1. Check if the car's corners is inside of the map
//
//Find all corners of the car
Rectangle carCornerPos = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);
//Detect if any of the corners is outside of the map
if (
!PathfindingController.IsPositionWithinGrid(carCornerPos.FL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.FR) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BL) ||
!PathfindingController.IsPositionWithinGrid(carCornerPos.BR))
{
//At least one of the corners is outside of the map
hasInvalidPosition = true;
return(hasInvalidPosition);
}
//
// Step 2. Check if the car's center position is far away from an obstacle
//
//We dont need to check if the car is colliding with an obstacle if the distance to an obstacle is great
IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(carPos);
//The car is not colliding with anything if the steps to an obstacle is greater than the length of the car
if (ObstaclesController.distanceToClosestObstacle[cellPos.x, cellPos.z] > carData.GetLength())
{
//This is a valid position
hasInvalidPosition = false;
return(hasInvalidPosition);
}
//
// Step 3. Check if the car is hitting an obstacle
//
//Find all corners of the car
Rectangle carCornerPosFat = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);
//Method 1 - Use the car's corners and then rectangle-rectangle-intersection with the obstacles
hasInvalidPosition = ObstacleDetectionCorners(carPos, carCornerPosFat);
//Method 2 - Approximate the car with circles
//hasInvalidPosition = ObstacleDetectionCircles(carCenterPos, heading, carData, carCornerPosFat);
return(hasInvalidPosition);
}
//Expand one node
private void ExpandNode(Node currentNode)
{
//To be able to expand we need the simulated car's heading and position
float heading = currentNode.heading;
//Expand both forward and reverse
for (int j = 0; j < driveDistances.Length; j++)
{
float driveDistance = driveDistances[j];
//Expand by looping through all steering angles
for (int i = 0; i < steeringAngles.Count; i++)
{
//Steering angle
float alpha = steeringAngles[i];
//Turning angle
float beta = (driveDistance / carData.GetWheelBase()) * Mathf.Tan(alpha);
//Simulate the skeleton car
Vector3 newCarPos = SkeletonCar.CalculateNewPosition(heading, beta, driveDistance, currentNode.carPos);
float newHeading = SkeletonCar.CalculateNewHeading(heading, beta);
//Get the cell pos of the new position
IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(newCarPos);
//
//Check if the car is colliding with obstacle or is outside of map
//
if (!ObstaclesDetection.TargetPositionWithinTrack(newCarPos, newHeading, carData))
//if (ObstaclesDetection.HasCarInvalidPosition(newCarPos, newHeading, carData)) (MATT)
{
continue;
}
//
//Check if this node is closed
//
//Important this is after obstacle/outside of map detection or may be out of range
int roundedAngle = RoundAngle(newHeading * Mathf.Rad2Deg);
if (closedCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
{
continue;
}
//
//Check if this node is cheaper than any other node by calculating costs
//
//First create a new node with all data we need to calculate costs
Node nextNode = new Node();
nextNode.cellPos = cellPos;
nextNode.carPos = newCarPos;
nextNode.heading = newHeading;
nextNode.steeringAngle = steeringAngles[i];
nextNode.isReversing = driveDistance < 0f ? true : false;
nextNode.h = HeuristicsController.heuristics[cellPos.x, cellPos.z];
nextNode.previousNode = currentNode;
//Now we can calculate the cost to reach this node
nextNode.g = CalculateCosts(nextNode);
//Is this cost lower than it was or have we not expanded to this this cell with this angle?
if (
((lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle) && nextNode.g < lowestCostCells[cellPos.x, cellPos.z][roundedAngle])) ||
!lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
{
//We havent expanded to this node before with this angle
if (!lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
{
lowestCostCells[cellPos.x, cellPos.z].Add(roundedAngle, nextNode.g);
}
//The costs is lower than a previous expansion
else
{
lowestCostCells[cellPos.x, cellPos.z][roundedAngle] = nextNode.g;
//Now we should remove the old node from the heap
//Actually not needed because it's costly to remove nodes from the heap
//So its better to just skip the node when finding nodes with lowest cost
}
//Add the node to the list with open nodes
openNodes.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);
}
}
}
}
//Figure out which cells the obstacle touch
private void CellObstacleDetection()
{
//Now we need to check which cells the obstacles intersect with
//Find if corners of the obstacles intersect with a cell
Vector3[] corners = new Vector3[4];
//Loop through all obstacles
for (int i = 0; i < obstaclesPosList.Count; i++)
{
//Loop through all corners of the obstacle cubes, and not just the center
Vector3 centerPos = obstaclesPosList[i].centerPos;
//To easier loop through the corners
corners[0] = obstaclesPosList[i].cornerPos.BL;
corners[1] = obstaclesPosList[i].cornerPos.FL;
corners[2] = obstaclesPosList[i].cornerPos.FR;
corners[3] = obstaclesPosList[i].cornerPos.BR;
//Loop through all corners
for (int j = 0; j < corners.Length; j++)
{
//In which cell is this position?
IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(corners[j]);
//It's an obstacle in this square
isObstacleInCell[cellPos.x, cellPos.z] = true;
//Populate the other arrays - need to do this for every corner
//Get the list
List <ObstacleData> obstaclesList = obstaclesInCell[cellPos.x, cellPos.z];
//Create a new list if needed
if (obstaclesList == null)
{
obstaclesList = new List <ObstacleData>();
obstaclesInCell[cellPos.x, cellPos.z] = obstaclesList;
}
//Check if the center of the obstacle already is in the list
bool isInList = false;
for (int k = 0; k < obstaclesList.Count; k++)
{
if (Vector3.SqrMagnitude(obstaclesList[k].centerPos - centerPos) < 0.001f)
{
isInList = true;
break;
}
}
if (!isInList)
{
obstaclesList.Add(obstaclesPosList[i]);
}
}
}
}
