void ADynamicWeather::AdjustSunPositionBasedOnActorRotation()
{
const FVector Direction = - GetActorQuat().GetForwardVector();
const FVector2D SphericalCoords = Direction.UnitCartesianToSpherical();
Weather.SunPolarAngle = FMath::RadiansToDegrees(SphericalCoords.X);
Weather.SunAzimuthAngle = FMath::RadiansToDegrees(SphericalCoords.Y);
}
float AWheeledVehicleAIController::GoToNextTargetLocation(FVector &Direction)
{
const auto &CurrentLocation = Vehicle->GetActorLocation();
const auto Target = [&](){
const auto &Result = TargetLocations.front();
return FVector{Result.X, Result.Y, CurrentLocation.Z};
}();
if (Target.Equals(CurrentLocation, 80.0f)) {
TargetLocations.pop();
if (!TargetLocations.empty()) {
return GoToNextTargetLocation(Direction);
} else {
return CalcStreeringValue(Direction);
}
}
Direction = (Target - CurrentLocation).GetSafeNormal();
const FVector &Forward = GetPawn()->GetActorForwardVector();
float dirAngle = Direction.UnitCartesianToSpherical().Y;
float actorAngle = Forward.UnitCartesianToSpherical().Y;
dirAngle *= (180.0f / PI);
actorAngle *= (180.0 / PI);
float angle = dirAngle - actorAngle;
if (angle > 180.0f) { angle -= 360.0f;} else if (angle < -180.0f) {
angle += 360.0f;
}
float Steering = 0.0f;
if (angle < -MaximumSteerAngle) {
Steering = -1.0f;
} else if (angle > MaximumSteerAngle) {
Steering = 1.0f;
} else {
Steering += angle / MaximumSteerAngle;
}
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::FollowingFixedRoute);
return Steering;
}
float AWheeledVehicleAIController::CalcStreeringValue(FVector &direction)
{
float steering = 0;
FVector BoxExtent = Vehicle->GetVehicleBoundsExtent();
FVector forward = Vehicle->GetActorForwardVector();
FVector rightSensorPosition(BoxExtent.X / 2.0f, (BoxExtent.Y / 2.0f) + 100.0f, 0.0f);
FVector leftSensorPosition(BoxExtent.X / 2.0f, -(BoxExtent.Y / 2.0f) - 100.0f, 0.0f);
float forwardMagnitude = BoxExtent.X / 2.0f;
float Magnitude = (float) sqrt(pow((double) leftSensorPosition.X, 2.0) + pow((double) leftSensorPosition.Y, 2.0));
//same for the right and left
float offset = FGenericPlatformMath::Acos(forwardMagnitude / Magnitude);
float actorAngle = forward.UnitCartesianToSpherical().Y;
float sinR = FGenericPlatformMath::Sin(actorAngle + offset);
float cosR = FGenericPlatformMath::Cos(actorAngle + offset);
float sinL = FGenericPlatformMath::Sin(actorAngle - offset);
float cosL = FGenericPlatformMath::Cos(actorAngle - offset);
rightSensorPosition.Y = sinR * Magnitude;
rightSensorPosition.X = cosR * Magnitude;
leftSensorPosition.Y = sinL * Magnitude;
leftSensorPosition.X = cosL * Magnitude;
FVector rightPositon = GetPawn()->GetActorLocation() + FVector(rightSensorPosition.X, rightSensorPosition.Y, 0.0f);
FVector leftPosition = GetPawn()->GetActorLocation() + FVector(leftSensorPosition.X, leftSensorPosition.Y, 0.0f);
FRoadMapPixelData rightRoadData = RoadMap->GetDataAt(rightPositon);
if (!rightRoadData.IsRoad()) { steering -= 0.2f;}
FRoadMapPixelData leftRoadData = RoadMap->GetDataAt(leftPosition);
if (!leftRoadData.IsRoad()) { steering += 0.2f;}
FRoadMapPixelData roadData = RoadMap->GetDataAt(GetPawn()->GetActorLocation());
if (!roadData.IsRoad()) {
steering = -1;
} else if (roadData.HasDirection()) {
direction = roadData.GetDirection();
FVector right = rightRoadData.GetDirection();
FVector left = leftRoadData.GetDirection();
forward.Z = 0.0f;
float dirAngle = direction.UnitCartesianToSpherical().Y;
float rightAngle = right.UnitCartesianToSpherical().Y;
float leftAngle = left.UnitCartesianToSpherical().Y;
dirAngle *= (180.0f / PI);
rightAngle *= (180.0 / PI);
leftAngle *= (180.0 / PI);
actorAngle *= (180.0 / PI);
float min = dirAngle - 90.0f;
if (min < -180.0f) { min = 180.0f + (min + 180.0f);}
float max = dirAngle + 90.0f;
if (max > 180.0f) { max = -180.0f + (max - 180.0f);}
if (dirAngle < -90.0 || dirAngle > 90.0) {
if (rightAngle < min && rightAngle > max) { steering -= 0.2f;}
if (leftAngle < min && leftAngle > max) { steering += 0.2f;}
} else {
if (rightAngle < min || rightAngle > max) { steering -= 0.2f;}
if (leftAngle < min || leftAngle > max) { steering += 0.2f;}
}
float angle = dirAngle - actorAngle;
if (angle > 180.0f) { angle -= 360.0f;} else if (angle < -180.0f) {
angle += 360.0f;
}
if (angle < -MaximumSteerAngle) {
steering = -1.0f;
} else if (angle > MaximumSteerAngle) {
steering = 1.0f;
} else {
steering += angle / MaximumSteerAngle;
}
}
Vehicle->SetAIVehicleState(ECarlaWheeledVehicleState::FreeDriving);
return steering;
}
