tengri/Source/TengriPlatformer/Movement/Collision/TengriCollisionResolver.cpp

// Source/TengriPlatformer/Movement/Collision/TengriCollisionResolver.cpp

#include "TengriCollisionResolver.h"
#include "Components/CapsuleComponent.h"
#include "Engine/World.h"

// ════════════════════════════════════════════════════════════════════════════
//                              CONSTANTS
// ════════════════════════════════════════════════════════════════════════════

namespace TengriPhysics
{
	// Contact offset to prevent surface penetration
	constexpr float ContactOffset = 0.005f;

	// Minimum movement distance (below this, stop iterating)
	constexpr float MinMoveDist = 0.001f;

	// Step-up: reject surfaces sloping toward us more than this
	constexpr float StepUpMaxSlopeZ = 0.2f;

	// Step-up: reject overhangs/ceilings
	constexpr float StepUpMinNormalZ = -0.01f;

	// Minimum upward velocity to count as "jumping"
	constexpr float MinUpwardVelocity = 0.01f;

	// Minimum Normal.Z to be considered ground in step-down
	constexpr float MinGroundNormalZ = 0.1f;

	// Step height tolerance for final validation
	constexpr float StepHeightTolerance = 1.0f;

	// Overlap check uses slightly smaller capsule
	constexpr float OverlapCheckScale = 0.95f;

	// Down trace extends slightly beyond step height
	constexpr float DownTraceMultiplier = 1.2f;
}

// ════════════════════════════════════════════════════════════════════════════
//                              PRIMITIVES
// ════════════════════════════════════════════════════════════════════════════

FTengriSweepResult UTengriCollisionResolver::PerformSweep(
	const UObject* WorldContext,
	const FVector& Start,
	const FVector& End,
	const UCapsuleComponent* Capsule,
	bool bShowDebug)
{
	FTengriSweepResult Result;
	Result.Location = End;
	Result.bBlocked = false;

	if (!WorldContext || !Capsule)
	{
		return Result;
	}

	UWorld* World = WorldContext->GetWorld();
	if (!World)
	{
		return Result;
	}

	FCollisionQueryParams Params;
	Params.AddIgnoredActor(Capsule->GetOwner());
	Params.bTraceComplex = false;

	const bool bHit = World->SweepSingleByChannel(
		Result.Hit,
		Start,
		End,
		Capsule->GetComponentQuat(),
		ECC_Visibility,
		FCollisionShape::MakeCapsule(
			Capsule->GetScaledCapsuleRadius(),
			Capsule->GetScaledCapsuleHalfHeight()
		),
		Params
	);

	if (bHit)
	{
		Result.bBlocked = true;
		Result.Location = Result.Hit.Location;
	}

	return Result;
}

FVector UTengriCollisionResolver::ClipVelocity(const FVector& Velocity, const FVector& Normal)
{
	const float Backoff = FVector::DotProduct(Velocity, Normal);
	if (Backoff > 0.f)
	{
		return Velocity;
	}
	return Velocity - (Normal * Backoff);
}

FVector UTengriCollisionResolver::ProjectVelocity(const FVector& Velocity, const FVector& Normal)
{
	const FVector Dir = ClipVelocity(Velocity, Normal).GetSafeNormal();
	return Dir * Velocity.Size();
}

// ════════════════════════════════════════════════════════════════════════════
//                              STEP UP
// ════════════════════════════════════════════════════════════════════════════

bool UTengriCollisionResolver::StepUp(
	const UObject* WorldContext,
	const FVector& StartLocation,
	const FVector& DesiredDelta,
	const FHitResult& ImpactHit,
	const UCapsuleComponent* Capsule,
	float MaxStepHeight,
	FVector& OutLocation)
{
	// Reject sloped surfaces and overhangs
	if (ImpactHit.ImpactNormal.Z > TengriPhysics::StepUpMaxSlopeZ ||
		ImpactHit.ImpactNormal.Z < TengriPhysics::StepUpMinNormalZ)
	{
		return false;
	}

	// Reject if falling
	if (DesiredDelta.Z < TengriPhysics::StepUpMinNormalZ)
	{
		return false;
	}

	// === Phase A: Trace Up ===
	const FVector StepUpEnd = StartLocation + FVector(0.f, 0.f, MaxStepHeight);
	const FTengriSweepResult UpSweep = PerformSweep(WorldContext, StartLocation, StepUpEnd, Capsule, false);

	// Reject if not enough headroom (hit ceiling before half step height)
	if (UpSweep.bBlocked && UpSweep.Location.Z < (StartLocation.Z + MaxStepHeight * 0.5f))
	{
		return false;
	}

	// === Phase B: Trace Forward ===
	// When pressing against a wall, DesiredDelta approaches zero.
	// We must check forward at least CapsuleRadius to avoid phasing through geometry.
	FVector ForwardDir = DesiredDelta.GetSafeNormal2D();
	if (ForwardDir.IsNearlyZero())
	{
		ForwardDir = -ImpactHit.ImpactNormal;
	}

	const float MinCheckDist = Capsule ? Capsule->GetScaledCapsuleRadius() : 30.0f;
	const float CheckDist = FMath::Max(DesiredDelta.Size2D(), MinCheckDist);
	const FVector ForwardEnd = UpSweep.Location + (ForwardDir * CheckDist);

	const FTengriSweepResult ForwardSweep = PerformSweep(WorldContext, UpSweep.Location, ForwardEnd, Capsule, false);

	// Reject if obstacle continues upward (wall, next stair step)
	if (ForwardSweep.bBlocked)
	{
		return false;
	}

	// === Phase C: Trace Down ===
	const FVector DownStart = ForwardSweep.Location;
	const FVector DownEnd = DownStart - FVector(0.f, 0.f, MaxStepHeight * TengriPhysics::DownTraceMultiplier);
	const FTengriSweepResult DownSweep = PerformSweep(WorldContext, DownStart, DownEnd, Capsule, false);

	if (!DownSweep.bBlocked || DownSweep.Hit.ImpactNormal.Z < TengriPhysics::MinGroundNormalZ)
	{
		return false;
	}

	// Validate actual height difference
	const float RealHeightDiff = DownSweep.Location.Z - StartLocation.Z;
	if (RealHeightDiff > (MaxStepHeight + TengriPhysics::StepHeightTolerance))
	{
		return false;
	}

	// === Phase D: Overlap Check ===
	// Verify we can stand at the new location without intersecting geometry
	FCollisionQueryParams OverlapParams;
	OverlapParams.AddIgnoredActor(Capsule->GetOwner());

	const bool bOverlap = WorldContext->GetWorld()->OverlapBlockingTestByChannel(
		DownSweep.Location,
		Capsule->GetComponentQuat(),
		ECC_Visibility,
		FCollisionShape::MakeCapsule(
			Capsule->GetScaledCapsuleRadius() * TengriPhysics::OverlapCheckScale,
			Capsule->GetScaledCapsuleHalfHeight() * TengriPhysics::OverlapCheckScale
		),
		OverlapParams
	);

	if (bOverlap)
	{
		return false;
	}

	OutLocation = DownSweep.Location;
	return true;
}

// ════════════════════════════════════════════════════════════════════════════
//                           GROUND SNAPPING
// ════════════════════════════════════════════════════════════════════════════

bool UTengriCollisionResolver::SnapToGround(
	const UObject* WorldContext,
	const UCapsuleComponent* Capsule,
	const float SnapDistance,
	const FSurfaceThresholds& Thresholds,
	FVector& OutLocation,
	FHitResult& OutHit)
{
	if (!WorldContext || !Capsule)
	{
		return false;
	}

	const FVector Start = Capsule->GetComponentLocation();
	const FVector End = Start - FVector(0.f, 0.f, SnapDistance);

	if (const FTengriSweepResult Sweep = PerformSweep(WorldContext, Start, End, Capsule, false); Sweep.bBlocked && Thresholds.IsWalkable(Sweep.Hit.ImpactNormal.Z))
	{
		OutLocation = Sweep.Location;
		OutHit = Sweep.Hit;
		return true;
	}

	return false;
}

// ════════════════════════════════════════════════════════════════════════════
//                          SLIDE HELPERS
// ════════════════════════════════════════════════════════════════════════════

namespace
{
	/** Calculate horizontal slide along overhang/ceiling when walking (not jumping) */
	FVector CalculateOverhangSlide(const FVector& RemainingDelta, const FVector& ImpactNormal)
	{
		// If jumping into ceiling, use normal clip
		if (RemainingDelta.Z > TengriPhysics::MinUpwardVelocity)
		{
			return UTengriCollisionResolver::ClipVelocity(RemainingDelta, ImpactNormal);
		}

		// Walking under sloped ceiling - slide horizontally only
		const FVector UpVector(0.f, 0.f, 1.f);
		FVector HorizontalTangent = FVector::CrossProduct(ImpactNormal, UpVector).GetSafeNormal();

		if (HorizontalTangent.IsNearlyZero())
		{
			return FVector::ZeroVector;
		}

		// Align tangent with desired movement direction
		if (FVector::DotProduct(HorizontalTangent, RemainingDelta) < 0.f)
		{
			HorizontalTangent *= -1.f;
		}

		return HorizontalTangent * FVector::DotProduct(RemainingDelta, HorizontalTangent);
	}

	/** Calculate slide along wall, preventing upward velocity from steep surfaces */
	FVector CalculateWallSlide(const FVector& RemainingDelta, const FVector& ImpactNormal)
	{
		FVector ClipDelta = UTengriCollisionResolver::ClipVelocity(RemainingDelta, ImpactNormal);

		// If wall would push us up, force horizontal movement only
		if (ClipDelta.Z > 0.f)
		{
			FVector HorizontalTangent = FVector::CrossProduct(ImpactNormal, FVector(0.f, 0.f, 1.f)).GetSafeNormal();

			if (!HorizontalTangent.IsNearlyZero())
			{
				if (FVector::DotProduct(HorizontalTangent, RemainingDelta) < 0.f)
				{
					HorizontalTangent *= -1.f;
				}
				return HorizontalTangent * FVector::DotProduct(RemainingDelta, HorizontalTangent);
			}

			ClipDelta.Z = 0.f;
		}

		return ClipDelta;
	}

	/** Apply crease logic when stuck between two surfaces */
	FVector ApplyCreaseLogic(
		const FVector& NewDelta,
		const FVector& RemainingDelta,
		const FVector& PrevNormal,
		const FVector& CurrentNormal)
	{
		if (const FVector Crease = FVector::CrossProduct(PrevNormal, CurrentNormal); Crease.SizeSquared() > TengriPhysics::ContactOffset)
		{
			const float Dot = FVector::DotProduct(RemainingDelta, Crease);
			return Crease * Dot;
		}

		return NewDelta;
	}
}

// ════════════════════════════════════════════════════════════════════════════
//                         MAIN RESOLUTION
// ════════════════════════════════════════════════════════════════════════════

FTengriSweepResult UTengriCollisionResolver::ResolveMovement(
	const UObject* WorldContext,
	const FVector& StartLocation,
	const FVector& DesiredDelta,
	const UCapsuleComponent* Capsule,
	const FSurfaceThresholds& Thresholds,
	const float MaxStepHeight,
	const int32 MaxIterations,
	const bool bShowDebug)
{
	FTengriSweepResult FinalResult;
	FinalResult.Location = StartLocation;

	FVector CurrentLocation = StartLocation;
	FVector RemainingDelta = DesiredDelta;
	FVector PrevNormal = FVector::ZeroVector;

	for (int32 Iteration = 0; Iteration < MaxIterations; ++Iteration)
	{
		const FVector Target = CurrentLocation + RemainingDelta;
		const FTengriSweepResult Sweep = PerformSweep(WorldContext, CurrentLocation, Target, Capsule, bShowDebug);

		FinalResult.CollisionCount++;

		// No collision - movement complete
		if (!Sweep.bBlocked)
		{
			CurrentLocation = Sweep.Location;
			break;
		}

		FinalResult.bBlocked = true;
		FinalResult.Hit = Sweep.Hit;

		// Classify surface
		const float NormalZ = Sweep.Hit.ImpactNormal.Z;
		const bool bIsFloor = Thresholds.IsWalkable(NormalZ);
		const bool bIsOverhang = Thresholds.IsOverhang(NormalZ);

		// Try step-up for walls
		if (!bIsFloor && !bIsOverhang)
		{
			if (FVector StepDest; StepUp(WorldContext, CurrentLocation, RemainingDelta, Sweep.Hit, Capsule, MaxStepHeight, StepDest))
			{
				CurrentLocation = StepDest;
				break;
			}
		}

		// Apply contact offset to prevent penetration
		CurrentLocation = Sweep.Location + (Sweep.Hit.ImpactNormal * TengriPhysics::ContactOffset);

		// Calculate slide direction based on surface type
		FVector NewDelta;
		if (bIsFloor)
		{
			NewDelta = ProjectVelocity(RemainingDelta, Sweep.Hit.ImpactNormal);
		}
		else if (bIsOverhang)
		{
			NewDelta = CalculateOverhangSlide(RemainingDelta, Sweep.Hit.ImpactNormal);
		}
		else
		{
			NewDelta = CalculateWallSlide(RemainingDelta, Sweep.Hit.ImpactNormal);
		}

		// Handle corner/crease situations
		if (Iteration > 0)
		{
			NewDelta = ApplyCreaseLogic(NewDelta, RemainingDelta, PrevNormal, Sweep.Hit.ImpactNormal);
		}

		PrevNormal = Sweep.Hit.ImpactNormal;
		RemainingDelta = NewDelta;

		// Stop if remaining movement is negligible
		if (RemainingDelta.SizeSquared() < FMath::Square(TengriPhysics::MinMoveDist))
		{
			break;
		}
	}

	FinalResult.Location = CurrentLocation;
	return FinalResult;
}