//We generated this path by using the rear-wheel position. To easier be able to make the vehicle follow the path
//we should translate the path to the front axle, by using the the distance between the front- and rear axle
//We might also want the mirrored version of the front axle along the rear axle, to get a path we can use when reversing
public static void CalculateFrontAxlePositions(List <Node> path, CarData carData, Vector3 vehicleStartDir, Vector3 vehicleEndDir, bool isMirrored)
{
//Move the waypoints in the heading direction with a distance
//This distance is the same as the distance between the front axle and rear axle (= wheel base)
float moveDistance = carData.WheelBase;
for (int i = 0; i < path.Count; i++)
{
Vector3 dir_before, dir_after;
if (i == 0)
{
dir_before = vehicleStartDir;
}
else
{
dir_before = (path[i].rearWheelPos - path[i - 1].rearWheelPos).normalized;
if (path[i].isReversing)
{
dir_before *= -1f;
}
}
if (i == path.Count - 1)
{
dir_after = vehicleEndDir;
}
else
{
dir_after = (path[i + 1].rearWheelPos - path[i].rearWheelPos).normalized;
if (path[i + 1].isReversing)
{
dir_after *= -1f;
}
}
//Vector3 dir = (dir_before + dir_after) * 0.5f;
Vector3 dir = dir_after;
if (isMirrored)
{
dir *= -1f;
}
if (!isMirrored)
{
path[i].frontWheelPos = path[i].rearWheelPos + dir * moveDistance;
}
else
{
path[i].reverseWheelPos = path[i].rearWheelPos + dir * moveDistance;
}
//path[i].frontWheelPos = path[i].rearWheelPos;
}
}
public Car(Transform carTrans, SelfDrivingVehicle.VehicleDataController carDataController)
{
this.carData = carDataController.carData;
this.rearWheelPos = carDataController.RearWheelPos(carTrans);
this.heading = carDataController.HeadingInRadians(carTrans);
}
//
// Test if one path is drivable
//
public static bool IsPathDrivable(List <Node> path, CarData carData, Map map)
{
//Ignore the first node because we know its drivable
for (int i = 1; i < path.Count; i++)
{
Vector3 carPos = path[i].rearWheelPos;
float carHeading = path[i].heading;
if (HasCarInvalidPosition(carPos, carHeading, carData, map))
{
//This path is not drivable
return(false);
}
}
//This path is drivable
return(true);
}
//
// Check if the trailer is colliding with the drag vehicle
//
public static bool IsTrailerCollidingWithDragVehicle(
Vector3 semiRearWheelPos, float semiHeading, CarData semiData,
Vector3 trailerRearWheelPos, float trailerHeading, CarData trailerData)
{
bool isColliding = false;
//Use triangle-traingle intersection so we need the rectangles
Vector3 trailerCenter = trailerData.GetCenterPos(trailerRearWheelPos, trailerHeading);
Rectangle trailerRect = CarData.GetCornerPositions(trailerCenter, trailerHeading, trailerData.carWidth * 0.9f, trailerData.CarLength);
//The semi's cabin rectangle
Vector3 cabinCenter = semiData.GetSemiCabinCenter(semiRearWheelPos, semiHeading);
//Make it slightly shorter or too many false collisions
Rectangle semiRect = CarData.GetCornerPositions(cabinCenter, semiHeading, semiData.carWidth, semiData.cabinLength * 0.95f);
if (Intersections.AreRectangleRectangleIntersecting(trailerRect, semiRect))
{
return(true);
}
return(isColliding);
}
public Car(Vector3 rearWheelPos, float heading, CarData carData)
{
this.rearWheelPos = rearWheelPos;
this.heading = heading;
this.carData = carData;
}
//
// Calculate heuristics
//
private static float HeuristicsToReachGoal(Cell[,] cellData, IntVector2 cellPos, Node node, Car endCar, CarData carData)
{
float heuristics = cellData[cellPos.x, cellPos.z].heuristics;
//But if we are close we might want to use the Reeds-Shepp distance as heuristics
//This distance can be pre-calculated
if (cellData[cellPos.x, cellPos.z].distanceToTarget < 20f)
{
int timeBefore = Environment.TickCount;
float RS_distance = ReedsShepp.GetShortestDistance(
node.rearWheelPos,
node.heading,
endCar.rearWheelPos,
endCar.HeadingInRadians,
carData.turningRadius);
timer_ReedsSheppHeuristics += Environment.TickCount - timeBefore;
//Should use the max value according to the Junior report
if (RS_distance > heuristics)
{
heuristics = RS_distance;
//Debug.Log("Added Reeds-Shepp heuristics");
}
}
return(heuristics);
}
//
// Get all children to a node
//
private static List <Node> GetChildrenToNode(Node currentNode, Map map, Cell[,] cellData, CarData carData, Car endCar, Car startTrailer)
{
List <Node> childNodes = new List <Node>();
//To be able to expand we need the simulated vehicle's heading and position
float heading = currentNode.heading;
//Expand both forward and reverse
for (int i = 0; i < driveDistances.Length; i++)
{
float driveDistance = driveDistances[i];
//Expand all steering angles
for (int j = 0; j < steeringAngles.Length; j++)
{
//Steering angle
float alpha = steeringAngles[j];
//Turning angle
float beta = (driveDistance / carData.WheelBase) * Mathf.Tan(alpha);
//Simulate the car driving forward by using a mathematical car model
Vector3 newRearWheelPos = VehicleSimulationModels.CalculateNewPosition(heading, beta, driveDistance, currentNode.rearWheelPos);
float newHeading = VehicleSimulationModels.CalculateNewHeading(heading, beta);
//In which cell did we end up?
IntVector2 cellPos = map.ConvertWorldToCell(newRearWheelPos);
//Because we are doing obstacle detection later, we have to check if this pos is within the map
if (!map.IsCellWithinGrid(cellPos))
{
continue;
}
//Generate a new child node
Node childNode = new Node(
previousNode: currentNode,
rearWheelPos: newRearWheelPos,
heading: newHeading,
isReversing: driveDistance < 0f ? true : false);
float heuristics = HeuristicsToReachGoal(cellData, cellPos, childNode, endCar, carData);
childNode.AddCosts(
gCost: CostToReachNode(childNode, map, cellData),
hCost: heuristics);
//Calculate the new heading of the trailer if we have a trailer
if (startTrailer != null)
{
//Whats the new trailer heading at this childNode
float thetaOld = currentNode.TrailerHeadingInRadians;
float thetaOldDragVehicle = currentNode.HeadingInRadians;
float D = driveDistance;
float d = startTrailer.carData.WheelBase;
float newTrailerHeading = VehicleSimulationModels.CalculateNewTrailerHeading(thetaOld, thetaOldDragVehicle, D, d);
childNode.TrailerHeadingInRadians = newTrailerHeading;
//The trailer sux when reversing so add an extra cost
if (childNode.isReversing)
{
childNode.gCost += Parameters.trailerReverseCost;
}
}
childNodes.Add(childNode);
}
}
//Expand Reeds-Shepp curve and add it as child node if we are "close" to the goal we want to reach
int timeBefore = Environment.TickCount;
//Dont do it every node because is expensive
IntVector2 goalCell = map.ConvertWorldToCell(endCar.rearWheelPos);
float distanceToEnd = cellData[goalCell.x, goalCell.z].distanceToTarget;
//The probability should increase the close to the end we are
float testProbability = Mathf.Clamp01((maxReedsSheppDist - distanceToEnd) / maxReedsSheppDist) * 0.2f;
float probability = UnityEngine.Random.Range(0f, 1f);
if ((distanceToEnd < maxReedsSheppDist && probability < testProbability) || (distanceToEnd < 40f && probability < 0.005f))
{
List <RSCar> shortestPath = ReedsShepp.GetShortestPath(
currentNode.rearWheelPos,
currentNode.heading,
endCar.rearWheelPos,
endCar.HeadingInRadians,
carData.turningRadius,
driveDistance,
generateOneWp: true);
if (shortestPath != null && shortestPath.Count > 1)
{
//The first node in this list is where we currently are so we will use the second node
//But we might need to use several Reeds-Shepp nodes because if the path is going from
//forward to reverse, we cant ignore the change in direction, so we add a node before the
//length which should be the driving distance
//But the easiest is just to add the second node
RSCar carToAdd = shortestPath[1];
bool isReversing = carToAdd.gear == RSCar.Gear.Back ? true : false;
IntVector2 cellPos = map.ConvertWorldToCell(carToAdd.pos);
//Because we are doing obstacle detection later, we have to check if this pos is within the map
if (map.IsCellWithinGrid(cellPos))
{
Node childNode = new Node(
previousNode: currentNode,
rearWheelPos: carToAdd.pos,
heading: carToAdd.HeadingInRad,
isReversing: isReversing);
float heuristics = HeuristicsToReachGoal(cellData, cellPos, childNode, endCar, carData);
childNode.AddCosts(
gCost: CostToReachNode(childNode, map, cellData),
hCost: heuristics);
childNodes.Add(childNode);
//Debug.Log("Added RS node");
}
}
}
timer_ReedsSheppNode += Environment.TickCount - timeBefore;
return(childNodes);
}
//
// Check if the car is colliding with an obstacle or is outside of map
//
public static bool HasCarInvalidPosition(Vector3 carRearWheelPos, float heading, CarData carData, Map map)
{
bool hasInvalidPosition = false;
//Step 1. Check if the car's rear wheel center is inside of the map
IntVector2 rearWheelCellPos = map.ConvertWorldToCell(carRearWheelPos);
if (!map.IsCellWithinGrid(rearWheelCellPos))
{
//This is not a valid position
hasInvalidPosition = true;
return(hasInvalidPosition);
}
//Step 2. Check if any of the car's corner is outside of the map
//Make the car bigger than it is to be on the safe side
float carLength = carData.CarLength + Parameters.marginOfSafety;
float carWidth = carData.carWidth + Parameters.marginOfSafety;
Vector3 carCenterPos = carData.GetCenterPos(carRearWheelPos, heading);
//Find all corners of the car
Rectangle corners = CarData.GetCornerPositions(carCenterPos, heading, carWidth, carLength);
//Detect if any of the corners is outside of the map = is the cell the corner is a part of the map
HashSet <IntVector2> carCellPositions = new HashSet <IntVector2>();
carCellPositions.Add(map.ConvertWorldToCell(corners.FL));
carCellPositions.Add(map.ConvertWorldToCell(corners.FR));
carCellPositions.Add(map.ConvertWorldToCell(corners.BL));
carCellPositions.Add(map.ConvertWorldToCell(corners.BR));
foreach (IntVector2 cellPos in carCellPositions)
{
if (!map.IsCellWithinGrid(cellPos))
{
//At least one of the corners is outside of the map
hasInvalidPosition = true;
return(hasInvalidPosition);
}
}
//Step 3. Check if some of the car's known positions are far away from an obstacle
//The car is not colliding with anything if the steps to an obstacle from center of car is greater than the length of the car
IntVector2 carCenterCellPos = map.ConvertWorldToCell(carCenterPos);
if (map.cellData[carCenterCellPos.x, carCenterCellPos.z].distanceToClosestObstacle > carData.CarLength * 0.7f)
{
//This is a valid position
hasInvalidPosition = false;
return(hasInvalidPosition);
}
//Step 4. Check if the car is hitting an obstacle
//Use the car's corners and then rectangle-rectangle-intersection with the obstacles
hasInvalidPosition = IsCarIntersectingWithObstacles(corners, carCenterCellPos, map);
return(hasInvalidPosition);
}