improve walking by storing last collision free position and restoring it in...

improve walking by storing last collision free position and restoring it in case that is needed, also move out of dynamic collisions
closes #13

Source/RWTHVRToolkit/Private/Pawn/VRPawnMovement.cpp

@@ -22,8 +22,10 @@ UVRPawnMovement::UVRPawnMovement(const FObjectInitializer& ObjectInitializer) :
 void UVRPawnMovement::BeginPlay()
 {
 	Super::BeginPlay();
-	LastCapsulePosition.Reset();
+	LastCollisionFreeCapsulePosition = CapsuleColliderComponent->GetComponentLocation();
 	LastSteeringCollisionVector = FVector(0,0,0);
+
+	ActorsToIgnore = { GetOwner() };
 }
 
 
@@ -43,11 +45,10 @@ void UVRPawnMovement::TickComponent(float DeltaTime, enum ELevelTick TickType, F
 		AddInputVector(InputVector);
 	}
 
-	const FVector CapsuleLocation = CapsuleColliderComponent->GetComponentLocation();
-	if(bDeactivatedWhileInCollision && !CreateCapsuleTrace(CapsuleLocation, CapsuleLocation).bBlockingHit)
-	{
-		bDeactivatedWhileInCollision=false;
-	}
+	//if me managed to get into a collision revert the movement since last Tick
+	CheckAndRevertCollisionSinceLastTick();
+	//check whether we are still in collision e.g. if an object has moved and got us into collision
+	MoveOutOfNewDynamicCollisions();
 	
 	if(NavigationMode == EVRNavigationModes::NAV_FLY || NavigationMode == EVRNavigationModes::NAV_WALK)
 	{
@@ -67,7 +68,7 @@ void UVRPawnMovement::TickComponent(float DeltaTime, enum ELevelTick TickType, F
 		MoveByGravityOrStepUp(DeltaTime);
 
 		//if we physically (in the tracking space) walked into something, move the world away (by moving the pawn)
-		if(!bDeactivatedWhileInCollision) CheckForPhysWalkingCollision();
+		CheckForPhysWalkingCollision();
 	}
 
 	if(NavigationMode == EVRNavigationModes::NAV_NONE)
@@ -134,17 +135,32 @@ void UVRPawnMovement::SetCapsuleColliderToUserSize()
 	CapsuleColliderComponent->SetWorldRotation(FRotator::ZeroRotator);
 }
 
-void UVRPawnMovement::CheckForPhysWalkingCollision()
+void UVRPawnMovement::CheckAndRevertCollisionSinceLastTick()
 {
-	if(!LastCapsulePosition.IsSet())
+	//check whether we are in a collision at the current position
+	const FVector CapsuleLocation = CapsuleColliderComponent->GetComponentLocation();
+	if (CreateCapsuleTrace(CapsuleLocation, CapsuleLocation).bBlockingHit)
 	{
-		//not yet set, so do nothing than setting it
-		LastCapsulePosition = CapsuleColliderComponent->GetComponentLocation();
-		return;
+		//if so move back to last position
+		UpdatedComponent->AddWorldOffset(LastCollisionFreeCapsulePosition - CapsuleLocation);
+	}
+	else
+	{
+		LastCollisionFreeCapsulePosition = CapsuleLocation;
+	}
 }
 
+void UVRPawnMovement::MoveOutOfNewDynamicCollisions()
+{
+	FVector ResolveDirection = GetOverlapResolveDirection();
+	ResolveDirection *= 1.5f; //scale it up for security distance
+	UpdatedComponent->AddWorldOffset(ResolveDirection);
+}
+
+void UVRPawnMovement::CheckForPhysWalkingCollision()
+{
 	const FVector CapsuleLocation = CapsuleColliderComponent->GetComponentLocation();
-	const FHitResult HitResult = CreateCapsuleTrace(LastCapsulePosition.GetValue(), CapsuleLocation);
+	const FHitResult HitResult = CreateCapsuleTrace(LastCollisionFreeCapsulePosition, CapsuleLocation);
 
 	//if this was not possible move the entire pawn away to avoid the head collision
 	if (HitResult.bBlockingHit)
@@ -153,18 +169,6 @@ void UVRPawnMovement::CheckForPhysWalkingCollision()
 		//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)
@@ -260,9 +264,6 @@ void UVRPawnMovement::ShiftVertically(float Distance, float VerticalAcceleration
 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())
 
@@ -270,3 +271,22 @@ FHitResult UVRPawnMovement::CreateCapsuleTrace(const FVector Start, FVector End,
 	UKismetSystemLibrary::CapsuleTraceSingle(GetWorld(), Start, End, CapsuleColliderComponent->GetScaledCapsuleRadius(), CapsuleColliderComponent->GetScaledCapsuleHalfHeight(), UEngineTypes::ConvertToTraceType(ECollisionChannel::ECC_Visibility), true, ActorsToIgnore, DrawType, Hit, true);
 	return Hit;
 }
+
+FVector UVRPawnMovement::GetOverlapResolveDirection()
+{
+	TArray<UPrimitiveComponent*> OverlappingComponents;
+	TArray<TEnumAsByte<EObjectTypeQuery>> traceObjectTypes;
+	traceObjectTypes.Add(UEngineTypes::ConvertToObjectType(ECollisionChannel::ECC_Visibility));
+	UKismetSystemLibrary::CapsuleOverlapComponents(GetWorld(), CapsuleColliderComponent->GetComponentLocation(), CapsuleColliderComponent->GetScaledCapsuleRadius(), CapsuleColliderComponent->GetScaledCapsuleHalfHeight(), traceObjectTypes, nullptr, ActorsToIgnore, OverlappingComponents);
+
+	FVector ResolveVector = FVector::ZeroVector;
+
+	//check what to do to move out of these collisions (or nothing if non is there)
+	//we just add the penetrations so in very unfortunate conditions this can become problematic/blocking but for now and our regular use cases this works
+	for(const UPrimitiveComponent* OverlappingComp : OverlappingComponents)
+	{
+		FHitResult Hit = CreateCapsuleTrace(CapsuleColliderComponent->GetComponentLocation(), OverlappingComp->GetComponentLocation(), false);
+		ResolveVector += Hit.ImpactNormal * Hit.PenetrationDepth;
+	}
+	return ResolveVector;
+}

Source/RWTHVRToolkit/Public/Pawn/VRPawnMovement.h

@@ -36,7 +36,9 @@ class RWTHVRTOOLKIT_API UVRPawnMovement : public UFloatingPawnMovement
 public:
 
 	virtual void BeginPlay() override;
+	void CheckAndRevertCollisionSinceLastTick();
 
+	void MoveOutOfNewDynamicCollisions();
 	virtual void TickComponent(float DeltaTime, enum ELevelTick TickType,
 	                           FActorComponentTickFunction* ThisTickFunction) override;
 
@@ -65,6 +67,7 @@ public:
 private:
 	//check for
 	FHitResult CreateCapsuleTrace(const FVector Start, FVector End, bool DrawDebug=false);
+	FVector GetOverlapResolveDirection();
 	void SetCapsuleColliderToUserSize();
 	void CheckForPhysWalkingCollision();
 	FVector GetCollisionSafeVirtualSteeringVec(FVector InputVector, float DeltaTime);
@@ -75,12 +78,10 @@ private:
 	UPROPERTY() USceneComponent* HeadComponent = nullptr;
 
 	float VerticalSpeed = 0.0f;
-	TOptional<FVector> LastCapsulePosition;
+	FVector LastCollisionFreeCapsulePosition;
 	FVector LastSteeringCollisionVector;
 
-	//this will deactivate all collision avoidance, so we can move out of a collision, e.g. if movements of the scene provoked a collision
-	bool bDeactivatedWhileInCollision = false;
-
 	//just stored for performance gains;
+	UPROPERTY()
 	TArray<AActor*> ActorsToIgnore;
 };