void UVehiclePathFollowingComponent::SetMoveSegment(int32 SegmentStartIndex)
{
    UPathFollowingComponent::SetMoveSegment(SegmentStartIndex);

    AWheeledVehicle* Owner = Cast<AWheeledVehicle>(MovementComp->GetOwner());

    if (Owner)
    {
        InitialDistanceToDestination = (Owner->GetActorLocation() - GetCurrentTargetLocation()).Size();
    }
}
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

        }
    }
}
コード例 #3
0
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

        }
    }
}