//Check if the target car has a valid position
        private bool HasTargetCarValidPosition(Vector3 targetPos, float heading, CarData carData)
        {
            bool  hasValidPosition = false;
            float targetCarHeading = heading * Mathf.Deg2Rad;

            if (ObstaclesDetection.TargetPositionWithinTrack(targetPos, targetCarHeading, carData)) //(MATT)
            {
                hasValidPosition = true;
            }

            return(hasValidPosition);
        }
        //Expand one node
        private void ExpandNode(Node currentNode)
        {
            //To be able to expand we need the simulated car's heading and position
            float heading = currentNode.heading;

            //Save which cells are expanded so we can close them after we have expanded all directions
            expandedCellsForward.Clear();
            expandedCellsReverse.Clear();

            //Expand both forward and reverse
            for (int j = 0; j < driveDistances.Length; j++)
            {
                float driveDistance = driveDistances[j];

                //Expand by looping through all steering angles
                for (int i = 0; i < steeringAngles.Count; i++)
                {
                    //Steering angle
                    float alpha = steeringAngles[i];

                    //Turning angle
                    float beta = (driveDistance / carData.GetWheelBase()) * Mathf.Tan(alpha);

                    //Simulate the skeleton car
                    Vector3 newCarPos = SkeletonCar.CalculateNewPosition(heading, beta, driveDistance, currentNode.carPos);

                    float newHeading = SkeletonCar.CalculateNewHeading(heading, beta);

                    //Get the cell pos of the new position
                    IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(newCarPos);

                    //Detect if the car is colliding with obstacle or is outside of map
                    if (!ObstaclesDetection.TargetPositionWithinTrack(newCarPos, newHeading, carData))
                    //if (ObstaclesDetection.HasCarInvalidPosition(newCarPos, newHeading, carData)) (MATT)
                    {
                        continue;
                    }
                    //Is this node closed? Important this is after obstacle/outside of map detection or may be out of range
                    else if (
                        (driveDistance < 0f && closedCellsReverse[cellPos.x, cellPos.z]) ||
                        (driveDistance > 0f && closedCellsForward[cellPos.x, cellPos.z]))
                    {
                        continue;
                    }
                    //We can create a new node because this is a valid position
                    else
                    {
                        //
                        //Calculate costs
                        //

                        //The cost it took to get to this node
                        float cost = Mathf.Abs(driveDistance);

                        //Add cost for turning if we are not having the same steering angle as previously
                        if (alpha != currentNode.steeringAngle)
                        {
                            cost += turningCost;
                        }

                        //Add a cost if we are close to an obstacle, its better to drive around them than close to them
                        //We can use the flow map to check this

                        /*  (MATT)
                         * if (ObstaclesController.distanceToClosestObstacle[cellPos.x, cellPos.z] < 6)
                         * {
                         *  cost += obstacleCost;
                         * }
                         */

                        //Add cost for reversing
                        if (driveDistance < 0f)
                        {
                            cost += reverseCost;
                        }

                        //Add a cost if we are switching from reverse -> forward or the opposite
                        bool isReversing = driveDistance < 0f ? true : false;

                        if ((isReversing && !currentNode.isReversing) || (!isReversing && currentNode.isReversing))
                        {
                            cost += switchingDirectionOfMovementCost;
                        }

                        //The cost to reach this node
                        float g2 = currentNode.g + cost;

                        //Is this cost lower than it was?
                        if (
                            (driveDistance > 0f && g2 < lowestCostForward[cellPos.x, cellPos.z]) ||
                            (driveDistance < 0f && g2 < lowestCostReverse[cellPos.x, cellPos.z]))
                        {
                            //We have found a better path
                            if (driveDistance > 0f)
                            {
                                //lowestCostForward[cellPos.x, cellPos.z] = g2;
                                expandedCellsForward.Add(new ExpandedCellsStorage(cellPos, g2));
                            }
                            if (driveDistance < 0f)
                            {
                                //lowestCostReverse[cellPos.x, cellPos.z] = g2;
                                expandedCellsReverse.Add(new ExpandedCellsStorage(cellPos, g2));
                            }

                            //
                            //Create a new node
                            //
                            Node nextNode = new Node();

                            nextNode.g            = g2;
                            nextNode.h            = HeuristicsController.heuristics[cellPos.x, cellPos.z];
                            nextNode.cellPos      = cellPos;
                            nextNode.previousNode = currentNode;

                            //Add the car data to the node
                            nextNode.carPos        = newCarPos;
                            nextNode.heading       = newHeading;
                            nextNode.steeringAngle = steeringAngles[i];
                            //Are we reversing?
                            nextNode.isReversing = driveDistance < 0f ? true : false;

                            //Add the node to the list with open nodes
                            openNodes.Add(nextNode);
                        }
                    }
                }
            }


            //Close all cells we expanded to from this node so we cant reach them again from another node
            if (expandedCellsForward.Count > 0)
            {
                for (int k = 0; k < expandedCellsForward.Count; k++)
                {
                    //closedArrayForward[expandedCellsForward[k].x, expandedCellsForward[k].z] = true;

                    IntVector2 cellPos = expandedCellsForward[k].cellPos;

                    if (expandedCellsForward[k].cost < lowestCostForward[cellPos.x, cellPos.z])
                    {
                        lowestCostForward[cellPos.x, cellPos.z] = expandedCellsForward[k].cost;
                    }
                }
            }
            if (expandedCellsReverse.Count > 0)
            {
                for (int k = 0; k < expandedCellsReverse.Count; k++)
                {
                    //closedArrayReverse[expandedCellsReverse[k].x, expandedCellsReverse[k].z] = true;

                    IntVector2 cellPos = expandedCellsReverse[k].cellPos;

                    if (expandedCellsReverse[k].cost < lowestCostReverse[cellPos.x, cellPos.z])
                    {
                        lowestCostReverse[cellPos.x, cellPos.z] = expandedCellsReverse[k].cost;
                    }
                }
            }
        }
예제 #3
0
        //Expand one node
        private void ExpandNode(Node currentNode)
        {
            //To be able to expand we need the simulated car's heading and position
            float heading = currentNode.heading;

            //Expand both forward and reverse
            for (int j = 0; j < driveDistances.Length; j++)
            {
                float driveDistance = driveDistances[j];

                //Expand by looping through all steering angles
                for (int i = 0; i < steeringAngles.Count; i++)
                {
                    //Steering angle
                    float alpha = steeringAngles[i];

                    //Turning angle
                    float beta = (driveDistance / carData.GetWheelBase()) * Mathf.Tan(alpha);

                    //Simulate the skeleton car
                    Vector3 newCarPos = SkeletonCar.CalculateNewPosition(heading, beta, driveDistance, currentNode.carPos);

                    float newHeading = SkeletonCar.CalculateNewHeading(heading, beta);

                    //Get the cell pos of the new position
                    IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(newCarPos);


                    //
                    //Check if the car is colliding with obstacle or is outside of map
                    //
                    if (!ObstaclesDetection.TargetPositionWithinTrack(newCarPos, newHeading, carData))
                    //if (ObstaclesDetection.HasCarInvalidPosition(newCarPos, newHeading, carData)) (MATT)
                    {
                        continue;
                    }


                    //
                    //Check if this node is closed
                    //
                    //Important this is after obstacle/outside of map detection or may be out of range
                    int roundedAngle = RoundAngle(newHeading * Mathf.Rad2Deg);

                    if (closedCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
                    {
                        continue;
                    }


                    //
                    //Check if this node is cheaper than any other node by calculating costs
                    //
                    //First create a new node with all data we need to calculate costs
                    Node nextNode = new Node();

                    nextNode.cellPos       = cellPos;
                    nextNode.carPos        = newCarPos;
                    nextNode.heading       = newHeading;
                    nextNode.steeringAngle = steeringAngles[i];
                    nextNode.isReversing   = driveDistance < 0f ? true : false;
                    nextNode.h             = HeuristicsController.heuristics[cellPos.x, cellPos.z];
                    nextNode.previousNode  = currentNode;

                    //Now we can calculate the cost to reach this node
                    nextNode.g = CalculateCosts(nextNode);


                    //Is this cost lower than it was or have we not expanded to this this cell with this angle?
                    if (
                        ((lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle) && nextNode.g < lowestCostCells[cellPos.x, cellPos.z][roundedAngle])) ||
                        !lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))

                    {
                        //We havent expanded to this node before with this angle
                        if (!lowestCostCells[cellPos.x, cellPos.z].ContainsKey(roundedAngle))
                        {
                            lowestCostCells[cellPos.x, cellPos.z].Add(roundedAngle, nextNode.g);
                        }
                        //The costs is lower than a previous expansion
                        else
                        {
                            lowestCostCells[cellPos.x, cellPos.z][roundedAngle] = nextNode.g;

                            //Now we should remove the old node from the heap
                            //Actually not needed because it's costly to remove nodes from the heap
                            //So its better to just skip the node when finding nodes with lowest cost
                        }

                        //Add the node to the list with open nodes
                        openNodes.Add(nextNode);
                    }
                }
            }
        }