Exemplo n.º 1
0
        //
        // Generate a path with Hybrid A*
        //
        public static List <Node> GeneratePath(Car startCar, Car endCar, Map map, List <Node> allExpandedNodes, Car startTrailer)
        {
            //Reset timers
            timer_selectLowestCostNode    = 0;
            timer_addNodeToHeap           = 0;
            timer_findChildren            = 0;
            timer_isCollidingWithObstacle = 0;
            timer_ReedsSheppNode          = 0;
            timer_ReedsSheppHeuristics    = 0;
            timer_TrailerCollision        = 0;
            //Other data we want to track
            //How many nodes did we prune?
            int prunedNodes = 0;
            //To track max number of nodes in the heap
            int maxNodesInHeap = 0;


            //Init the data structure we need
            int mapWidth = map.MapWidth;

            //Is faster to cache this than using map.cellData
            Cell[,] cellData = map.cellData;

            //Open nodes - the parameter is how many items can fit in the heap
            //If we lower the heap size it will still find a path, which is more drunk
            Heap <Node> openNodes = new Heap <Node>(200000);

            //int in the dictionaries below is the rounded heading used to enter a cell
            HashSet <int>[,] closedCells = new HashSet <int> [mapWidth, mapWidth];
            //The node in the cell with the lowest g-cost at a certain angle
            Dictionary <int, Node>[,] lowestCostNodes = new Dictionary <int, Node> [mapWidth, mapWidth];
            //Trailer
            //int in the dictionaries below is the rounded heading used to enter a cell
            HashSet <int>[,] closedCellsTrailer     = new HashSet <int> [mapWidth, mapWidth];
            HashSet <int>[,] lowestCostNodesTrailer = new HashSet <int> [mapWidth, mapWidth];

            for (int x = 0; x < mapWidth; x++)
            {
                for (int z = 0; z < mapWidth; z++)
                {
                    closedCells[x, z]     = new HashSet <int>();
                    lowestCostNodes[x, z] = new Dictionary <int, Node>();

                    //Trailer
                    closedCellsTrailer[x, z]     = new HashSet <int>();
                    lowestCostNodesTrailer[x, z] = new HashSet <int>();
                }
            }


            //Create the first node
            //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
            IntVector2 startCellPos = map.ConvertWorldToCell(startCar.rearWheelPos);

            Node node = new Node(
                previousNode: null,
                rearWheelPos: startCar.rearWheelPos,
                heading: startCar.HeadingInRadians,
                isReversing: false);

            if (startTrailer != null)
            {
                node.TrailerHeadingInRadians = startTrailer.HeadingInRadians;
            }

            node.AddCosts(gCost: 0f, hCost: cellData[startCellPos.x, startCellPos.z].heuristics);

            openNodes.Add(node);


            //The end of the path, which we will return
            Node finalNode = null;


            //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 break out of the loop if it takes too long time
            int iterations = 0;

            IntVector2 goalCellPos = map.ConvertWorldToCell(endCar.rearWheelPos);

            while (!found && !resign)
            {
                if (iterations > 400000)
                {
                    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
                {
                    if (openNodes.Count > maxNodesInHeap)
                    {
                        maxNodesInHeap = openNodes.Count;
                    }

                    //Get the node with the lowest f cost
                    int timeBefore = Environment.TickCount;

                    Node nextNode = openNodes.RemoveFirst();

                    timer_selectLowestCostNode += Environment.TickCount - timeBefore;


                    //Close this cell
                    IntVector2 cellPos = map.ConvertWorldToCell(nextNode.rearWheelPos);

                    int roundedHeading = HelpStuff.RoundValue(nextNode.HeadingInDegrees, headingResolution);

                    HashSet <int> closedHeadingsInThisCell = closedCells[cellPos.x, cellPos.z];

                    bool haveAlreadyClosedCell = false;

                    //Close the cell, but we can still drive into this cell from another angle
                    if (!closedHeadingsInThisCell.Contains(roundedHeading))
                    {
                        closedHeadingsInThisCell.Add(roundedHeading);
                    }
                    else if (startTrailer == null)
                    {
                        haveAlreadyClosedCell = true;
                    }


                    if (startTrailer != null)
                    {
                        int roundedHeadingTrailer = HelpStuff.RoundValue(nextNode.TrailerHeadingInDegrees, headingResolutionTrailer);

                        HashSet <int> closedTrailerHeadingsInThisCell = closedCellsTrailer[cellPos.x, cellPos.z];

                        if (!closedTrailerHeadingsInThisCell.Contains(roundedHeadingTrailer))
                        {
                            closedTrailerHeadingsInThisCell.Add(roundedHeadingTrailer);
                        }
                        else
                        {
                            haveAlreadyClosedCell = true;
                        }
                    }

                    //We have already expanded a better node with the same heading so dont expand this node
                    if (haveAlreadyClosedCell)
                    {
                        iterations -= 1;

                        continue;
                    }


                    //For debugging
                    allExpandedNodes.Add(nextNode);

                    //Check if the vehicle has reached the target
                    float distanceSqrToGoal = (nextNode.rearWheelPos - endCar.rearWheelPos).sqrMagnitude;

                    //But we also need to make sure the vehiclke has correct heading
                    float headingDifference = Mathf.Abs(endCar.HeadingInDegrees - nextNode.HeadingInDegrees);

                    //If we end up in the same cell or is within a certain distance from the goal
                    if ((distanceSqrToGoal < posAccuracy * posAccuracy || (cellPos.x == goalCellPos.x && cellPos.z == goalCellPos.z)) &&
                        headingDifference < headingAccuracy)
                    {
                        found = true;

                        Debug.Log("Found a path");

                        finalNode = nextNode;

                        //Make sure the end node has the same position as the target
                        finalNode.rearWheelPos.x = endCar.rearWheelPos.x;
                        finalNode.rearWheelPos.z = endCar.rearWheelPos.z;
                    }
                    //If we havent found the goal, then expand this node
                    else
                    {
                        //Get all child nodes
                        timeBefore = Environment.TickCount;

                        List <Node> children = GetChildrenToNode(nextNode, map, cellData, startCar.carData, endCar, startTrailer);

                        timer_findChildren += Environment.TickCount - timeBefore;


                        //Should we add any of the child nodes to the open nodes?
                        foreach (Node child in children)
                        {
                            IntVector2 childCell = map.ConvertWorldToCell(child.rearWheelPos);

                            int roundedChildHeading = HelpStuff.RoundValue(child.HeadingInDegrees, headingResolution);

                            //Has this cell been closed with this heading?
                            //If so, it means we already have a path at this cell with this heading,
                            //and the existing node is cheaper because we have already expanded it
                            if (closedCells[childCell.x, childCell.z].Contains(roundedChildHeading) && startTrailer == null)
                            {
                                prunedNodes += 1;

                                continue;
                            }
                            //If we have a trailer
                            else if (closedCells[childCell.x, childCell.z].Contains(roundedChildHeading) && startTrailer != null)
                            {
                                int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);

                                if (closedCellsTrailer[childCell.x, childCell.z].Contains(roundedTrailerHeading))
                                {
                                    prunedNodes += 1;

                                    continue;
                                }
                            }

                            //Have we already expanded a node with lower cost in this cell at this heading?
                            float costSoFar = child.gCost;

                            //The dictionary with lowest cost nodes in this cell
                            Dictionary <int, Node> nodesWithLowestCost = lowestCostNodes[childCell.x, childCell.z];

                            //Have we expanded with this angle to the cell before?
                            if (nodesWithLowestCost.ContainsKey(roundedChildHeading) && startTrailer == null)
                            {
                                //If the open node has a large gCost then we need to update that node with data
                                //from the child node
                                if (costSoFar < nodesWithLowestCost[roundedChildHeading].gCost)
                                {
                                    //If this child node is better then we should update the node in the open list
                                    //which is faster than deleting the old node and adding this child node
                                    Node existingNode = nodesWithLowestCost[roundedChildHeading];

                                    child.StealDataFromThisNode(existingNode);

                                    //Modify the heap-position of the node already in the open nodes
                                    openNodes.UpdateItem(existingNode);
                                }
                                //If the open node has a smaller gCost, then we dont need this child node, so do nothing

                                prunedNodes += 1;

                                continue;
                            }
                            //We have a trailer
                            else if (nodesWithLowestCost.ContainsKey(roundedChildHeading) && startTrailer != null)
                            {
                                //Have we expanded to this node before with this trailer heading
                                int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);

                                if (lowestCostNodesTrailer[childCell.x, childCell.z].Contains(roundedTrailerHeading))
                                {
                                    //If the open node has a large gCost then we need to update that node with data
                                    //from the child node
                                    if (costSoFar < nodesWithLowestCost[roundedChildHeading].gCost)
                                    {
                                        //If this child node is better then we should update the node in the open list
                                        //which is faster than deleting the old node and adding this child node
                                        Node existingNode = nodesWithLowestCost[roundedChildHeading];

                                        child.StealDataFromThisNode(existingNode);

                                        //Modify the heap-position of the node already in the open nodes
                                        openNodes.UpdateItem(existingNode);
                                    }
                                    //If the open node has a smaller gCost, then we dont need this child node, so do nothing

                                    prunedNodes += 1;

                                    continue;
                                }
                            }
                            else
                            {
                                //Add the node to the cell with this angle
                                nodesWithLowestCost[roundedChildHeading] = child;

                                if (startTrailer != null)
                                {
                                    int roundedTrailerHeading = HelpStuff.RoundValue(child.TrailerHeadingInDegrees, headingResolutionTrailer);

                                    lowestCostNodesTrailer[childCell.x, childCell.z].Add(roundedTrailerHeading);
                                }
                            }


                            //Dont add the node if its colliding with an obstacle or is outside of map
                            timeBefore = Environment.TickCount;
                            if (ObstaclesDetection.HasCarInvalidPosition(child.rearWheelPos, child.heading, startCar.carData, map))
                            {
                                prunedNodes += 1;

                                continue;
                            }
                            timer_isCollidingWithObstacle += Environment.TickCount - timeBefore;


                            //Trailer obstacle calculations
                            int startTrailerTimer = Environment.TickCount;

                            if (startTrailer != null)
                            {
                                //Now we need to check if this new position is valid by checking for collision with obstacles and the drag vehicle
                                //To do that we need the rear wheel pos of the trailer with this heading
                                //We know where the trailer is attached to the drag vehicle
                                Vector3 trailerAttachmentPoint = startCar.carData.GetTrailerAttachmentPoint(child.rearWheelPos, child.HeadingInRadians);

                                //Now we need the trailer's rear-wheel pos based on the new heading
                                Vector3 trailerRearWheelPos = startTrailer.carData.GetTrailerRearWheelPos(trailerAttachmentPoint, child.TrailerHeadingInRadians);

                                //Obstacle detection
                                //With the environment
                                if (ObstaclesDetection.HasCarInvalidPosition(trailerRearWheelPos, child.TrailerHeadingInRadians, startTrailer.carData, map))
                                {
                                    prunedNodes += 1;

                                    //Debug.Log("Semi trailer environment collision");

                                    continue;
                                }
                                //With the drag vehicle
                                if (ObstaclesDetection.IsTrailerCollidingWithDragVehicle(
                                        child.rearWheelPos, child.HeadingInRadians, startCar.carData,
                                        trailerRearWheelPos, child.TrailerHeadingInRadians, startTrailer.carData))
                                {
                                    prunedNodes += 1;

                                    //Debug.Log("Semi trailer collision");

                                    continue;
                                }
                            }
                            timer_TrailerCollision += Environment.TickCount - startTrailerTimer;


                            timeBefore = Environment.TickCount;

                            openNodes.Add(child);

                            timer_addNodeToHeap += Environment.TickCount - timeBefore;
                        }
                    }
                }
            }


            //Generate the final path when Hybrid A* has found the goal
            List <Node> finalPath = GenerateFinalPath(finalNode);


            //Display how long time everything took
            string display = DisplayController.GetDisplayTimeText(timer_selectLowestCostNode, "Select lowest cost node");

            display += DisplayController.GetDisplayTimeText(timer_addNodeToHeap, "Add new node to heap");

            display += DisplayController.GetDisplayTimeText(timer_findChildren, "Find children");

            display += DisplayController.GetDisplayTimeText(timer_isCollidingWithObstacle, "Is node colliding");

            display += DisplayController.GetDisplayTimeText(timer_ReedsSheppNode, "Reeds-Shepp Node");

            display += DisplayController.GetDisplayTimeText(timer_ReedsSheppHeuristics, "Reeds-Shepp Heuristics");

            display += DisplayController.GetDisplayTimeText(timer_TrailerCollision, "Trailer collision");

            display += DisplayController.GetDisplayText("Max nodes in heap", maxNodesInHeap, ". ");

            display += DisplayController.GetDisplayText("Expanded nodes", allExpandedNodes.Count, ". ");

            display += DisplayController.GetDisplayText("Pruned nodes", prunedNodes, null);

            Debug.Log(display);


            //Display car positions along the final path
            DisplayController.DisplayVehicleAlongPath(finalPath, startCar.carData, startTrailer);


            return(finalPath);
        }