//
// 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 the shortest Reeds-Shepp path and display it
private void DisplayShortestPath(Transform startCarTrans, Transform goalCarTrans)
{
Vector3 startPos = startCarTrans.position;
float startHeading = startCarTrans.eulerAngles.y * Mathf.Deg2Rad;
Vector3 goalPos = goalCarTrans.position;
float goalHeading = goalCarTrans.eulerAngles.y * Mathf.Deg2Rad;
float turningRadius = SimController.current.GetActiveCarData().carData.turningRadius;
//Get the shortest Reeds-Shepp path
List <RSCar> shortestPath = ReedsShepp.GetShortestPath(
startPos, startHeading, goalPos, goalHeading, turningRadius, wpDistance: 1f, generateOneWp: false);
//If we found a path
if (shortestPath != null && shortestPath.Count > 1)
{
//Display the path with line renderers
DisplayPath(shortestPath);
}
}
private void SmoothThisPath(Map map)
{
//Generate the Reeds-Shepp path with some turning radius
//Get the shortest path
Vector3 startPos = startTrans.position;
Vector3 endPos = endTrans.position;
float startRot = startTrans.rotation.eulerAngles.y * Mathf.Deg2Rad;
float endRot = endTrans.rotation.eulerAngles.y * Mathf.Deg2Rad;
//Make sure the start and end are within the map
if (!(map.IsPosWithinGrid(startPos) && map.IsPosWithinGrid(endPos)))
{
return;
}
//Get the shortest path
List <RSCar> shortestPath = ReedsShepp.GetShortestPath(startPos, startRot, endPos, endRot, 12f, 1f, generateOneWp: false);
//Make it unsmooth to easier see the difference
Random.InitState(0);
for (int i = 1; i < shortestPath.Count - 1; i++)
{
Vector3 p = shortestPath[i].pos;
float dist = 0.4f;
p.x += Random.Range(-dist, dist);
p.z += Random.Range(-dist, dist);
shortestPath[i].pos = p;
}
//To Node data formart
List <Node> nodes = new List <Node>();
for (int i = 1; i < shortestPath.Count - 1; i++)
{
Node previousNode = null;
Vector3 pos = shortestPath[i].pos;
float heading = shortestPath[i].HeadingInRad;
bool isReversing = shortestPath[i].gear == RSCar.Gear.Back ? true : false;
Node node = new Node(previousNode, pos, heading, isReversing);
nodes.Add(node);
}
//Smooth the path and push it away from obstacles
List <Node> smoothPath = ModifyPath.SmoothPath(nodes, map, false, isDebugOn: true);
//Display
DisplayPath(nodes, Color.white);
DisplayPathNodes(nodes, Color.black);
DisplayPath(smoothPath, Color.blue);
//DisplayPathNodes(smoothPath, Color.black);
}
//
// 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);
}
