//Find the shortest Reeds-Shepp path
List <Node> GetShortestReedsSheppPath(Node nextNode, Transform goalTrans, CarData carData)
{
//Get the current position and heading of the car
//In Hybrid A* we can't use the car, but the current position and heading in search tree
Vector3 treeCarPos = nextNode.carPos;
//We store heading in radians but dubins generator is currently using degrees as input
float startHeading = nextNode.heading;
Vector3 goalPos = goalTrans.position;
float goalHeading = goalTrans.eulerAngles.y * Mathf.Deg2Rad;
List <Node> shortestPath = reedsSheppPathGenerator.GetShortestReedSheppPath(
treeCarPos,
startHeading,
goalPos,
goalHeading);
//If we have a path and it is not blocked by obstacle
//if (shortestPath != null && ObstaclesDetection.IsFixedPathDrivable(shortestPath, carData)) (MATT)
if (shortestPath != null)
{
return(shortestPath);
}
return(null);
}
// Use this for initialization
void Start()
{
carData = GetComponent <CarData>();
targetPosition = new Vector3(195, 0, 25);
//Draw the lidar visualization
line = gameObject.GetComponent <LineRenderer>();
line.SetVertexCount(numLineSegments + 1);
line.useWorldSpace = false;
DrawLidar();
}
void Awake()
{
egoCar = GameObject.Find("EgoCar");
egoCarInterface = egoCar.GetComponent <EgoCarInterface>();
egoCarData = egoCar.GetComponent <CarData>();
heuristicsController = new HeuristicsController();
smoothPathController = new SmoothPathController();
debugController = GetComponent <DebugController>();
}
//Check if the target car has a valid position
private bool HasTargetCarValidPosition(Vector3 targetPos, float heading, CarData carData)
{
bool hasValidPosition = false;
float targetCarHeading = heading * Mathf.Deg2Rad;
if (ObstaclesDetection.TargetPositionWithinTrack(targetPos, targetCarHeading, carData)) //(MATT)
{
hasValidPosition = true;
}
return(hasValidPosition);
}
void Start()
{
//Move the center of mass down
Rigidbody carRB = GetComponent <Rigidbody>();
carRB.centerOfMass = carRB.centerOfMass - new Vector3(0f, 0.8f, 0f);
PIDScript = GetComponent <PIDController>();
carData = GetComponent <CarData>();
followPathScript = GetComponent <FollowPath>();
}
//Test if one path is drivable
public static bool IsFixedPathDrivable(List <Node> path, CarData carData)
{
for (int i = 0; i < path.Count; i++)
{
Vector3 carPos = path[i].carPos;
float carHeading = path[i].heading;
if (HasCarInvalidPosition(carPos, carHeading, carData))
{
//This path is not drivable
return(false);
}
}
//This path is drivable
return(true);
}
//Generate a path with Hybrid A*
public void GenerateHybridAStarPath(Transform targetTrans, List <Node> finalPath, List <Node> allExpandedNodes)
{
CarData targetCarData = targetTrans.GetComponent <CarData>();
//Get the data belonging to the current active car
this.carData = SimController.current.GetActiveCarData();
//Each car may have a different turning radius
reedsSheppPathGenerator = new GenerateReedsShepp(carData.GetTurningRadius());
//Everything we need to reset before we begin
Reset();
//Run the main loop
RunHybridAStar(targetCarData, allExpandedNodes);
//Generate the final path when Hybrid A* has found the goal
GenerateFinalPath(finalPath);
}
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);
}
}
}
}
}
//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);
}
//Approximate the car's area with circles to detect if there's an obstacle within the circle
private static bool ObstacleDetectionCircles(Vector3 carPos, float heading, CarData carData, Rectangle carCornerPos)
{
bool hasInvalidPosition = false;
Vector3[] circlePositions = new Vector3[3];
//Get the position of the 3 circles that approximates the size of the car
circlePositions = SkeletonCar.GetCirclePositions(carPos, heading, circlePositions);
//
//Find all obstacles close to the car
//
//The car's length is 5 m and each obstacle is 1 m, so add a little to be on the safe side
//float searchRadius = carData.GetLength() * 0.5f + 1.5f;
//List<ObstacleData> obstaclesThatAreClose = FindCloseObstaclesWithinRadius(carPos, searchRadius * searchRadius);
List <ObstacleData> obstaclesThatAreClose = FindCloseObstaclesCell(carPos, carCornerPos);
//If there are no obstacle close, then return
if (obstaclesThatAreClose.Count == 0)
{
return(hasInvalidPosition);
}
//
//If there are obstacles around the car, then we have to see if some of them intersect
//
//The radius of one circle that approximates the area of the car
//The width of the car is 2 m but the circle has to be larger to provide a margin of safety
float circleRadius = 1.40f;
//But we also need to add the radius of the box obstacle which has a width of 1 m
float criticalRadius = circleRadius + 0.5f;
//And square it to speed up
float criticalRadiusSqr = criticalRadius * criticalRadius;
//Loop through all circles and detect if there's an obstacle within the circle
for (int i = 0; i < circlePositions.Length; i++)
{
Vector3 currentCirclePos = circlePositions[i];
//Is there an obstacle within the radius of this circle
for (int j = 0; j < obstaclesThatAreClose.Count; j++)
{
float distSqr = (currentCirclePos - obstaclesThatAreClose[j].centerPos).sqrMagnitude;
if (distSqr < criticalRadiusSqr)
{
hasInvalidPosition = true;
return(hasInvalidPosition);
}
}
}
return(hasInvalidPosition);
}
//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);
}
//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);
}
}
}
}
void Start()
{
carScript = GetComponent <CarController>();
carData = GetComponent <CarData>();
}
