Draft: Improve walking implementation

I reworked the walking movement to use capsule colliders everywhere to avoid falling thorugh things etc.

Still needs testing in the CAVE

Source/RWTHVRToolkit/Private/Pawn/VRPawnMovement.cpp

-
-#include "Pawn/VRPawnMovement.h"
-#include "DrawDebugHelpers.h"
-
-UVRPawnMovement::UVRPawnMovement(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
-{
-	CapsuleColliderComponent = CreateDefaultSubobject<UCapsuleComponent>(TEXT("CapsuleCollider"));
-	CapsuleColliderComponent->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics);
-	CapsuleColliderComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Overlap);
-	CapsuleColliderComponent->SetCollisionResponseToChannel(ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
-	CapsuleColliderComponent->SetCapsuleSize(CapsuleRadius, 80.0f);
-}
-
-void UVRPawnMovement::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction){
-
-	SetCapsuleColliderToUserSize();
-
-	FVector PositionChange = GetPendingInputVector();
-
-	if (NavigationMode == EVRNavigationModes::NAV_WALK)
-	{
-		PositionChange.Z = 0.0f;
-		ConsumeInputVector();
-		AddInputVector(PositionChange);
-	}
-	
-	if(NavigationMode == EVRNavigationModes::NAV_FLY || NavigationMode == EVRNavigationModes::NAV_WALK)
-	{
-		MoveByGravityOrStepUp(DeltaTime);
-		CheckForPhysWalkingCollision();
-
-		if(CheckForVirtualMovCollision(PositionChange, DeltaTime))
-		{
-			ConsumeInputVector();
-		}
-	}
-
-	if(NavigationMode == EVRNavigationModes::NAV_NONE)
-	{
-		ConsumeInputVector();
-	}
-
-	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
-	
-	LastHeadPosition = HeadComponent->GetComponentLocation();
-}
-
-bool UVRPawnMovement::CheckForVirtualMovCollision(FVector PositionChange, float DeltaTime)
-{
-	FVector ProbePosition = PositionChange.GetSafeNormal() * GetMaxSpeed() * DeltaTime;
-	FHitResult FHitResultVR;
-	CapsuleColliderComponent->AddWorldOffset(ProbePosition, true, &FHitResultVR);
-	if (FVector::Distance(FHitResultVR.Location, CapsuleColliderComponent->GetComponentLocation()) < CapsuleColliderComponent->GetScaledCapsuleRadius())
-	{
-		return true;
-	}
-	return false;
-}
-
-void UVRPawnMovement::SetHeadComponent(USceneComponent* NewHeadComponent)
-{
-	HeadComponent = NewHeadComponent;
-	CapsuleColliderComponent->SetupAttachment(HeadComponent);
-	const float HalfHeight = 80.0f; //this is just an initial value to look good in editor
-	CapsuleColliderComponent->SetCapsuleSize(CapsuleRadius, HalfHeight);
-	CapsuleColliderComponent->SetWorldLocation(FVector(0.0f, 0.0f,HalfHeight));
-}
-
-void UVRPawnMovement::SetCapsuleColliderToUserSize()
-{
-	float CharachterSize = abs(UpdatedComponent->GetComponentLocation().Z - HeadComponent->GetComponentLocation().Z);
-
-	if (CharachterSize > MaxStepHeight)
-	{
-		float ColliderHeight = CharachterSize - MaxStepHeight;
-		float ColliderHalfHeight = ColliderHeight / 2.0f;
-		if (ColliderHalfHeight <= CapsuleRadius)
-		{//Make the collider to a Sphere
-			CapsuleColliderComponent->SetCapsuleSize(ColliderHalfHeight, ColliderHalfHeight);
-		}
-		else
-		{//Make the collider to a Capsule
-			CapsuleColliderComponent->SetCapsuleSize(CapsuleRadius, ColliderHalfHeight);
-		}
-
-		CapsuleColliderComponent->SetWorldLocation(HeadComponent->GetComponentLocation());
-		CapsuleColliderComponent->AddWorldOffset(FVector(0, 0, -ColliderHalfHeight));
-		CapsuleColliderComponent->SetWorldRotation(FRotator(0, 0, 1));
-	}
-	else
-	{
-		CapsuleColliderComponent->SetWorldLocation(HeadComponent->GetComponentLocation());
-		CapsuleColliderComponent->SetWorldRotation(FRotator(0, 0, 1));
-	}
-}
-
-void UVRPawnMovement::CheckForPhysWalkingCollision()
-{
-	FVector CurrentHeadPosition = HeadComponent->GetComponentLocation();
-	FVector Direction = CurrentHeadPosition - LastHeadPosition;
-	FHitResult FHitResultPhys;
-	CapsuleColliderComponent->AddWorldOffset(Direction, true, &FHitResultPhys);
-
-	if (FHitResultPhys.bBlockingHit)
-	{
-		UpdatedComponent->AddLocalOffset(FHitResultPhys.Normal*FHitResultPhys.PenetrationDepth);
-	}
-}
-
-void UVRPawnMovement::MoveByGravityOrStepUp(float DeltaSeconds)
-{
-	FVector StartLineTraceUnderCollider = CapsuleColliderComponent->GetComponentLocation();
-	StartLineTraceUnderCollider.Z -= CapsuleColliderComponent->GetScaledCapsuleHalfHeight();
-	FHitResult HitDetailsMultiLineTrace = CreateMultiLineTrace(FVector(0, 0, -1), StartLineTraceUnderCollider, CapsuleColliderComponent->GetScaledCapsuleRadius() / 4.0f, false);
-	float DistanceDifference = abs(MaxStepHeight - HitDetailsMultiLineTrace.Distance);
-	//Going up (in Fly and Walk Mode)
-	if ((HitDetailsMultiLineTrace.bBlockingHit && HitDetailsMultiLineTrace.Distance < MaxStepHeight))
-	{
-		ShiftVertically(DistanceDifference, UpSteppingAcceleration, DeltaSeconds, 1);
-	}
-	//Gravity (only in Walk Mode)
-	else if (NavigationMode==EVRNavigationModes::NAV_WALK && ((HitDetailsMultiLineTrace.bBlockingHit && HitDetailsMultiLineTrace.Distance > MaxStepHeight) || (HitDetailsMultiLineTrace.Actor == nullptr && HitDetailsMultiLineTrace.Distance != -1.0f)))
-	{
-		ShiftVertically(DistanceDifference, GravityAcceleration, DeltaSeconds, -1);
-	}
-}
-
-void UVRPawnMovement::ShiftVertically(float DiffernceDistance, float VerticalAcceleration, float DeltaSeconds, int Direction)
-{
-	VerticalSpeed += VerticalAcceleration * DeltaSeconds;
-	if (VerticalSpeed*DeltaSeconds < DiffernceDistance)
-	{
-		UpdatedComponent->AddLocalOffset(FVector(0.f, 0.f, Direction * VerticalSpeed * DeltaSeconds));
-	}
-	else
-	{
-		UpdatedComponent->AddLocalOffset(FVector(0.f, 0.f, Direction * DiffernceDistance));
-		VerticalSpeed = 0;
-	}
-}
-
-FHitResult UVRPawnMovement::CreateLineTrace(FVector Direction, const FVector Start, bool Visibility)
-{
-	//Re-initialize hit info
-	FHitResult HitDetails = FHitResult(ForceInit);
-
-	FVector End = ((Direction * 1000.f) + Start);
-	// additional trace parameters
-	FCollisionQueryParams TraceParams(FName(TEXT("InteractTrace")), true, NULL);
-	TraceParams.bTraceComplex = true; //to use complex collision on whatever we interact with to provide better precision.
-	TraceParams.bReturnPhysicalMaterial = true; //to provide details about the physical material, if one exists on the thing we hit, to come back in our hit result.
-
-	if (Visibility)
-		DrawDebugLine(GetWorld(), Start, End, FColor::Green, false, 1, 0, 1);
-
-	if (GetWorld()->LineTraceSingleByChannel(HitDetails, Start, End, ECC_Visibility, TraceParams))
-	{
-		if (HitDetails.bBlockingHit)
-		{
-		}
-	}
-	return HitDetails;
-}
-
-FHitResult UVRPawnMovement::CreateMultiLineTrace(FVector Direction, const FVector Start, float Radius, bool Visibility)
-{
-	TArray<FVector> StartVectors;
-	TArray<FHitResult> OutHits;
-	FHitResult HitDetailsMultiLineTrace;
-	HitDetailsMultiLineTrace.Distance = -1.0f;//(Distance=-1) not existing, but to know if this Variable not Initialized(when all Traces not compatible)
-
-	StartVectors.Add(Start); //LineTraceCenter
-	StartVectors.Add(Start + FVector(0, -Radius, 0)); //LineTraceLeft
-	StartVectors.Add(Start + FVector(0, +Radius, 0)); //LineTraceRight
-	StartVectors.Add(Start + FVector(+Radius, 0, 0)); //LineTraceFront
-	StartVectors.Add(Start + FVector(-Radius, 0, 0)); //LineTraceBehind
-
-	bool IsBlockingHitAndSameActor = true;
-	bool IsAllNothingHiting = true;
-	// loop through TArray
-	for (FVector& Vector : StartVectors)
-	{
-		FHitResult OutHit = CreateLineTrace(Direction, Vector, Visibility);
-		OutHits.Add(OutHit);
-		IsBlockingHitAndSameActor &= (OutHit.Actor == OutHits[0].Actor); //If all Hiting the same Object, then you are (going up/down) or (walking)
-		IsAllNothingHiting &= (OutHit.Actor == nullptr); //If all Hiting nothing, then you are falling
-	}
-
-	if (IsBlockingHitAndSameActor || IsAllNothingHiting)
-		HitDetailsMultiLineTrace = OutHits[0];
-
-	return HitDetailsMultiLineTrace;
-}
+
+#include "Pawn/VRPawnMovement.h"
+#include "DrawDebugHelpers.h"
+#include "Kismet/KismetSystemLibrary.h"
+
+UVRPawnMovement::UVRPawnMovement(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
+{
+	// the capsule is used to store the players size and position, e.g., for other interactions and as starting point
+	// for the capsule trace (which however not use the capsule component directly)
+	CapsuleColliderComponent = CreateDefaultSubobject<UCapsuleComponent>(TEXT("CapsuleCollider"));
+	CapsuleColliderComponent->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics);
+	CapsuleColliderComponent->SetCollisionResponseToAllChannels(ECollisionResponse::ECR_Overlap);
+	CapsuleColliderComponent->SetCollisionResponseToChannel(ECollisionChannel::ECC_WorldStatic, ECollisionResponse::ECR_Block);
+	CapsuleColliderComponent->SetCapsuleSize(CapsuleRadius, 80.0f);
+
+	//set some defaults for the UFloatingPawnMovement component, which are more reasonable for usage in VR
+	MaxSpeed = 300.f;
+	Acceleration = 800.f;
+	Deceleration = 2000.f;
+}
+
+void UVRPawnMovement::BeginPlay()
+{
+	Super::BeginPlay();
+	LastCapsulePosition.Reset();
+	LastSteeringCollisionVector = FVector(0,0,0);
+}
+
+
+void UVRPawnMovement::TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction *ThisTickFunction){
+
+	SetCapsuleColliderToUserSize();
+
+	FVector InputVector = GetPendingInputVector();
+
+	if (NavigationMode == EVRNavigationModes::NAV_WALK)
+	{
+		// you are only allowed to move horizontally in NAV_WALK
+		// everything else will be handled by stepping-up/gravity
+		// so remove Z component for the input vector of the UFloatingPawnMovement
+		InputVector.Z = 0.0f;
+		ConsumeInputVector();
+		AddInputVector(InputVector);
+	}
+
+	const FVector CapsuleLocation = CapsuleColliderComponent->GetComponentLocation();
+	if(bDeactivatedWhileInCollision && !CreateCapsuleTrace(CapsuleLocation, CapsuleLocation).bBlockingHit)
+	{
+		bDeactivatedWhileInCollision=false;
+	}
+	
+	if(NavigationMode == EVRNavigationModes::NAV_FLY || NavigationMode == EVRNavigationModes::NAV_WALK)
+	{
+		if(InputVector.Size() > 0.001){
+			const FVector SafeSteeringInput = GetCollisionSafeVirtualSteeringVec(InputVector, DeltaTime);
+			if(SafeSteeringInput != InputVector)
+			{
+				// if we would move into something if we apply this input (estimating distance by max speed)
+				// we only apply its perpendicular part (unless it is facing away from the collision)
+				ConsumeInputVector();
+				AddInputVector(SafeSteeringInput);
+			}
+		}
+
+		// so we add stepping-up (for both walk and fly)
+		// and gravity for walking only
+		MoveByGravityOrStepUp(DeltaTime);
+
+		//if we physically (in the tracking space) walked into something, move the world away (by moving the pawn)
+		if(!bDeactivatedWhileInCollision) CheckForPhysWalkingCollision();
+	}
+
+	if(NavigationMode == EVRNavigationModes::NAV_NONE)
+	{
+		//just remove whatever input is there
+		ConsumeInputVector();
+	}
+
+	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
+}
+
+void UVRPawnMovement::SetHeadComponent(USceneComponent* NewHeadComponent)
+{
+	HeadComponent = NewHeadComponent;
+	CapsuleColliderComponent->SetupAttachment(HeadComponent);
+	const float HalfHeight = 80.0f; //this is just an initial value to look good in editor
+	CapsuleColliderComponent->SetCapsuleSize(CapsuleRadius, HalfHeight);
+	CapsuleColliderComponent->SetWorldLocation(FVector(0.0f, 0.0f,HalfHeight));
+}
+
+void UVRPawnMovement::SetCapsuleColliderToUserSize()
+{
+
+	// the collider should be placed
+	//	between head and floor + MaxStepHeight
+	//             head
+	//            /    \
+	//           /      \
+	//          |        |
+	//          |        |
+	//          |collider|
+	//          |        |
+	//          |        |
+	//           \      /
+	//            \ __ /
+	//              |
+	//         MaxStepHeight
+	//              |
+	// floor: ______________
+
+	const float UserSize = HeadComponent->GetComponentLocation().Z - UpdatedComponent->GetComponentLocation().Z;
+
+	if (UserSize > MaxStepHeight)
+	{
+		const float ColliderHeight = UserSize - MaxStepHeight;
+		const float ColliderHalfHeight = ColliderHeight / 2.0f;
+		if (ColliderHalfHeight <= CapsuleRadius)
+		{
+			//the capsule will actually be compressed to a sphere
+			CapsuleColliderComponent->SetCapsuleSize(ColliderHalfHeight, ColliderHalfHeight);
+		}
+		else
+		{
+			CapsuleColliderComponent->SetCapsuleSize(CapsuleRadius, ColliderHalfHeight);
+		}
+
+		CapsuleColliderComponent->SetWorldLocation(HeadComponent->GetComponentLocation() - FVector(0, 0, ColliderHalfHeight));
+	}
+	else
+	{
+		CapsuleColliderComponent->SetWorldLocation(HeadComponent->GetComponentLocation());
+	}
+
+	CapsuleColliderComponent->SetWorldRotation(FRotator::ZeroRotator);
+}
+
+void UVRPawnMovement::CheckForPhysWalkingCollision()
+{
+	if(!LastCapsulePosition.IsSet())
+	{
+		//not yet set, so do nothing than setting it
+		LastCapsulePosition = CapsuleColliderComponent->GetComponentLocation();
+		return;
+	}
+
+	const FVector CapsuleLocation = CapsuleColliderComponent->GetComponentLocation();
+	const FHitResult HitResult = CreateCapsuleTrace(LastCapsulePosition.GetValue(), CapsuleLocation);
+
+	//if this was not possible move the entire pawn away to avoid the head collision
+	if (HitResult.bBlockingHit)
+	{
+		const FVector MoveOutVector = HitResult.Location-CapsuleLocation;
+		//move it out twice as far, to avoid getting stuck situations
+		UpdatedComponent->AddWorldOffset(2*MoveOutVector);
+	}
+
+
+	//only update if not in collision
+	if(!CreateCapsuleTrace(CapsuleLocation, CapsuleLocation).bBlockingHit)
+	{
+		LastCapsulePosition = CapsuleColliderComponent->GetComponentLocation();;
+	}
+	else{
+		//we are still in collision, so deactivate collision handling until this stopped
+		bDeactivatedWhileInCollision=true;
+		LastCapsulePosition.Reset();
+	}
+}
+
+FVector UVRPawnMovement::GetCollisionSafeVirtualSteeringVec(FVector InputVector, float DeltaTime)
+{
+
+	const float SafetyFactor = 3.0f; //so we detect collision a bit earlier
+	const FVector CapsuleLocation = CapsuleColliderComponent->GetComponentLocation();
+	FVector ProbePosition = SafetyFactor * InputVector.GetSafeNormal() * GetMaxSpeed() * DeltaTime + CapsuleLocation;
+	const FHitResult TraceResult = CreateCapsuleTrace(CapsuleLocation, ProbePosition);
+	if (!TraceResult.bBlockingHit)
+	{
+		//everything is fine, use that vector
+		return InputVector;
+	}
+
+	//otherwise remove the component of that vector that goes towards the collision
+	FVector CollisionVector = TraceResult.Location - CapsuleLocation;
+
+	//sometimes (if by chance we already moved into collision entirely CollisionVector is 0
+	if(! CollisionVector.Normalize())
+	{
+		//then we probably start already in collision, so we use the last one
+		CollisionVector = LastSteeringCollisionVector;
+	}
+	else
+	{
\ No newline at end of file
+		LastSteeringCollisionVector = CollisionVector;
+	}
+
+	FVector SafeInput = InputVector;
+	const float DotProduct =  FVector::DotProduct(InputVector, CollisionVector);
+	if(DotProduct>0.0f)
+	{
+		// only keep perpendicular part of the input vector (remove anything towards hit)
+		SafeInput -= DotProduct * CollisionVector;
+	}
+	return SafeInput;
+}
+
+void UVRPawnMovement::MoveByGravityOrStepUp(float DeltaSeconds)
+{
+	const FVector DownTraceStart = CapsuleColliderComponent->GetComponentLocation();
+	const float DownTraceDist = MaxFallingDepth < 0.0f ? 1000.0f : MaxFallingDepth;
+	const FVector DownTraceDir = FVector(0,0,-1);
+	const FVector DownTraceEnd = DownTraceStart + DownTraceDist * DownTraceDir;
+
+	const FHitResult DownTraceHitResult = CreateCapsuleTrace(DownTraceStart, DownTraceEnd);
+	float HeightDifference = 0.0f;
+
+	if(DownTraceHitResult.bBlockingHit)
+	{
+		HeightDifference = DownTraceHitResult.ImpactPoint.Z - UpdatedComponent->GetComponentLocation().Z;
+		//so for HeightDifference>0, we have to move the pawn up; for HeightDifference<0 we have to move it down
+	}
+
+	//Going up (in Fly and Walk Mode)
+	if (HeightDifference>0.0f && HeightDifference<=MaxStepHeight)
+	{
+		ShiftVertically(HeightDifference, UpSteppingAcceleration, DeltaSeconds);
+	}
+
+	if(NavigationMode!=EVRNavigationModes::NAV_WALK)
+	{
+		return;
+	}
+
+	if(!DownTraceHitResult.bBlockingHit && MaxFallingDepth<0.0f)
+	{
+		HeightDifference = -1000.0f; //just fall
+	}
+
+	//Gravity (only in Walk Mode)
+	if (HeightDifference<0.0f)
+	{
+		ShiftVertically(HeightDifference, GravityAcceleration, DeltaSeconds);
+	}
+}
+
+void UVRPawnMovement::ShiftVertically(float Distance, float VerticalAcceleration, float DeltaSeconds)
+{
+	VerticalSpeed += VerticalAcceleration * DeltaSeconds;
+	if (abs(VerticalSpeed*DeltaSeconds) < abs(Distance))
+	{
+		UpdatedComponent->AddWorldOffset(FVector(0.f, 0.f,  VerticalSpeed * DeltaSeconds));
+	}
+	else
+	{
+		UpdatedComponent->AddWorldOffset(FVector(0.f, 0.f,  Distance));
+		VerticalSpeed = 0;
+	}
+}
+
+FHitResult UVRPawnMovement::CreateCapsuleTrace(const FVector Start, FVector End, bool DrawDebug)
+{
+	const EDrawDebugTrace::Type DrawType = DrawDebug ? EDrawDebugTrace::Type::ForDuration : EDrawDebugTrace::Type::None;
+	if(ActorsToIgnore.Num()==0){
+		ActorsToIgnore.Add(GetOwner());
+	}
+
+	//UE_LOG(LogTemp, Warning, TEXT("Capsule from %s to %s"), *Start.ToString(), *End.ToString())
+
+	FHitResult Hit;
+	UKismetSystemLibrary::CapsuleTraceSingle(GetWorld(), Start, End, CapsuleColliderComponent->GetScaledCapsuleRadius(), CapsuleColliderComponent->GetScaledCapsuleHalfHeight(), UEngineTypes::ConvertToTraceType(ECollisionChannel::ECC_Visibility), true, ActorsToIgnore, DrawType, Hit, true);
+	return Hit;
+}