SuggestProjectileVelocity function from unreal engine

GamePlayStatistics.cpp:

/** SuggestProjectileVelocity **/

// note: this will automatically fall back to line test if radius is small enough// Based on analytic solution to ballistic angle of launch http://en.wikipedia.org/wiki/Trajectory_of_a_projectile#Angle_required_to_hit_coordinate_.28x.2Cy.29bool UGameplayStatics::SuggestProjectileVelocity(const UObject* WorldContextObject, FVector& OutTossVelocity, FVector Start, FVector End, float TossSpeed, bool bFavorHighArc, float CollisionRadius, float OverrideGravityZ, ESuggestProjVelocityTraceOption::Type TraceOption, const FCollisionResponseParams& ResponseParam, const TArray<AActor*>& ActorsToIgnore, bool bDrawDebug){
const FVector FlightDelta = End - Start;
const FVector DirXY = FlightDelta.GetSafeNormal2D();
const float DeltaXY = FlightDelta.Size2D();

const float DeltaZ = FlightDelta.Z;

const float TossSpeedSq = FMath::Square(TossSpeed);

const UWorld* const World = GEngine->GetWorldFromContextObject(WorldContextObject, EGetWorldErrorMode::LogAndReturnNull);
if (World == nullptr)
{
return false;
}
const float GravityZ = FMath::IsNearlyEqual(OverrideGravityZ, 0.0f) ? -World->GetGravityZ() : -OverrideGravityZ;

// v^4 - g*(g*x^2 + 2*y*v^2)
const float InsideTheSqrt = FMath::Square(TossSpeedSq) - GravityZ * ( (GravityZ * FMath::Square(DeltaXY)) + (2.f * DeltaZ * TossSpeedSq) );
if (InsideTheSqrt < 0.f)
{
// sqrt will be imaginary, therefore no solutions
return false;
}

// if we got here, there are 2 solutions: one high-angle and one low-angle.

const float SqrtPart = FMath::Sqrt(InsideTheSqrt);

// this is the tangent of the firing angle for the first (+) solution
const float TanSolutionAngleA = (TossSpeedSq + SqrtPart) / (GravityZ * DeltaXY);
// this is the tangent of the firing angle for the second (-) solution
const float TanSolutionAngleB = (TossSpeedSq - SqrtPart) / (GravityZ * DeltaXY);
// mag in the XY dir = sqrt( TossSpeedSq / (TanSolutionAngle^2 + 1) );
const float MagXYSq_A = TossSpeedSq / (FMath::Square(TanSolutionAngleA) + 1.f);
const float MagXYSq_B = TossSpeedSq / (FMath::Square(TanSolutionAngleB) + 1.f);

bool bFoundAValidSolution = false;

// trace if desired
if (TraceOption == ESuggestProjVelocityTraceOption::DoNotTrace)
{
// choose which arc
const float FavoredMagXYSq = bFavorHighArc ? FMath::Min(MagXYSq_A, MagXYSq_B) : FMath::Max(MagXYSq_A, MagXYSq_B);
const float ZSign = bFavorHighArc ?
(MagXYSq_A < MagXYSq_B) ? FMath::Sign(TanSolutionAngleA) : FMath::Sign(TanSolutionAngleB) :
(MagXYSq_A > MagXYSq_B) ? FMath::Sign(TanSolutionAngleA) : FMath::Sign(TanSolutionAngleB);

// finish calculations
const float MagXY = FMath::Sqrt(FavoredMagXYSq);
const float MagZ = FMath::Sqrt(TossSpeedSq - FavoredMagXYSq); // pythagorean

// final answer!
OutTossVelocity = (DirXY * MagXY) + (FVector::UpVector * MagZ * ZSign);
bFoundAValidSolution = true;

#if ENABLE_DRAW_DEBUG
if (bDrawDebug)
{
static const float StepSize = 0.125f;
FVector TraceStart = Start;
for ( float Step=0.f; Step<1.f; Step+=StepSize )
{
const float TimeInFlight = (Step+StepSize) * DeltaXY/MagXY;
// d = vt + .5 a t^2
const FVector TraceEnd = Start + OutTossVelocity*TimeInFlight + FVector(0.f, 0.f, 0.5f * -GravityZ * FMath::Square(TimeInFlight) - CollisionRadius);
DrawDebugLine( World, TraceStart, TraceEnd, (bFoundAValidSolution ? FColor::Yellow : FColor::Red), true );
TraceStart = TraceEnd;
}
}#endif // ENABLE_DRAW_DEBUG
}
else
{
// need to trace to validate

// sort potential solutions by priority
float PrioritizedSolutionsMagXYSq[2];
PrioritizedSolutionsMagXYSq[0] = bFavorHighArc ? FMath::Min(MagXYSq_A, MagXYSq_B) : FMath::Max(MagXYSq_A, MagXYSq_B);
PrioritizedSolutionsMagXYSq[1] = bFavorHighArc ? FMath::Max(MagXYSq_A, MagXYSq_B) : FMath::Min(MagXYSq_A, MagXYSq_B);

float PrioritizedSolutionZSign[2];
PrioritizedSolutionZSign[0] = bFavorHighArc ?
(MagXYSq_A < MagXYSq_B) ? FMath::Sign(TanSolutionAngleA) : FMath::Sign(TanSolutionAngleB) :
(MagXYSq_A > MagXYSq_B) ? FMath::Sign(TanSolutionAngleA) : FMath::Sign(TanSolutionAngleB);
PrioritizedSolutionZSign[1] = bFavorHighArc ?
(MagXYSq_A > MagXYSq_B) ? FMath::Sign(TanSolutionAngleA) : FMath::Sign(TanSolutionAngleB) :
(MagXYSq_A < MagXYSq_B) ? FMath::Sign(TanSolutionAngleA) : FMath::Sign(TanSolutionAngleB);

FVector PrioritizedProjVelocities[2];

// try solutions in priority order
int32 ValidSolutionIdx = INDEX_NONE;
for (int32 CurrentSolutionIdx=0; (CurrentSolutionIdx<2); ++CurrentSolutionIdx)
{
const float MagXY = FMath::Sqrt( PrioritizedSolutionsMagXYSq[CurrentSolutionIdx] );
const float MagZ = FMath::Sqrt( TossSpeedSq - PrioritizedSolutionsMagXYSq[CurrentSolutionIdx] ); // pythagorean
const float ZSign = PrioritizedSolutionZSign[CurrentSolutionIdx];

PrioritizedProjVelocities[CurrentSolutionIdx] = (DirXY * MagXY) + (FVector::UpVector * MagZ * ZSign);

// iterate along the arc, doing stepwise traces
bool bFailedTrace = false;
static const float StepSize = 0.125f;
FVector TraceStart = Start;
for ( float Step=0.f; Step<1.f; Step+=StepSize )
{
const float TimeInFlight = (Step+StepSize) * DeltaXY/MagXY;

// d = vt + .5 a t^2
const FVector TraceEnd = Start + PrioritizedProjVelocities[CurrentSolutionIdx]*TimeInFlight + FVector(0.f, 0.f, 0.5f * -GravityZ * FMath::Square(TimeInFlight) - CollisionRadius);

if ( (TraceOption == ESuggestProjVelocityTraceOption::OnlyTraceWhileAscending) && (TraceEnd.Z < TraceStart.Z) )
{
// falling, we are done tracing
if (!bDrawDebug)
{
// if we're drawing, we continue stepping without the traces
// else we can just trivially end the iteration loop
break;
}
}
else
{
FCollisionQueryParams QueryParams(SCENE_QUERY_STAT(SuggestProjVelTrace), true);
QueryParams.AddIgnoredActors(ActorsToIgnore);
if (World->SweepTestByChannel(TraceStart, TraceEnd, FQuat::Identity, ECC_WorldDynamic, FCollisionShape::MakeSphere(CollisionRadius), QueryParams, ResponseParam))
{
// hit something, failed
bFailedTrace = true;

#if ENABLE_DRAW_DEBUG
if (bDrawDebug)
{
// draw failed segment in red
DrawDebugLine( World, TraceStart, TraceEnd, FColor::Red, true );
}#endif // ENABLE_DRAW_DEBUG

break;
}

}

#if ENABLE_DRAW_DEBUG
if (bDrawDebug)
{
DrawDebugLine( World, TraceStart, TraceEnd, FColor::Yellow, true );
}#endif // ENABLE_DRAW_DEBUG

// advance
TraceStart = TraceEnd;
}

if (bFailedTrace == false)
{
// passes all traces along the arc, we have a valid solution and can be done
ValidSolutionIdx = CurrentSolutionIdx;
break;
}
}

if (ValidSolutionIdx != INDEX_NONE)
{
OutTossVelocity = PrioritizedProjVelocities[ValidSolutionIdx];
bFoundAValidSolution = true;
}
}

return bFoundAValidSolution;}