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;
}
}
