void Update()
        {
            //Check if the target position has changed
            if (targetCarTrans.position != egoCarInterface.GetTargetPosition())
            {
                targetCarTrans.position = egoCarInterface.GetTargetPosition();
                targetCarTrans.rotation = egoCarInterface.GetCurrentTransform().rotation;       //temporary
                float heading = egoCarInterface.GetCurrentHeading();                            //temporary

                // temporary testing to see where a new CarData can be created only from position and rotation
                // Tried similar instantiation as GenerateHybridAStarPath() from HybridAStarAngle.cs
                Debug.Log(string.Format("Target car position: {0}", targetCarData.GetCarTransform().position));
                Debug.Log(string.Format("Target car rear wheel: {0}", targetCarData.GetRearWheelPos()));

                //Check if the target car has a valid position
                if (HasTargetCarValidPosition(targetCarTrans.position, heading, targetCarData))
                {
                    //Stop the car
                    SimController.current.StopCar();

                    //Wait for the car to stop before trying to find a path
                    StartCoroutine(WaitForCarToStop());
                }
            }
        }
        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);
                        }
                    }
                }
            }
        }
Beispiel #4
0
        //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 Transform GetCurrentTransform()
 {
     return(carData.GetCarTransform());
 }