void UCrowdFollowingComponent::FollowPathSegment(float DeltaTime)
{
if (!bEnableCrowdSimulation)
{
Super::FollowPathSegment(DeltaTime);
return;
}
if (bUpdateDirectMoveVelocity)
{
const FVector CurrentTargetPt = DestinationActor.IsValid() ? DestinationActor->GetActorLocation() : GetCurrentTargetLocation();
const FVector AgentLoc = GetCrowdAgentLocation();
const FVector NewDirection = (CurrentTargetPt - AgentLoc).GetSafeNormal();
const bool bDirectionChanged = !NewDirection.Equals(CrowdAgentMoveDirection);
if (bDirectionChanged)
{
CurrentDestination.Set(Path->GetBaseActor(), CurrentTargetPt);
CrowdAgentMoveDirection = NewDirection;
MoveSegmentDirection = NewDirection;
UCrowdManager* Manager = UCrowdManager::GetCurrent(GetWorld());
Manager->SetAgentMoveDirection(this, NewDirection);
UE_VLOG(GetOwner(), LogCrowdFollowing, Log, TEXT("Updated direct move direction for crowd agent."));
}
}
UpdateMoveFocus();
}
void UCrowdFollowingComponent::FollowPathSegment(float DeltaTime)
{
if (!bEnableCrowdSimulation)
{
Super::FollowPathSegment(DeltaTime);
return;
}
if (bUpdateDirectMoveVelocity && DestinationActor.IsValid())
{
const FVector CurrentTargetPt = DestinationActor->GetActorLocation();
const float DistSq = (CurrentTargetPt - GetCurrentTargetLocation()).SizeSquared();
if (DistSq > FMath::Square(10.0f))
{
UCrowdManager* Manager = UCrowdManager::GetCurrent(GetWorld());
const FVector AgentLoc = GetCrowdAgentLocation();
CurrentDestination.Set(Path->GetBaseActor(), CurrentTargetPt);
CrowdAgentMoveDirection = (CurrentTargetPt - AgentLoc).GetSafeNormal();
MoveSegmentDirection = CrowdAgentMoveDirection;
Manager->SetAgentMoveDirection(this, MoveSegmentDirection);
UE_VLOG(GetOwner(), LogCrowdFollowing, Log, TEXT("Updated direct move direction for crowd agent."));
}
}
UpdateMoveFocus();
}
void UCrowdFollowingComponent::OnLanded()
{
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (bEnableCrowdSimulation && CrowdManager)
{
const ICrowdAgentInterface* IAgent = Cast<ICrowdAgentInterface>(this);
CrowdManager->UpdateAgentState(IAgent);
}
}
void UCrowdFollowingComponent::UpdateCrowdAgentParams() const
{
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
const ICrowdAgentInterface* IAgent = Cast<ICrowdAgentInterface>(this);
CrowdManager->UpdateAgentParams(IAgent);
}
}
void UCrowdFollowingComponent::OnPathFinished(EPathFollowingResult::Type Result)
{
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (bEnableCrowdSimulation && CrowdManager)
{
CrowdManager->ClearAgentMoveTarget(this);
}
Super::OnPathFinished(Result);
}
void UCrowdFollowingComponent::Cleanup()
{
Super::Cleanup();
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
const ICrowdAgentInterface* IAgent = Cast<ICrowdAgentInterface>(this);
CrowdManager->UnregisterAgent(IAgent);
}
}
void UCrowdFollowingComponent::PauseMove(FAIRequestID RequestID, bool bResetVelocity)
{
if (bEnableCrowdSimulation && (Status != EPathFollowingStatus::Paused) && RequestID.IsEquivalent(GetCurrentRequestId()))
{
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
CrowdManager->PauseAgent(this);
}
}
Super::PauseMove(RequestID, bResetVelocity);
}
void UCrowdFollowingComponent::AbortMove(const FString& Reason, FAIRequestID RequestID, bool bResetVelocity, bool bSilent, uint8 MessageFlags)
{
if (bEnableCrowdSimulation && (Status != EPathFollowingStatus::Idle) && RequestID.IsEquivalent(GetCurrentRequestId()))
{
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
CrowdManager->ClearAgentMoveTarget(this);
}
}
Super::AbortMove(Reason, RequestID, bResetVelocity, bSilent, MessageFlags);
}
void UCrowdFollowingComponent::Initialize()
{
Super::Initialize();
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
ICrowdAgentInterface* IAgent = Cast<ICrowdAgentInterface>(this);
CrowdManager->RegisterAgent(IAgent);
}
else
{
bEnableCrowdSimulation = false;
}
}
void UCrowdFollowingComponent::FinishUsingCustomLink(INavLinkCustomInterface* CustomNavLink)
{
const bool bPrevCustomLink = CurrentCustomLinkOb.IsValid();
Super::FinishUsingCustomLink(CustomNavLink);
if (bEnableCrowdSimulation)
{
const bool bCurrentCustomLink = CurrentCustomLinkOb.IsValid();
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (bPrevCustomLink && !bCurrentCustomLink && CrowdManager)
{
const ICrowdAgentInterface* IAgent = Cast<ICrowdAgentInterface>(this);
CrowdManager->OnAgentFinishedCustomLink(IAgent);
}
}
}
void UCrowdFollowingComponent::ResumeMove(FAIRequestID RequestID)
{
if (bEnableCrowdSimulation && (Status == EPathFollowingStatus::Paused) && RequestID.IsEquivalent(GetCurrentRequestId()))
{
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
const bool bReplanPath = bEnableSimulationReplanOnResume && HasMovedDuringPause();
CrowdManager->ResumeAgent(this, bReplanPath);
}
// reset cached direction, will be set again after velocity update
// but before it happens do not change actor's focus point (rotation)
CrowdAgentMoveDirection = FVector::ZeroVector;
}
Super::ResumeMove(RequestID);
}
void UCrowdFollowingComponent::DescribeSelfToVisLog(FVisualLogEntry* Snapshot) const
{
if (!bEnableCrowdSimulation)
{
Super::DescribeSelfToVisLog(Snapshot);
return;
}
FVisualLogStatusCategory Category;
Category.Category = TEXT("Path following");
if (DestinationActor.IsValid())
{
Category.Add(TEXT("Goal"), GetNameSafe(DestinationActor.Get()));
}
FString StatusDesc = GetStatusDesc();
FNavMeshPath* NavMeshPath = Path.IsValid() ? Path->CastPath<FNavMeshPath>() : NULL;
FAbstractNavigationPath* DirectPath = Path.IsValid() ? Path->CastPath<FAbstractNavigationPath>() : NULL;
if (Status == EPathFollowingStatus::Moving)
{
StatusDesc += FString::Printf(TEXT(" [path:%d, visited:%d]"), PathStartIndex, LastPathPolyIndex);
}
Category.Add(TEXT("Status"), StatusDesc);
Category.Add(TEXT("Path"), !Path.IsValid() ? TEXT("none") : NavMeshPath ? TEXT("navmesh") : DirectPath ? TEXT("direct") : TEXT("unknown"));
UObject* CustomLinkOb = GetCurrentCustomLinkOb();
if (CustomLinkOb)
{
Category.Add(TEXT("SmartLink"), CustomLinkOb->GetName());
}
UCrowdManager* Manager = UCrowdManager::GetCurrent(GetWorld());
if (Manager && !Manager->IsAgentValid(this))
{
Category.Add(TEXT("Simulation"), TEXT("unable to register!"));
}
Snapshot->Status.Add(Category);
}
void UCrowdFollowingComponent::GetDebugStringTokens(TArray<FString>& Tokens, TArray<EPathFollowingDebugTokens::Type>& Flags) const
{
if (!bEnableCrowdSimulation)
{
Super::GetDebugStringTokens(Tokens, Flags);
return;
}
Tokens.Add(GetStatusDesc());
Flags.Add(EPathFollowingDebugTokens::Description);
UCrowdManager* Manager = UCrowdManager::GetCurrent(GetWorld());
if (Manager && !Manager->IsAgentValid(this))
{
Tokens.Add(TEXT("simulation"));
Flags.Add(EPathFollowingDebugTokens::ParamName);
Tokens.Add(TEXT("NOT ACTIVE"));
Flags.Add(EPathFollowingDebugTokens::FailedValue);
}
if (Status != EPathFollowingStatus::Moving)
{
return;
}
FString& StatusDesc = Tokens[0];
if (Path.IsValid())
{
FNavMeshPath* NavMeshPath = Path->CastPath<FNavMeshPath>();
if (NavMeshPath)
{
StatusDesc += FString::Printf(TEXT(" (path:%d, visited:%d)"), PathStartIndex, LastPathPolyIndex);
}
else if (Path->CastPath<FAbstractNavigationPath>())
{
StatusDesc += TEXT(" (direct)");
}
else
{
StatusDesc += TEXT(" (unknown path)");
}
}
// get cylinder of moving agent
float AgentRadius = 0.0f;
float AgentHalfHeight = 0.0f;
AActor* MovingAgent = MovementComp->GetOwner();
MovingAgent->GetSimpleCollisionCylinder(AgentRadius, AgentHalfHeight);
if (bFinalPathPart)
{
float CurrentDot = 0.0f, CurrentDistance = 0.0f, CurrentHeight = 0.0f;
uint8 bFailedDot = 0, bFailedDistance = 0, bFailedHeight = 0;
DebugReachTest(CurrentDot, CurrentDistance, CurrentHeight, bFailedHeight, bFailedDistance, bFailedHeight);
Tokens.Add(TEXT("dist2D"));
Flags.Add(EPathFollowingDebugTokens::ParamName);
Tokens.Add(FString::Printf(TEXT("%.0f"), CurrentDistance));
Flags.Add(bFailedDistance ? EPathFollowingDebugTokens::FailedValue : EPathFollowingDebugTokens::PassedValue);
Tokens.Add(TEXT("distZ"));
Flags.Add(EPathFollowingDebugTokens::ParamName);
Tokens.Add(FString::Printf(TEXT("%.0f"), CurrentHeight));
Flags.Add(bFailedHeight ? EPathFollowingDebugTokens::FailedValue : EPathFollowingDebugTokens::PassedValue);
}
else
{
const FVector CurrentLocation = MovementComp->GetActorFeetLocation();
// make sure we're not too close to end of path part (poly count can always fail when AI goes off path)
const float DistSq = (GetCurrentTargetLocation() - CurrentLocation).SizeSquared();
const float PathSwitchThresSq = FMath::Square(AgentRadius * 5.0f);
Tokens.Add(TEXT("distance"));
Flags.Add(EPathFollowingDebugTokens::ParamName);
Tokens.Add(FString::Printf(TEXT("%.0f"), FMath::Sqrt(DistSq)));
Flags.Add((DistSq < PathSwitchThresSq) ? EPathFollowingDebugTokens::PassedValue : EPathFollowingDebugTokens::FailedValue);
}
}
void UCrowdFollowingComponent::SetMoveSegment(int32 SegmentStartIndex)
{
if (!bEnableCrowdSimulation)
{
Super::SetMoveSegment(SegmentStartIndex);
return;
}
PathStartIndex = SegmentStartIndex;
LastPathPolyIndex = PathStartIndex;
if (Path.IsValid() == false || Path->IsValid() == false || GetOwner() == NULL)
{
return;
}
FVector CurrentTargetPt = Path->GetPathPoints()[1].Location;
FNavMeshPath* NavMeshPath = Path->CastPath<FNavMeshPath>();
FAbstractNavigationPath* DirectPath = Path->CastPath<FAbstractNavigationPath>();
if (NavMeshPath)
{
#if WITH_RECAST
if (NavMeshPath->PathCorridor.IsValidIndex(PathStartIndex) == false)
{
// this should never matter, but just in case
UE_VLOG(GetOwner(), LogCrowdFollowing, Error, TEXT("SegmentStartIndex in call to UCrowdFollowingComponent::SetMoveSegment is out of path corridor array's bounds (index: %d, array size %d)")
, PathStartIndex, NavMeshPath->PathCorridor.Num());
PathStartIndex = FMath::Clamp<int32>(PathStartIndex, 0, NavMeshPath->PathCorridor.Num() - 1);
}
// cut paths into parts to avoid problems with crowds getting into local minimum
// due to using only first 10 steps of A*
// do NOT use PathPoints here, crowd simulation disables path post processing
// which means, that PathPoints contains only start and end position
// full path is available through PathCorridor array (poly refs)
ARecastNavMesh* RecastNavData = Cast<ARecastNavMesh>(MyNavData);
const int32 PathPartSize = 15;
const int32 LastPolyIdx = NavMeshPath->PathCorridor.Num() - 1;
int32 PathPartEndIdx = FMath::Min(PathStartIndex + PathPartSize, LastPolyIdx);
FVector PtA, PtB;
const bool bStartIsNavLink = RecastNavData->GetLinkEndPoints(NavMeshPath->PathCorridor[PathStartIndex], PtA, PtB);
const bool bEndIsNavLink = RecastNavData->GetLinkEndPoints(NavMeshPath->PathCorridor[PathPartEndIdx], PtA, PtB);
if (bStartIsNavLink)
{
PathStartIndex = FMath::Max(0, PathStartIndex - 1);
}
if (bEndIsNavLink)
{
PathPartEndIdx = FMath::Max(0, PathPartEndIdx - 1);
}
bFinalPathPart = (PathPartEndIdx == LastPolyIdx);
if (!bFinalPathPart)
{
RecastNavData->GetPolyCenter(NavMeshPath->PathCorridor[PathPartEndIdx], CurrentTargetPt);
}
else if (NavMeshPath->IsPartial())
{
RecastNavData->GetClosestPointOnPoly(NavMeshPath->PathCorridor[PathPartEndIdx], Path->GetPathPoints()[1].Location, CurrentTargetPt);
}
// not safe to read those directions yet, you have to wait until crowd manager gives you next corner of string pulled path
CrowdAgentMoveDirection = FVector::ZeroVector;
MoveSegmentDirection = FVector::ZeroVector;
CurrentDestination.Set(Path->GetBaseActor(), CurrentTargetPt);
LogPathPartHelper(GetOwner(), NavMeshPath, PathStartIndex, PathPartEndIdx);
UE_VLOG_SEGMENT(GetOwner(), LogCrowdFollowing, Log, MovementComp->GetActorFeetLocation(), CurrentTargetPt, FColor::Red, TEXT("path part"));
UE_VLOG(GetOwner(), LogCrowdFollowing, Log, TEXT("SetMoveSegment, from:%d segments:%d%s"),
PathStartIndex, (PathPartEndIdx - PathStartIndex)+1, bFinalPathPart ? TEXT(" (final)") : TEXT(""));
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
CrowdManager->SetAgentMovePath(this, NavMeshPath, PathStartIndex, PathPartEndIdx);
}
#endif
}
else if (DirectPath)
{
// direct paths are not using any steering or avoidance
// pathfinding is replaced with simple velocity request
const FVector AgentLoc = MovementComp->GetActorFeetLocation();
bFinalPathPart = true;
bCheckMovementAngle = true;
bUpdateDirectMoveVelocity = true;
CurrentDestination.Set(Path->GetBaseActor(), CurrentTargetPt);
CrowdAgentMoveDirection = (CurrentTargetPt - AgentLoc).GetSafeNormal();
MoveSegmentDirection = CrowdAgentMoveDirection;
UE_VLOG(GetOwner(), LogCrowdFollowing, Log, TEXT("SetMoveSegment, direct move"));
UE_VLOG_SEGMENT(GetOwner(), LogCrowdFollowing, Log, AgentLoc, CurrentTargetPt, FColor::Red, TEXT("path"));
UCrowdManager* CrowdManager = UCrowdManager::GetCurrent(GetWorld());
if (CrowdManager)
{
CrowdManager->SetAgentMoveDirection(this, CrowdAgentMoveDirection);
}
}
else
{
UE_VLOG(GetOwner(), LogCrowdFollowing, Error, TEXT("SetMoveSegment, unknown path type!"));
}
}