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;
}
void AORCAManager::SimulateORCA(float DeltaTime)
{
solver->ClearAgents();
for (UAvoidanceComponent* av : units)
{
if (!av || av->IsPendingKill() || av->IsBeingDestroyed() || !av->pawn || av->pawn->IsPendingKill() || av->pawn->IsActorBeingDestroyed())
{
continue;
}
FVector2D pos{ av->pawn->GetActorLocation() };
FVector2D vel{ av->pawn->GetVelocity() };
if (vel.SizeSquared() > av->MaxVelocity * av->MaxVelocity)
{
vel.Normalize();
vel *= av->MaxVelocity;
}
FVector2D PreferredVelocity = av->GetPreferredVelocity();
if (PreferredVelocity.ContainsNaN())
{
UE_LOG(LogRVOTest, VeryVerbose, TEXT("AvoidanceComponent:: TICK , preferredvelocity: %f %f"), PreferredVelocity.X, PreferredVelocity.Y);
PreferredVelocity = av->GetPreferredVelocity();
}
Agent agent{ pos.X, pos.Y, vel.X, vel.Y, av->radius, PreferredVelocity.X, PreferredVelocity.Y, av->MaxVelocity, av->MaxAcceleration * DeltaTime };
solver->GetAgent(solver->AddAgent()) = agent;
}
CalculateNearest();
solver->ComputeNewVelocities();
int id = -1;
for (UAvoidanceComponent* av : units)
{
if (!av || av->IsPendingKill())
{
continue;
}
id++;
Agent& agent = solver->GetAgent(id);
FVector input{ agent.vx_new - agent.vx, agent.vy_new - agent.vy, 0.f };
FVector inputDir;
float inputLength;
input.ToDirectionAndLength(inputDir, inputLength);
av->SetNewAvoidanceVelocity(FVector2D{ agent.vx_new, agent.vy_new }, FVector2D{ inputDir }, inputLength / agent.maxAccMagnitude);
UE_LOG(LogRVOTest, VeryVerbose, TEXT("orcaman:: new vel: %f %f"), agent.vx_new, agent.vy_new);
if (FVector2D{ agent.vx_new, agent.vy_new }.ContainsNaN())
{
UE_LOG(LogRVOTest, VeryVerbose, TEXT("BAM ORCAMan szivas"));
}
}
}
