//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); } } } } }
//Change the car's speed to slow down when curvy path or close to end of path void ChangeSpeed() { //Slow down if we reach an endpoint or a if we are about to change driving direction bool isGoingTooFast = false; //The car's current speed float carSpeed = carScript.GetCarSpeed(); //Change 1 //If we are driving forward, then change speed depending on angle of the path we are following if (!wayPoints[currentWayPointIndex].isReversing) { //Change speed depending on the angle to the next waypoint int lookAheadIndex = currentWayPointIndex + 4; //As we get closer to the end we cant look ahead too many waypoints lookAheadIndex = Mathf.Clamp(lookAheadIndex, 2, wayPoints.Count - 1); Vector3 P2 = GetWayPointPos(lookAheadIndex); Vector3 P1 = GetWayPointPos(currentWayPointIndex); Vector3 carPos = carScript.transform.position; //Angle is 0 degrees if P2 is infront of P1 in the direction of the car float angle = Vector3.Angle(P1 - carPos, P2 - carPos); //print(angle); if (angle > 10f && carSpeed > 15f) { isGoingTooFast = true; } } //Change 2 //Slow down the car as it's getting close to a change, like forward -> reverse bool closeToChange = false; bool currentDrivingDirection = wayPoints[currentWayPointIndex].isReversing; bool isCloseToEnd = false; //The number of nodes until change int counter = 0; //The max number of nodes until we care about a change int maxNodesUntilChange = 8; for (int i = currentWayPointIndex + 1; i < wayPoints.Count - 0; i++) { if (counter > maxNodesUntilChange) { break; } if (wayPoints[i].isReversing != currentDrivingDirection) { closeToChange = true; //print("Close to change in driving direction"); break; } //Last waypoint if (i == wayPoints.Count - 1) { closeToChange = true; isCloseToEnd = true; //print("Close to end of path"); } counter += 1; } //print(counter); //Now we need to slow down to something close to 0 if (closeToChange) { //The easiest way is to use an animation curve float percentage = (float)counter / (float)maxNodesUntilChange; //Has to be different or the car will drive very slowly when changing direction float minCarSpeed = 5f; if (isCloseToEnd) { minCarSpeed = 1f; } float maxCarSpeed = minCarSpeed + (slowDownCurve.Evaluate(percentage) * 20f); //print(maxCarSpeed); if (carSpeed > maxCarSpeed) { isGoingTooFast = true; } } //Change 3 //Slow down the car if it is deviting too much from the path and heading float CTE = Mathf.Abs(CalculateCTE()); float wantedHeading = wayPoints[currentWayPointIndex].heading * Mathf.Rad2Deg; //This is already in degrees float currentHeading = carData.GetHeading(); float headingDiff = Mathf.Abs(wantedHeading - currentHeading); if (CTE > 0.2f && headingDiff > 5f) { isGoingTooFast = true; } //Stop the car if its going too fast if (isGoingTooFast && carScript.GetCarSpeed() > 5f) { carScript.StopCar(); //print("Stop car"); } else { //print("Drive car"); } }
//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); } } } }
public float GetCurrentHeading() { return(carData.GetHeading()); }