FVector2D UAvoidanceComponent::GetPreferredVelocity()
{
FVector2D ToTarget;
if (bUseAITargetLocation && aiController)
{
ToTarget = FVector2D{ aiController->GetPathFollowingComponent()->GetCurrentTargetLocation() };
//FVector2D actLoc {pawn->GetActorLocation() };
//UE_LOG(LogRVOTest, Warning, TEXT("UAv::prefv GetCurrentTargetLocation, %f %f, actloc: %f %f"), CurrentTarget.X, CurrentTarget.Y, actLoc.X, actLoc.Y);
//UE_LOG(LogRVOTest, Warning, TEXT("UAv::Totarget: %f %f"), ToTarget.X, ToTarget.Y);
}
else
{
ToTarget = FVector2D{ CurrentTarget - FVector2D{ pawn->GetActorLocation() } };
}
float sqrDist = ToTarget.SizeSquared();
if (sqrDist < AcceptanceSquared)
return FVector2D::ZeroVector;
float m = 1.f / (SlowdownSquared - AcceptanceSquared);
float b = -m * AcceptanceSquared;
float k = (sqrDist < SlowdownSquared && pathFollowSlowdown) ? sqrDist * m + b : 1.f;
if (k < SMALL_NUMBER)
{
return FVector2D::ZeroVector;
}
FVector2D res = ToTarget.GetSafeNormal() * MaxVelocity * k;
if (res.ContainsNaN())
{
UE_LOG(LogRVOTest, VeryVerbose, TEXT("UAvoidanceComp:: GetPreferredV , %f %f"), res.X, res.Y);
}
return res;
}
FVector2D UKismetMathLibrary::Normal2D(FVector2D A)
{
return A.GetSafeNormal();
}
