Mantle code

bool UCustomCharacterMovementComponent::TryMantle()
{
	if (bCanVault) return false;
	
	// Location of the base of the capsule
	FVector BaseLocation = UpdatedComponent->GetComponentLocation() + FVector::DownVector * GetCapsuleHalfHeight();
	// Forward vector
	FVector Forward = UpdatedComponent->GetForwardVector().GetSafeNormal2D();
	// Actors to ignore
	auto Params = MainCharacter->GetIgnoreCharacterParams();
	// Max height reachable by the mantle
	float MaxHeight = GetCapsuleHalfHeight() * 2 + MantleReachHeight;

	float CosMantleMinWallSteepnessAngle = FMath::Cos(FMath::DegreesToRadians(MantleMinWallSteepnessAngle));
	float CosMantleMaxSurfaceAngle = FMath::Cos(FMath::DegreesToRadians(MantleMaxSurfaceAngle));
	float CosMantleMaxAlignmentAngle = FMath::Cos(FMath::DegreesToRadians(MantleMaxAlignmentAngle));

	// CHECK OBSTACLE FRONT FACE
	
	FHitResult FrontHit;

	float CheckDistance = GetCapsuleRadius() + MantleMaxDistance;

	// The starting point of the line trace takes into account the step height and a customisable offset

	FVector FrontStart = BaseLocation + FVector::UpVector * (MaxStepHeight - 1);
	FrontStart.Z += MantleBaseStartOffset;

	for (int i = 0; i < 10; i++)
	{
		DRAW_LINE(FrontStart, FrontStart + Forward * CheckDistance, FColor::Red)

		if (GetWorld()->LineTraceSingleByProfile(FrontHit, FrontStart, FrontStart + Forward * CheckDistance,
			"BlockAll", Params)) break;

		FrontStart += FVector::UpVector * (2.f * GetCapsuleHalfHeight() - (MaxStepHeight - 1)) / 9;
	}
	
	if (!FrontHit.IsValidBlockingHit()) return false;
	
	float CosWallSteepnessAngle = FrontHit.Normal | FVector::UpVector;
	// Check if the front of the object is too steep
	if (FMath::Abs(CosWallSteepnessAngle) > CosMantleMinWallSteepnessAngle 
		|| (Forward | -FrontHit.Normal) <  CosMantleMaxAlignmentAngle) return false;

	DRAW_POINT(FrontHit.Location, FColor::Red)

	// CHECK OBSTACLE HEIGHT
	
	TArray<FHitResult> HeightHits;
	FHitResult SurfaceHit; 

	// Project the UP vector onto the normal vector of the object hit and normalize it
	// This give us a vector that goes UP in the direction of the wall
	
	FVector WallUpVector = FVector::VectorPlaneProject(FVector::UpVector, FrontHit.Normal).GetSafeNormal();
	float WallCos = FVector::UpVector | FrontHit.Normal;
	float WallSin = FMath::Sqrt(1 - WallCos * WallCos);

	// The starting point for the line trace is the location of the wall + a forward vector scaled by the WALL-UP vector
	// times the magnitude we want this vector (the max height we can perform the mantle), all divided by WALL-SIN
	// WALL-SIN will be between 0 and 1 thus making TRACE-START bigger to account for the potential steepness of the wall
	// (to ensure that TRACE-START is always of the same length)

	FVector TraceStart = FrontHit.Location + Forward + WallUpVector * (MaxHeight - (MaxStepHeight - 1)) / WallSin;
	DRAW_LINE(TraceStart, FrontHit.Location + Forward, FColor::Orange);

	if(!GetWorld()->LineTraceMultiByProfile(HeightHits, TraceStart, FrontHit.Location + Forward, "BlockAll", Params))
	{
		return false;
	}

	for(const FHitResult& Hit : HeightHits)
	{
		// The tag is for the level designer to specify specific objects that should not be "mantable"
		if (Hit.IsValidBlockingHit() && !Hit.GetActor()->ActorHasTag("NotMantle"))
		{
			SurfaceHit = Hit;
			break;
		}
	}

	// Check if the surface hit is valid OR if the surface is too steep to perform the mantle
	if (!SurfaceHit.IsValidBlockingHit() || (SurfaceHit.Normal | FVector::UpVector) < CosMantleMaxSurfaceAngle) return false;

	// We can consider the value of the dot product as the component of the first vector over the second one,
	// thus giving us the height of the surface from the base location

	float Height = (SurfaceHit.Location - BaseLocation) | FVector::UpVector;

	PRINT_SCREEN(FString::Printf(TEXT("Height: %f"), Height));

	DRAW_POINT(SurfaceHit.Location, FColor::Blue);

	if (Height > MaxHeight) return false;

	// CHECK CLEARANCE

	float SurfaceCos = FVector::UpVector | SurfaceHit.Normal;
	float SurfaceSin = FMath::Sqrt(1 - SurfaceCos * SurfaceCos);

	// The point the capsule should mantle to, it takes into account the size of the capsule and any potential steepness of the surface

	FVector ClearanceCapsuleLocation = SurfaceHit.Location + Forward * GetCapsuleRadius() + 
		FVector::UpVector * (GetCapsuleHalfHeight() + 1 + GetCapsuleRadius() * 2 * SurfaceSin);

	FCollisionShape CapsuleShape = FCollisionShape::MakeCapsule(GetCapsuleRadius(), GetCapsuleHalfHeight());

	if (GetWorld()->OverlapAnyTestByProfile(ClearanceCapsuleLocation, FQuat::Identity, "BlockAll", CapsuleShape, Params))
	{
		DRAW_CAPSULE(ClearanceCapsuleLocation, FColor::Red)
		return false;
	}

	// Check if there are any obstacles between the player and the final point
	if (GetWorld()->OverlapAnyTestByProfile(GetActorLocation() + 
		FVector::UpVector * (GetCapsuleHalfHeight() + 1 + GetCapsuleRadius() * 2 * SurfaceSin), 
		FQuat::Identity, "BlockAll", CapsuleShape, Params))
	{
		DRAW_CAPSULE(ClearanceCapsuleLocation, FColor::Black)
		return false;
	}
	
	DRAW_CAPSULE(SurfaceHit.Location + Forward * GetCapsuleRadius() + FVector::UpVector * GetCapsuleHalfHeight(), FColor::Green)
	
	MantleLocation = SurfaceHit.Location + Forward * GetCapsuleRadius() + FVector::UpVector * GetCapsuleHalfHeight();
	bCanMantle = true;
	bHighMantle = true;

	if (Height < LowMantleCutoff) bHighMantle = false;
	
	return true;
}