TeleportationComponent.cpp

// Fill out your copyright notice in the Description page of Project Settings.


#include "Pawn/Navigation/TeleportationComponent.h"

#include "EnhancedInputComponent.h"
#include "NavigationSystem.h"
#include "Engine/LocalPlayer.h"
#include "GameFramework/PlayerController.h"
#include "NiagaraFunctionLibrary.h"
#include "Kismet/GameplayStatics.h"
#include "NiagaraDataInterfaceArrayFunctionLibrary.h"
#include "Utility/RWTHVRUtilities.h"
#include "MotionControllerComponent.h"
#include "Camera/CameraComponent.h"
#include "Kismet/KismetMathLibrary.h"

void UTeleportationComponent::SetupPlayerInput(UInputComponent* PlayerInputComponent)
{
	Super::SetupPlayerInput(PlayerInputComponent);

	if (!VRPawn || !VRPawn->HasLocalNetOwner())
	{
		return;
	}

	TeleportTraceComponent = UNiagaraFunctionLibrary::SpawnSystemAtLocation(
		GetWorld(), TeleportTraceSystem, VRPawn->GetActorLocation(), FRotator(0), FVector(1), true, true,
		ENCPoolMethod::AutoRelease, true);

	// simple way of changing the handedness
	if (bMoveWithRightHand)
	{
		TeleportationHand = VRPawn->RightHand;
		RotationHand = VRPawn->LeftHand;
	}
	else
	{
		TeleportationHand = VRPawn->LeftHand;
		RotationHand = VRPawn->RightHand;
	}

	UEnhancedInputComponent* EI = Cast<UEnhancedInputComponent>(PlayerInputComponent);
	if (!EI)
	{
		UE_LOG(Toolkit, Error, TEXT("Cannot cast Input Component to Enhanced Inpu Component in VRPawnMovement"));
		return;
	}

	// teleporting
	EI->BindAction(Teleport, ETriggerEvent::Started, this, &UTeleportationComponent::OnStartTeleportTrace);
	EI->BindAction(Teleport, ETriggerEvent::Triggered, this, &UTeleportationComponent::UpdateTeleportTrace);
	EI->BindAction(Teleport, ETriggerEvent::Completed, this, &UTeleportationComponent::OnEndTeleportTrace);
	EI->BindAction(Teleport, ETriggerEvent::Canceled, this, &UTeleportationComponent::OnEndTeleportTrace);

	EI->BindAction(CancelTeleport,ETriggerEvent::Started, this, &UTeleportationComponent::OnCancelTeleport);

	// turning is defined in MovementComponentBase
}

void UTeleportationComponent::PlayHapticEffect(UHapticFeedbackEffect_Base* HapticEffect, float Intensity)
{
	Intensity = FMath::Clamp(Intensity, 0.f, 1.f);
	APlayerController* PlayerController = UGameplayStatics::GetPlayerController(GetWorld(), 0);
	PlayerController->PlayHapticEffect(HapticEffect, EControllerHand::Right,Intensity);
}

void UTeleportationComponent::BeginPlay()
{
	Super::BeginPlay();

	if (!BPTeleportVisualizer)
	{
		UE_LOG(Toolkit, Error,
			   TEXT("SetupPlayerInput: BPTeleportVisualizer must be set to an Actor class that can be spawned!"));
		return;
	}

	TeleportVisualizer =
		GetWorld()->SpawnActor<ATeleportVisualizer>(BPTeleportVisualizer, VRPawn->GetActorLocation(), VRPawn->GetActorRotation());
	
	TeleportTraceComponent->SetVisibility(false);
	TeleportVisualizer->SetActorHiddenInGame(true);
	TeleportVisualizer->Platform->SetHiddenInGame(false);
	FVector PawnBottomLocation = VRPawn->GetActorLocation();
	PawnBottomLocation.Z = VRPawn->GetActorLocation().Z;
	TeleportVisualizer->Platform->SetWorldLocation(PawnBottomLocation);
	
}

void UTeleportationComponent::CleanupTeleportVisualization()
{
	if (!VRPawn)
		return;
	// End Teleport Trace
	bTeleportTraceActive = false;
	TeleportTraceComponent->SetVisibility(false);
	TeleportVisualizer->SetActorHiddenInGame(true);
	TeleportVisualizer->HorizontalIndicator->SetHiddenInGame(true);
	TeleportVisualizer->VerticalIndicator->SetHiddenInGame(true);
	//TeleportVisualizer->PreTravelPlatform->SetWorldLocation(TeleportVisualizer->Platform->GetComponentLocation());
	bHasVerticalPhaseStarted=false;
	bValidTeleportLocation = false;
}

void UTeleportationComponent::UpdateUserPreview()
{
	FVector NewHMDPos = TeleportVisualizer->PreTravelPlatform->GetComponentLocation();
	NewHMDPos.Z += VRPawn->HeadCameraComponent->GetComponentLocation().Z - TeleportVisualizer->PreTravelPlatform->GetComponentLocation().Z; // works since the user stands on the platform (or at least on the same Z level)
	
	TeleportVisualizer->HMDPreview->SetRelativeLocation(FVector(0,0,NewHMDPos.Z));
	TeleportVisualizer->HMDPreview->SetWorldRotation(VRPawn->HeadCameraComponent->GetComponentRotation());
	
	// Set rotation of vertical indicator arrows to always face pawn
	FRotator RotationToPawn = UKismetMathLibrary::FindLookAtRotation(TeleportVisualizer->VerticalIndicator->GetComponentLocation(), VRPawn->GetActorLocation());
	// only rotate around Z axis
	RotationToPawn.Pitch = 0;
	RotationToPawn.Roll = 0;
	TeleportVisualizer->VerticalIndicator->SetRelativeRotation(RotationToPawn);
}

// On button press -> show teleport trace
void UTeleportationComponent::OnStartTeleportTrace(const FInputActionValue& Value)
{
	// Start Trace
	bTeleportTraceActive = true;
	TeleportTraceComponent->SetVisibility(true);
	TeleportVisualizer->SetActorHiddenInGame(false);
	
	// Reset cancel teleport functionality
	bTeleportCancelled = false; 
}

void UTeleportationComponent::OnCancelTeleport(const FInputActionValue& Value)
{
	CleanupTeleportVisualization();
	bTeleportCancelled = true;
}

void UTeleportationComponent::SetPreTravelInformationVisibility(bool bVisibility)
{
}

void UTeleportationComponent::SelectHorizontalLocation()
{
	const FVector StartPosition = TeleportationHand->GetComponentLocation();
	const FVector ForwardVector = TeleportationHand->GetForwardVector();

	TArray<AActor> ActorsToIgnore;

	FPredictProjectilePathParams PredictParams = FPredictProjectilePathParams(
		TeleportProjectileRadius, StartPosition, TeleportLaunchSpeed * ForwardVector, 5.0, ECC_WorldStatic);

	PredictParams.ActorsToIgnore.Add(VRPawn);
	PredictParams.ActorsToIgnore.Add(TeleportVisualizer);

	UGameplayStatics::PredictProjectilePath(GetWorld(), PredictParams, PredictResult);

	const FVector HitLocation = PredictResult.HitResult.Location;
	const bool bValidHit = PredictResult.HitResult.IsValidBlockingHit();
	// check if this is a valid location to move to

	FVector OutLocation;
	const bool bValidProjection = IsValidTeleportLocation(PredictResult.HitResult, OutLocation);

	if (bUseNavMesh)
	{
		SelectedHorizontalPosition = OutLocation;
		if (bValidTeleportLocation != bValidProjection)
		{
			bValidTeleportLocation = bValidProjection;
			TeleportVisualizer->SetActorHiddenInGame(!bValidTeleportLocation);
		}
	}
	else
	{
		if (bValidHit)
		{
			SelectedHorizontalPosition = HitLocation;
			// Don't sweep on first call to correctly initialize platform position
			if(bIsFirstHorizontalCall)
			{
				TeleportVisualizer->PreTravelPlatform->SetWorldLocation(SelectedHorizontalPosition, false); // teleport pre travel mesh to impact point
				bIsFirstHorizontalCall = false;
			}
		}
	}

	TArray<FVector> PathPoints;
	PathPoints.Add(StartPosition);
	for (FPredictProjectilePathPointData PData : PredictResult.PathData)
	{
		PathPoints.Add(PData.Location);
	}
	UNiagaraDataInterfaceArrayFunctionLibrary::SetNiagaraArrayVector(TeleportTraceComponent, FName("User.PointArray"),
																	 PathPoints);
}

void UTeleportationComponent::SelectVerticalLocation()
{
		float OpeningAngle = GetAngleBetweenVectors(ReferenceVector, VRPawn->RightHand->GetForwardVector());
	
    // don't change height if angle is below deadzone and the deadzone has never been left
    if(abs(OpeningAngle) <= Deadzone && !hasLeftDeadZone)
    {
        //SetVerticalActorColor(FLinearColor::Gray);
        PlayHapticEffect(TeleportVisualizer->HapticFeedback,0.1);
        return;
    }
    //SetVerticalActorColor(FLinearColor::Blue); // indicate that vertical selection is now active only if the deadzone has been left
	
    // if this is the first time the deadzone is left, play faint haptic cue to indicate that
    if (hasLeftDeadZone == false) 
    {
        PlayHapticEffect(TeleportVisualizer->HapticFeedback, 0.1); // 
        hasLeftDeadZone = true;// disable deadzone check if height change has begun
    }

    //UE_LOG(LogTemp, Warning, TEXT("OpeningAngle = %f"), OpeningAngle);
    FVector NewPos = SelectedHorizontalPosition;
    
    // "1:1" Movement if angle is below velocity zone
    if (abs(OpeningAngle) < VelocityZone)
    {
        float FixedOffset = OpeningAngle * ElevationSpeed;
    	AngularOffset = OpeningAngle * ElevationSpeed;

        NewPos.Z = NewPos.Z + AngularOffset + HeightOffset; // make movements correspond directly to controller angle
        //NewPos.Z += HeightOffset; // account for height changes made in the velocity zone
        TeleportVisualizer->PreTravelPlatform->SetWorldLocation(NewPos, true);
    }
    else // velocity-based movements if angle is within velocity zone
    {
        float Sign = (OpeningAngle < 0) ? -1 : 1; // determine the sign based on the opening angle

        float OvershootAngle = (abs(OpeningAngle) - VelocityZone) * Sign; // determines how far we are above/below the velocity zone

        /*float OldHeightOffset = HeightOffset;*/

        HeightOffset += OvershootAngle * VelocitySpeed;

        NewPos.Z += HeightOffset + AngularOffset;
        /*bool bWasMoved =*/ TeleportVisualizer->PreTravelPlatform->SetWorldLocation(NewPos, true); // change location and check whether movement occurred or if platform was hitting ground

        /*if(!bWasMoved)
        {
            HeightOffset = OldHeightOffset; //don't change height offset if the platform wasn't moved
        }*/
    }
}

float UTeleportationComponent::GetAngleBetweenVectors(FVector Vector1, FVector Vector2)
{
	AngleProjectionPlane = FPlane(VRPawn->RightHand->GetComponentLocation(), VRPawn->RightHand->GetComponentLocation() + ReferenceVector, VRPawn->RightHand->GetComponentLocation() + FVector(0, 0, 1));

	FVector MCLoc = VRPawn->RightHand->GetComponentLocation();

	Vector1 = Vector1.GetSafeNormal();
	Vector2 = Vector2.GetSafeNormal();

	// Mapping the vectors to a plane somehow helps with jittery behaviour ?!
	Vector1 = FVector::VectorPlaneProject(Vector1, AngleProjectionPlane.GetNormal());
	Vector2 = FVector::VectorPlaneProject(Vector2, AngleProjectionPlane.GetNormal());

	// Uncomment this to see the projection
	//DrawDebugSolidPlane(GetWorld(), AngleProjectionPlane, RightMC->GetComponentLocation(), 50, FColor::Magenta, false, 1);
	//DrawDebugDirectionalArrow(GetWorld(), MCLoc, MCLoc + Vector1 * 30, 5, FColor::Red, false, -1,1,2);
	//DrawDebugDirectionalArrow(GetWorld(), MCLoc, MCLoc + Vector2 * 30, 5, FColor::Green, false, -1,1,2);
    
	FVector CrossProduct = FVector::CrossProduct(Vector1,Vector2);

	//DrawDebugDirectionalArrow(GetWorld(), MCLoc, MCLoc + PlaneNormal * 30, 5, FColor::Cyan, false, -1, 1, 2);
    
	float OpeningAngle = FMath::Atan2(FVector::CrossProduct(Vector1, Vector2).Size(), FVector::DotProduct(Vector1, Vector2));
	OpeningAngle = FMath::RadiansToDegrees(OpeningAngle);
    
	// Take sign of cross product to determine if angle was opened upwards or downwards
	float Distance = AngleProjectionPlane.PlaneDot(MCLoc + CrossProduct * 30);
	return FMath::Sign(Distance) * OpeningAngle;
}

// called while button is pressed (Triggered)
void UTeleportationComponent::UpdateTeleportTrace(const FInputActionValue& Value)
{
	
	float AxisValue = Value.Get<float>();
	
	// If we have cancelled the teleport trace via a button press, return.
	// Only allow teleportation again if teleport button is fully let down and pressed again
	if(bTeleportCancelled)
	{
		return;
	}
	
	UpdateUserPreview();

	if(TeleportState == ETeleportState::BaseState)
	{
		SetPreTravelInformationVisibility(true);
	}

	if(TeleportState != ETeleportState::VerticalSelection)
	{
		if(AxisValue <= TwoStepThreshold)
		{
			TeleportState = ETeleportState::HorizontalSelection;
		}
		if(AxisValue > TwoStepThreshold && bHorizontalPhaseCompleted)
		{
			if(!bHasVerticalPhaseStarted)
			{
				PlayHapticEffect(TeleportVisualizer->HapticFeedback,1);
				bHasVerticalPhaseStarted = true;
			}

			TeleportState = ETeleportState::VerticalSelection;
			// store current forward vector of right hand for further computations in the height selection phase
			ReferenceVector = VRPawn->RightHand->GetForwardVector();
		}
	}

	switch (TeleportState)
	{
	case ETeleportState::BaseState:
		SetPreTravelInformationVisibility(true);
		TeleportState = ETeleportState::HorizontalSelection;
		break;

	case ETeleportState::HorizontalSelection:
		TeleportVisualizer->HorizontalIndicator->SetHiddenInGame(false);
		SelectHorizontalLocation();
		bHorizontalPhaseCompleted = true;
		break;

	case ETeleportState::VerticalSelection:
		// Update the teleport trace
			TeleportVisualizer->HorizontalIndicator->SetHiddenInGame(true);
		TeleportVisualizer->VerticalIndicator->SetHiddenInGame(false);
		SelectVerticalLocation();
		break;

	default:
		UE_LOG(LogTemp,Error,TEXT("TeleportationComponent: Invalid Teleport Phase"))
		break;
	}
	
}

bool UTeleportationComponent::IsValidTeleportLocation(const FHitResult& Hit, FVector& ProjectedLocation) const
{
	UNavigationSystemV1* NavSys = FNavigationSystem::GetCurrent<UNavigationSystemV1>(GetWorld());
	const FNavAgentProperties& AgentProps = FNavAgentProperties(15, 160);
	FNavLocation ProjectedNavLocation;
	const bool bProjectPoint =
		(NavSys &&
		 NavSys->ProjectPointToNavigation(Hit.Location, ProjectedNavLocation, INVALID_NAVEXTENT, &AgentProps));
	ProjectedLocation = ProjectedNavLocation.Location;
	return bProjectPoint /*&& Hit.IsValidBlockingHit()*/;
}

// On button release -> remove trace and teleport user to location
void UTeleportationComponent::OnEndTeleportTrace(const FInputActionValue& Value)
{
	TeleportState=ETeleportState::BaseState;
	HeightOffset=0; //reset height offset for next teleport
	hasLeftDeadZone=false;
	bIsFirstHorizontalCall=true;
	bHorizontalPhaseCompleted=false;
	
	// If teleport was cancelled via button press, do not teleport.
	// Allow teleportation again, if we let go and press the teleport button again.
	if(bTeleportCancelled)
	{
		return;
	}
	CleanupTeleportVisualization();
	VRPawn->TeleportTo(TeleportVisualizer->PreTravelPlatform->GetComponentLocation(), VRPawn->GetActorRotation());
}