void UVehiclePathFollowingComponent::FollowPathSegment(float DeltaTime)
{
if (MovementComp)
{
UWheeledVehicleMovementComponent* VehicleMoveComp =
Cast<UWheeledVehicleMovementComponent>(MovementComp);
AWheeledVehicle* Owner = Cast<AWheeledVehicle>(VehicleMoveComp->GetOwner());
if (Owner && VehicleMoveComp)
{
if(DesiredVehicleSpeedInKMH == 0.0f)
VehicleMoveComp->SetHandbrakeInput(true);
else
VehicleMoveComp->SetHandbrakeInput(false);
FVector VehicleLocation = Owner->GetActorLocation();
FVector Destination = GetCurrentTargetLocation();
FVector DirectionToDestination = Destination - VehicleLocation; //Vector pointing to current destination (not necessarily the final destination)
//____STEERING____
float DesiredSteering = 0.f;
float DeltaYaw = (DirectionToDestination.ToOrientationRotator() - Owner->GetActorForwardVector().ToOrientationRotator()).Yaw;
bool DestinationToRight = DeltaYaw >= 0;
if (DeltaYaw > 180.f || DeltaYaw < -180.f) {
DestinationToRight = !DestinationToRight;
if (DestinationToRight && DeltaYaw >= -360.f + MaxSteeringAngle)
{
DesiredSteering = 1.f;
}
else if (!DestinationToRight && DeltaYaw <= 360.f - MaxSteeringAngle)
{
DesiredSteering = -1.f;
}
else if(DestinationToRight)
{
DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(-360.f, -360.f + MaxSteeringAngle), FVector2D(0, 1.0f), DeltaYaw);
}
else
{
DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(360.f - MaxSteeringAngle, 360.f), FVector2D(-1.f, 0.f), DeltaYaw);
}
}
else
{
if (DestinationToRight && DeltaYaw >= MaxSteeringAngle)
{
DesiredSteering = 1.f;
}
else if (!DestinationToRight && DeltaYaw <= -MaxSteeringAngle)
{
DesiredSteering = -1.f;
}
else
DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(-MaxSteeringAngle, MaxSteeringAngle), FVector2D(-1.f, 1.f), DeltaYaw);
}
CurrentSteering = DesiredSteering;
VehicleMoveComp->SetSteeringInput(CurrentSteering);
//____END STEERING____
//____THROTTLE____
float CurrentSpeedInKMH = VehicleMoveComp->GetForwardSpeed() * 0.036f; //Convert to KM/H
//TODO: Allow speed to be 0, but never 0 if DesiredSpeed is > 0
float EstiamtedDesiredSpeed = DesiredVehicleSpeedInKMH - (FMath::Abs(CurrentSteering) * DesiredVehicleSpeedInKMH * 0.8);
CurrentThrottle += UPIDController::NextValue(VelocityController, EstiamtedDesiredSpeed - CurrentSpeedInKMH, DeltaTime);
CurrentThrottle = FMath::Clamp(CurrentThrottle, 0.f, 1.f);
VehicleMoveComp->SetThrottleInput(CurrentThrottle);
//____END THROTTLE____
//Direction the wheel is facing
DesiredMoveDirection = Owner->GetActorForwardVector().ToOrientationRotator().Add(0.f, FMath::GetMappedRangeValueClamped(FVector2D(-1.f, 1.f), FVector2D(-90.f, 90.f), CurrentSteering), 0.f).Vector();
ABikeV3Pawn* BikePawn = Cast<ABikeV3Pawn>(Owner);
if(BikePawn)
BikePawn->CurrentHandlingInput = CurrentSteering;
#ifdef NDEBUG
float DistanceToDestination = DirectionToDestination.Size();
DrawDebugPoint(GetWorld(), Destination, 50.0f, FColor::Blue);
DrawDebugLine(GetWorld(), VehicleLocation, VehicleLocation + DirectionToDestination, FColor::Yellow);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(0, 1.0f, FColor::Cyan, FString::Printf(TEXT("Throttle %0.3f Steering: %0.3f"), CurrentThrottle, CurrentSteering));
GEngine->AddOnScreenDebugMessage(1, 1.0f, FColor::Cyan, FString::Printf(TEXT("Distance to destination: %0.2f"), DistanceToDestination));
GEngine->AddOnScreenDebugMessage(2, 1.0f, FColor::Cyan, FString::Printf(TEXT("Current Speed in KM/H: %0.2f"), CurrentSpeedInKMH));
GEngine->AddOnScreenDebugMessage(3, 1.0f, FColor::Cyan, FString::Printf(TEXT("DeltaYaw %0.3f"), DeltaYaw));
}
#endif
}
}
}
void UVehiclePathFollowingComponent::FollowPathSegment(float DeltaTime)
{
if (MovementComp)
{
UWheeledVehicleMovementComponent* VehicleMoveComp =
Cast<UWheeledVehicleMovementComponent>(MovementComp);
AWheeledVehicle* Owner = Cast<AWheeledVehicle>(VehicleMoveComp->GetOwner());
if (Owner && VehicleMoveComp)
{
FVector VehicleLocation = Owner->GetActorLocation();
FVector Destination = GetCurrentTargetLocation();
FVector DirectionToDestion = Destination - VehicleLocation;
float DistanceToDestination = DirectionToDestion.Size();
FRotator RotatorToDestination = FRotationMatrix::MakeFromX(DirectionToDestion.GetSafeNormal2D()).Rotator();
float DeltaYaw = (RotatorToDestination - Owner->GetActorRotation()).Yaw;
bool DestinationInFront = DeltaYaw - 70.0f <= EPSILON && DeltaYaw + 70.0f >= EPSILON;
float DesiredSteering = FMath::GetMappedRangeValueClamped(FVector2D(-180.0f, 180.0f), FVector2D(-1.0f, 1.0f), DeltaYaw);
if (!DestinationInFront && DistanceToDestination < 1e3) // reverse
DesiredSteering = -DesiredSteering;
VehicleMoveComp->SetHandbrakeInput(false);
// The DesiredSteering includes the sensed steering already
CurrentSteering += UPIDController::NextValue(HeadingController, DesiredSteering - CurrentSteering, DeltaTime);
VehicleMoveComp->SetSteeringInput(CurrentSteering);
float PercentDistanceLeft = DistanceToDestination / InitialDistanceToDestination;
float DesiredThrottle = 0.0f;
// #TODO Turn hardcoded values into variables
if (DestinationInFront)
DesiredThrottle = FMath::Clamp(PercentDistanceLeft, 0.35f, 0.8f);
else
DesiredThrottle = -0.5f; // default throttle when going in reverse
/* #TODO Fix Math:
Allow user to specify a maximum vehicle speed that the PID
controller should approach.
*/
CurrentThrottle += UPIDController::NextValue(VelocityController, DesiredThrottle - CurrentThrottle, DeltaTime);
VehicleMoveComp->SetThrottleInput(CurrentThrottle);
#ifdef NDEBUG
DrawDebugPoint(GetWorld(), Destination, 50.0f, FColor::Blue);
DrawDebugLine(GetWorld(), VehicleLocation, VehicleLocation + DirectionToDestion, FColor::Yellow);
if (GEngine)
{
GEngine->AddOnScreenDebugMessage(0, 1.0f, FColor::Cyan, FString::Printf(TEXT("Throttle %0.3f Steering: %0.3f"), CurrentThrottle, CurrentSteering));
GEngine->AddOnScreenDebugMessage(1, 1.0f, FColor::Cyan, FString::Printf(TEXT("Distance to destination: %0.2f"), DistanceToDestination));
}
#endif
}
}
}