//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); }