コード例 #1
0
        //We generated this path by using the rear-wheel position. To easier be able to make the vehicle follow the path
        //we should translate the path to the front axle, by using the the distance between the front- and rear axle
        //We might also want the mirrored version of the front axle along the rear axle, to get a path we can use when reversing
        public static void CalculateFrontAxlePositions(List <Node> path, CarData carData, Vector3 vehicleStartDir, Vector3 vehicleEndDir, bool isMirrored)
        {
            //Move the waypoints in the heading direction with a distance
            //This distance is the same as the distance between the front axle and rear axle (= wheel base)
            float moveDistance = carData.WheelBase;

            for (int i = 0; i < path.Count; i++)
            {
                Vector3 dir_before, dir_after;

                if (i == 0)
                {
                    dir_before = vehicleStartDir;
                }
                else
                {
                    dir_before = (path[i].rearWheelPos - path[i - 1].rearWheelPos).normalized;

                    if (path[i].isReversing)
                    {
                        dir_before *= -1f;
                    }
                }

                if (i == path.Count - 1)
                {
                    dir_after = vehicleEndDir;
                }
                else
                {
                    dir_after = (path[i + 1].rearWheelPos - path[i].rearWheelPos).normalized;

                    if (path[i + 1].isReversing)
                    {
                        dir_after *= -1f;
                    }
                }

                //Vector3 dir = (dir_before + dir_after) * 0.5f;

                Vector3 dir = dir_after;

                if (isMirrored)
                {
                    dir *= -1f;
                }

                if (!isMirrored)
                {
                    path[i].frontWheelPos = path[i].rearWheelPos + dir * moveDistance;
                }
                else
                {
                    path[i].reverseWheelPos = path[i].rearWheelPos + dir * moveDistance;
                }

                //path[i].frontWheelPos = path[i].rearWheelPos;
            }
        }
コード例 #2
0
        public Car(Transform carTrans, SelfDrivingVehicle.VehicleDataController carDataController)
        {
            this.carData = carDataController.carData;

            this.rearWheelPos = carDataController.RearWheelPos(carTrans);

            this.heading = carDataController.HeadingInRadians(carTrans);
        }
コード例 #3
0
        //
        // Test if one path is drivable
        //
        public static bool IsPathDrivable(List <Node> path, CarData carData, Map map)
        {
            //Ignore the first node because we know its drivable
            for (int i = 1; i < path.Count; i++)
            {
                Vector3 carPos = path[i].rearWheelPos;

                float carHeading = path[i].heading;

                if (HasCarInvalidPosition(carPos, carHeading, carData, map))
                {
                    //This path is not drivable
                    return(false);
                }
            }

            //This path is drivable
            return(true);
        }
コード例 #4
0
        //
        // Check if the trailer is colliding with the drag vehicle
        //
        public static bool IsTrailerCollidingWithDragVehicle(
            Vector3 semiRearWheelPos, float semiHeading, CarData semiData,
            Vector3 trailerRearWheelPos, float trailerHeading, CarData trailerData)
        {
            bool isColliding = false;

            //Use triangle-traingle intersection so we need the rectangles
            Vector3 trailerCenter = trailerData.GetCenterPos(trailerRearWheelPos, trailerHeading);

            Rectangle trailerRect = CarData.GetCornerPositions(trailerCenter, trailerHeading, trailerData.carWidth * 0.9f, trailerData.CarLength);

            //The semi's cabin rectangle
            Vector3 cabinCenter = semiData.GetSemiCabinCenter(semiRearWheelPos, semiHeading);

            //Make it slightly shorter or too many false collisions
            Rectangle semiRect = CarData.GetCornerPositions(cabinCenter, semiHeading, semiData.carWidth, semiData.cabinLength * 0.95f);

            if (Intersections.AreRectangleRectangleIntersecting(trailerRect, semiRect))
            {
                return(true);
            }

            return(isColliding);
        }
コード例 #5
0
 public Car(Vector3 rearWheelPos, float heading, CarData carData)
 {
     this.rearWheelPos = rearWheelPos;
     this.heading      = heading;
     this.carData      = carData;
 }
コード例 #6
0
        //
        // Calculate heuristics
        //
        private static float HeuristicsToReachGoal(Cell[,] cellData, IntVector2 cellPos, Node node, Car endCar, CarData carData)
        {
            float heuristics = cellData[cellPos.x, cellPos.z].heuristics;

            //But if we are close we might want to use the Reeds-Shepp distance as heuristics
            //This distance can be pre-calculated
            if (cellData[cellPos.x, cellPos.z].distanceToTarget < 20f)
            {
                int timeBefore = Environment.TickCount;

                float RS_distance = ReedsShepp.GetShortestDistance(
                    node.rearWheelPos,
                    node.heading,
                    endCar.rearWheelPos,
                    endCar.HeadingInRadians,
                    carData.turningRadius);

                timer_ReedsSheppHeuristics += Environment.TickCount - timeBefore;

                //Should use the max value according to the Junior report
                if (RS_distance > heuristics)
                {
                    heuristics = RS_distance;

                    //Debug.Log("Added Reeds-Shepp heuristics");
                }
            }

            return(heuristics);
        }
コード例 #7
0
        //
        // 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);
        }
コード例 #8
0
        //
        // Check if the car is colliding with an obstacle or is outside of map
        //
        public static bool HasCarInvalidPosition(Vector3 carRearWheelPos, float heading, CarData carData, Map map)
        {
            bool hasInvalidPosition = false;


            //Step 1. Check if the car's rear wheel center is inside of the map
            IntVector2 rearWheelCellPos = map.ConvertWorldToCell(carRearWheelPos);

            if (!map.IsCellWithinGrid(rearWheelCellPos))
            {
                //This is not a valid position
                hasInvalidPosition = true;

                return(hasInvalidPosition);
            }


            //Step 2. Check if any of the car's corner is outside of the map

            //Make the car bigger than it is to be on the safe side
            float carLength = carData.CarLength + Parameters.marginOfSafety;
            float carWidth  = carData.carWidth + Parameters.marginOfSafety;

            Vector3 carCenterPos = carData.GetCenterPos(carRearWheelPos, heading);

            //Find all corners of the car
            Rectangle corners = CarData.GetCornerPositions(carCenterPos, heading, carWidth, carLength);

            //Detect if any of the corners is outside of the map = is the cell the corner is a part of the map
            HashSet <IntVector2> carCellPositions = new HashSet <IntVector2>();

            carCellPositions.Add(map.ConvertWorldToCell(corners.FL));
            carCellPositions.Add(map.ConvertWorldToCell(corners.FR));
            carCellPositions.Add(map.ConvertWorldToCell(corners.BL));
            carCellPositions.Add(map.ConvertWorldToCell(corners.BR));

            foreach (IntVector2 cellPos in carCellPositions)
            {
                if (!map.IsCellWithinGrid(cellPos))
                {
                    //At least one of the corners is outside of the map
                    hasInvalidPosition = true;

                    return(hasInvalidPosition);
                }
            }


            //Step 3. Check if some of the car's known positions are far away from an obstacle

            //The car is not colliding with anything if the steps to an obstacle from center of car is greater than the length of the car
            IntVector2 carCenterCellPos = map.ConvertWorldToCell(carCenterPos);

            if (map.cellData[carCenterCellPos.x, carCenterCellPos.z].distanceToClosestObstacle > carData.CarLength * 0.7f)
            {
                //This is a valid position
                hasInvalidPosition = false;

                return(hasInvalidPosition);
            }


            //Step 4. Check if the car is hitting an obstacle

            //Use the car's corners and then rectangle-rectangle-intersection with the obstacles
            hasInvalidPosition = IsCarIntersectingWithObstacles(corners, carCenterCellPos, map);



            return(hasInvalidPosition);
        }