private void UpdateNextIntersectionDirection(Driver.Sensors sensors) { // We need to be on a street and not having been on a street previously (as we would have already decided where to go). Street currentStreet = sensors.carTracker.street; if (currentStreet != null && previousStreet == null) { // Determine where to go at the end of this street. CardinalDirection currentDirection = sensors.carTracker.streetCardinalDirection; Intersection targetIntersection = GetTargetIntersection(sensors); // See where we might want to go. var potentialDirections = new List <CardinalDirection>(); CardinalDirection oppositeDirection = DirectionHelpers.GetOppositeDirection(currentDirection); foreach (CardinalDirection direction in DirectionHelpers.cardinalDirections) { // Don't try to go backwards by default. if (direction == oppositeDirection) { continue; } // Direction is possible if the intersection has this street. Street street = targetIntersection.GetStreetInDirection(direction); if (street != null) { // Make sure we're not entering into a one-way street from the wrong direction. if (street.isOneWay == false || !targetIntersection.HasStopLineForStreet(street)) { potentialDirections.Add(direction); } } } // Only if no options were there, go back. if (potentialDirections.Count == 0) { potentialDirections.Add(oppositeDirection); } // Choose a random option. int randomIndex = UnityEngine.Random.Range(0, potentialDirections.Count); // Determine exiting values. nextIntersectionExitingCardinalDirection = potentialDirections[randomIndex]; nextIntersectionExitingDirection = DirectionHelpers.cardinalDirectionVectors[nextIntersectionExitingCardinalDirection]; nextIntersectionExitingStreet = targetIntersection.GetStreetInDirection(nextIntersectionExitingCardinalDirection); // Figure out which lane to go into. float angleToTarget = Vector3.SignedAngle(sensors.carTracker.streetDirection, DirectionHelpers.cardinalDirectionVectors[nextIntersectionExitingCardinalDirection], Vector3.up); Street nextStreet = targetIntersection.GetStreetInDirection(nextIntersectionExitingCardinalDirection); if (nextIntersectionExitingCardinalDirection == currentDirection) { // Be in the middle (or right) lane when going straight. nextIntersectionExitingLane = nextStreet.validLanesCount / 2 + 1; nextIntersectionTurningIntent = Driver.TurningIntent.Straight; } else if (nextIntersectionExitingCardinalDirection == oppositeDirection) { // Turn into leftmost lane for U-turns. nextIntersectionExitingLane = 1; nextIntersectionTurningIntent = Driver.TurningIntent.UTurn; } else if (angleToTarget < 0) { // Be in leftmost lane after upcoming left turns. nextIntersectionExitingLane = 1; nextIntersectionTurningIntent = Driver.TurningIntent.Left; } else { // Be in the rightmost lane after upcoming right turns. nextIntersectionExitingLane = nextStreet.validLanesCount; nextIntersectionTurningIntent = Driver.TurningIntent.Right; } nextIntersectionExitingPoint = nextIntersectionExitingStreet.GetCenterOfLanePosition(nextIntersectionExitingLane, 0, nextIntersectionExitingCardinalDirection); // Store entering values. nextIntersectionEnteringCardinalDirection = currentDirection; nextIntersectionEnteringDirection = DirectionHelpers.cardinalDirectionVectors[currentDirection]; nextIntersectionEnteringStreet = currentStreet; // Figure out which lane to enter the intersection from. switch (nextIntersectionTurningIntent) { case Driver.TurningIntent.Straight: // Be in the middle (or right) lane when going straight. nextIntersectionEnteringLane = currentStreet.validLanesCount / 2 + 1; break; case Driver.TurningIntent.Left: // Be in leftmost lane for upcoming left turns. nextIntersectionEnteringLane = 1; break; case Driver.TurningIntent.Right: // Be in the rightmost lane for upcoming right turns. nextIntersectionEnteringLane = currentStreet.validLanesCount; break; case Driver.TurningIntent.UTurn: // Turn from the sidewalk for U-turns. nextIntersectionEnteringLane = 0; break; } nextIntersectionEnteringPoint = nextIntersectionEnteringStreet.GetCenterOfLanePosition(nextIntersectionEnteringLane, nextIntersectionEnteringStreet.length, nextIntersectionEnteringCardinalDirection); } // Update street. previousStreet = currentStreet; }
private void UpdateTargetLaneAndDirection(Driver.Sensors sensors, Driver.Actuators actuators) { Street currentStreet = sensors.carTracker.street; Intersection currentIntersection = sensors.carTracker.intersection; float distanceToIntersection = GetDistanceToTargetIntersection(sensors); float turningTargetDistance = Math.Max(turningTargetMinimumDistance, turningTargetTime * sensors.car.speed); // See if we should start performing a turn. bool performingTurn = false; switch (nextIntersectionTurningIntent) { case Driver.TurningIntent.Left: case Driver.TurningIntent.Right: // When turning left and right, start turning when close enough to the intersection. performingTurn = currentIntersection != null || distanceToIntersection < turningTargetDistance; break; case Driver.TurningIntent.UTurn: // When performing a U-turn, start in the intersection. performingTurn = currentIntersection != null; break; } if (performingTurn) { DrawDebugIntersectionArc(); switch (nextIntersectionTurningIntent) { case Driver.TurningIntent.Left: case Driver.TurningIntent.Right: // For right turns, follow an arc. float xEnter = Vector3.Dot(nextIntersectionExitingDirection, nextIntersectionEnteringPoint); float xExit = Vector3.Dot(nextIntersectionExitingDirection, nextIntersectionExitingPoint); float xDelta = xExit - xEnter; float zEnter = Vector3.Dot(nextIntersectionEnteringDirection, nextIntersectionEnteringPoint); float zExit = Vector3.Dot(nextIntersectionEnteringDirection, nextIntersectionExitingPoint); float zDelta = zExit - zEnter; // Place a target in front of the car. Vector3 carPosition = sensors.car.transform.position; Vector3 target = carPosition + sensors.car.transform.forward * turningTargetDistance; Debug.DrawLine(carPosition + Vector3.up * 2, target + Vector3.up * 2, Color.magenta); float xTarget = Vector3.Dot(nextIntersectionExitingDirection, target) - xEnter; float zTarget = Vector3.Dot(nextIntersectionEnteringDirection, target) - zEnter; // Move target onto the arc. float zTargetFraction = zTarget / zDelta; float xTargetFraction = xTarget / xDelta; float angle = Mathf.Atan2(zTargetFraction, 1 - xTargetFraction); xTargetFraction = 1 - Mathf.Cos(angle); zTargetFraction = xTargetFraction > 1 ? 1 : Mathf.Sin(angle); xTarget = xTargetFraction * xDelta; zTarget = zTargetFraction * zDelta; target = nextIntersectionEnteringPoint + nextIntersectionExitingDirection * xTarget + nextIntersectionEnteringDirection * zTarget; // Direct the car towards the adjusted target. actuators.targetDirection = (target - carPosition).normalized; Debug.DrawLine(carPosition + Vector3.up * 2, target + Vector3.up * 2, Color.blue); break; case Driver.TurningIntent.UTurn: // For U-turns, start by turning right as tight as possible. if (Vector3.Angle(nextIntersectionEnteringDirection, sensors.car.transform.forward) < 45) { actuators.targetDirection = Quaternion.AngleAxis(90, Vector3.up) * nextIntersectionEnteringDirection; } else { actuators.targetDirection = nextIntersectionExitingDirection; } break; } actuators.targetLane = nextIntersectionExitingLane; } else if (currentStreet != null) { // If we're on a street, go to correct turning lane. float angleToTarget = Vector3.SignedAngle(sensors.carTracker.streetDirection, DirectionHelpers.cardinalDirectionVectors[nextIntersectionExitingCardinalDirection], Vector3.up); CardinalDirection currentDirection = sensors.carTracker.streetCardinalDirection; CardinalDirection oppositeDirection = DirectionHelpers.GetOppositeDirection(currentDirection); int currentLane = sensors.carTracker.currentLane; int desiredLane = nextIntersectionEnteringLane; if (desiredLane != currentLane) { // Start moving to the desired lane when close enough to the intersection. int laneDifference = desiredLane - currentLane; float minimumDistanceToIntersection = sensors.driverProfile.distanceForChangingLane * Mathf.Abs(laneDifference); // For U-turns from the sidewalk, only drive onto the sidewalk in the last meters. if (nextIntersectionTurningIntent == Driver.TurningIntent.UTurn && currentLane == 1) { minimumDistanceToIntersection = uTurnDriveToSidewalkDistance; } if (distanceToIntersection < minimumDistanceToIntersection) { actuators.turningIntent = nextIntersectionTurningIntent; // Make sure the neighbor lane is free in the safe distance region before and after the car. int neighborLane = currentLane + Math.Sign(laneDifference); float safeDistance = CalculateSafeDistance(sensors); Vector3 streetDirection = sensors.carTracker.streetDirection; float carLength = sensors.car.bounds.size.z; Vector3 origin = sensors.carTracker.GetCenterOfLanePosition(neighborLane) + (safeDistance + carLength / 2) * streetDirection + Vector3.up * sensors.car.bounds.extents.y; float checkDistance = 2 * safeDistance + carLength; if (!Physics.Raycast(origin, -streetDirection, checkDistance)) { actuators.targetLane = desiredLane; Debug.DrawRay(origin, -streetDirection * checkDistance, Color.green, 1); } else { Debug.DrawRay(origin, -streetDirection * checkDistance, Color.red); } } } actuators.targetDirection = sensors.carTracker.streetDirection; } }