void AAIController::FindPathForMoveRequest(const FAIMoveRequest& MoveRequest, FPathFindingQuery& Query, FNavPathSharedPtr& OutPath) const
{
SCOPE_CYCLE_COUNTER(STAT_AI_Overall);
UNavigationSystem* NavSys = UNavigationSystem::GetCurrent(GetWorld());
if (NavSys)
{
FPathFindingResult PathResult = NavSys->FindPathSync(Query);
if (PathResult.Result != ENavigationQueryResult::Error)
{
if (PathResult.IsSuccessful() && PathResult.Path.IsValid())
{
if (MoveRequest.IsMoveToActorRequest())
{
PathResult.Path->SetGoalActorObservation(*MoveRequest.GetGoalActor(), 100.0f);
}
PathResult.Path->EnableRecalculationOnInvalidation(true);
OutPath = PathResult.Path;
}
}
else
{
UE_VLOG(this, LogAINavigation, Error, TEXT("Trying to find path to %s resulted in Error")
, MoveRequest.IsMoveToActorRequest() ? *GetNameSafe(MoveRequest.GetGoalActor()) : *MoveRequest.GetGoalLocation().ToString());
UE_VLOG_SEGMENT(this, LogAINavigation, Error, GetPawn() ? GetPawn()->GetActorLocation() : FAISystem::InvalidLocation
, MoveRequest.GetGoalLocation(), FColor::Red, TEXT("Failed move to %s"), *GetNameSafe(MoveRequest.GetGoalActor()));
}
}
}
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!"));
}
}
void UCrowdManager::DebugTick() const
{
#if WITH_RECAST
if (DetourCrowd == NULL || DetourAgentDebug == NULL)
{
return;
}
for (auto It = ActiveAgents.CreateConstIterator(); It; ++It)
{
const FCrowdAgentData& AgentData = It.Value();
if (AgentData.IsValid())
{
UpdateSelectedDebug(It.Key(), AgentData.AgentIndex);
}
}
// on screen debugging
const dtCrowdAgent* SelectedAgent = DetourAgentDebug->idx >= 0 ? DetourCrowd->getAgent(DetourAgentDebug->idx) : NULL;
if (SelectedAgent && CrowdDebugDrawing::bDebugSelectedActors)
{
if (CrowdDebugDrawing::bDrawDebugCorners)
{
DrawDebugCorners(SelectedAgent);
}
if (CrowdDebugDrawing::bDrawDebugCollisionSegments)
{
DrawDebugCollisionSegments(SelectedAgent);
}
if (CrowdDebugDrawing::bDrawDebugPath)
{
DrawDebugPath(SelectedAgent);
}
if (CrowdDebugDrawing::bDrawDebugVelocityObstacles)
{
DrawDebugVelocityObstacles(SelectedAgent);
}
if (CrowdDebugDrawing::bDrawDebugPathOptimization)
{
DrawDebugPathOptimization(SelectedAgent);
}
if (CrowdDebugDrawing::bDrawDebugNeighbors)
{
DrawDebugNeighbors(SelectedAgent);
}
}
if (CrowdDebugDrawing::bDrawDebugBoundaries)
{
DrawDebugSharedBoundary();
}
// vislog debugging
if (CrowdDebugDrawing::bDebugVisLog)
{
for (auto It = ActiveAgents.CreateConstIterator(); It; ++It)
{
const ICrowdAgentInterface* IAgent = It.Key();
const UObject* AgentOb = IAgent ? Cast<const UObject>(IAgent) : NULL;
const AActor* LogOwner = AgentOb ? Cast<const AActor>(AgentOb->GetOuter()) : NULL;
const FCrowdAgentData& AgentData = It.Value();
const dtCrowdAgent* CrowdAgent = AgentData.IsValid() ? DetourCrowd->getAgent(AgentData.AgentIndex) : NULL;
if (CrowdAgent && LogOwner)
{
FString LogData = DetourAgentDebug->agentLog.FindRef(AgentData.AgentIndex);
if (LogData.Len() > 0)
{
UE_VLOG(LogOwner, LogCrowdFollowing, Log, *LogData);
}
{
FVector P0 = Recast2UnrealPoint(CrowdAgent->npos);
for (int32 Idx = 0; Idx < CrowdAgent->ncorners; Idx++)
{
FVector P1 = Recast2UnrealPoint(&CrowdAgent->cornerVerts[Idx * 3]);
UE_VLOG_SEGMENT(LogOwner, LogCrowdFollowing, Log, P0 + CrowdDebugDrawing::Offset, P1 + CrowdDebugDrawing::Offset, CrowdDebugDrawing::Corner, TEXT(""));
UE_VLOG_BOX(LogOwner, LogCrowdFollowing, Log, FBox::BuildAABB(P1 + CrowdDebugDrawing::Offset, FVector(2, 2, 2)), CrowdDebugDrawing::Corner, TEXT("%d"), CrowdAgent->cornerFlags[Idx]);
P0 = P1;
}
}
ARecastNavMesh* RecastNavData = Cast<ARecastNavMesh>(MyNavData);
if (RecastNavData)
{
for (int32 Idx = 0; Idx < CrowdAgent->corridor.getPathCount(); Idx++)
{
dtPolyRef PolyRef = CrowdAgent->corridor.getPath()[Idx];
TArray<FVector> PolyPoints;
RecastNavData->GetPolyVerts(PolyRef, PolyPoints);
UE_VLOG_CONVEXPOLY(LogOwner, LogCrowdFollowing, Verbose, PolyPoints, FColor::Cyan, TEXT(""));
}
}
if (CrowdAgent->ncorners && (CrowdAgent->cornerFlags[CrowdAgent->ncorners - 1] & DT_STRAIGHTPATH_OFFMESH_CONNECTION))
{
FVector P0 = Recast2UnrealPoint(&CrowdAgent->cornerVerts[(CrowdAgent->ncorners - 1) * 3]);
UE_VLOG_SEGMENT(LogOwner, LogCrowdFollowing, Log, P0, P0 + CrowdDebugDrawing::Offset * 2.0f, CrowdDebugDrawing::CornerLink, TEXT(""));
}
if (CrowdAgent->corridor.hasNextFixedCorner())
{
FVector P0 = Recast2UnrealPoint(CrowdAgent->corridor.getNextFixedCorner());
UE_VLOG_BOX(LogOwner, LogCrowdFollowing, Log, FBox::BuildAABB(P0 + CrowdDebugDrawing::Offset, FVector(10, 10, 10)), CrowdDebugDrawing::CornerFixed, TEXT(""));
}
if (CrowdAgent->corridor.hasNextFixedCorner2())
{
FVector P0 = Recast2UnrealPoint(CrowdAgent->corridor.getNextFixedCorner2());
UE_VLOG_BOX(LogOwner, LogCrowdFollowing, Log, FBox::BuildAABB(P0 + CrowdDebugDrawing::Offset, FVector(10, 10, 10)), CrowdDebugDrawing::CornerFixed, TEXT(""));
}
for (int32 Idx = 0; Idx < CrowdAgent->boundary.getSegmentCount(); Idx++)
{
const float* s = CrowdAgent->boundary.getSegment(Idx);
const int32 SegFlags = CrowdAgent->boundary.getSegmentFlags(Idx);
const FColor Color = (SegFlags & DT_CROWD_BOUNDARY_IGNORE) ? CrowdDebugDrawing::CollisionSegIgnored :
(dtTriArea2D(CrowdAgent->npos, s, s + 3) < 0.0f) ? CrowdDebugDrawing::CollisionSeg1 :
CrowdDebugDrawing::CollisionSeg0;
FVector Pt0 = Recast2UnrealPoint(s);
FVector Pt1 = Recast2UnrealPoint(s + 3);
UE_VLOG_SEGMENT_THICK(LogOwner, LogCrowdFollowing, Log, Pt0 + CrowdDebugDrawing::Offset, Pt1 + CrowdDebugDrawing::Offset, Color, 3.0f, TEXT(""));
}
}
}
}
DetourAgentDebug->agentLog.Reset();
#endif // WITH_RECAST
}
bool UNavigationComponent::GeneratePathTo(const FVector& GoalLocation, TSharedPtr<const FNavigationQueryFilter> QueryFilter)
{
if (GetWorld() == NULL || GetWorld()->GetNavigationSystem() == NULL || GetOuter() == NULL)
{
return false;
}
// Make sure we're trying to path to the closest valid location TO that location rather than the absolute location.
// After talking with Steve P., if we need to ever allow pathing WITHOUT projecting to nav-mesh, it will probably be
// best to do so using a flag while keeping this as the default behavior. NOTE:` If any changes to this behavior
// are made, you should search for other places in UNavigationComponent using NavMeshGoalLocation and make sure the
// behavior remains consistent.
// make sure that nav data is updated
GetNavData();
const FNavAgentProperties* NavAgentProps = MyNavAgent ? MyNavAgent->GetNavAgentProperties() : NULL;
if (NavAgentProps != NULL)
{
FVector NavMeshGoalLocation;
#if ENABLE_VISUAL_LOG
UE_VLOG_LOCATION(GetOwner(), GoalLocation, 34, FColor(0,127,14), TEXT("GoalLocation"));
const FVector ProjectionExtent = MyNavData ? MyNavData->GetDefaultQueryExtent() : FVector(DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL, DEFAULT_NAV_QUERY_EXTENT_HORIZONTAL, DEFAULT_NAV_QUERY_EXTENT_VERTICAL);
UE_VLOG_BOX(GetOwner(), FBox(GoalLocation - ProjectionExtent, GoalLocation + ProjectionExtent), FColor::Green, TEXT_EMPTY);
#endif
if (ProjectPointToNavigation(GoalLocation, NavMeshGoalLocation) == QuerySuccess)
{
UE_VLOG_SEGMENT(GetOwner(), GoalLocation, NavMeshGoalLocation, FColor::Blue, TEXT_EMPTY);
UE_VLOG_LOCATION(GetOwner(), NavMeshGoalLocation, 34, FColor::Blue, TEXT_EMPTY);
UNavigationSystem* NavSys = GetWorld()->GetNavigationSystem();
FPathFindingQuery Query(MyNavData, MyNavAgent->GetNavAgentLocation(), NavMeshGoalLocation, QueryFilter);
const EPathFindingMode::Type Mode = bDoHierarchicalPathfinding ? EPathFindingMode::Hierarchical : EPathFindingMode::Regular;
FPathFindingResult Result = NavSys->FindPathSync(*MyNavAgent->GetNavAgentProperties(), Query, Mode);
// try reversing direction
if (bSearchFromGoalWhenOutOfNodes && !bDoHierarchicalPathfinding &&
Result.Path.IsValid() && Result.Path->DidSearchReachedLimit())
{
// quick check if path exists
bool bPathExists = false;
{
DECLARE_SCOPE_CYCLE_COUNTER(TEXT("Pathfinding: HPA* test time"), STAT_Navigation_PathVerifyTime, STATGROUP_Navigation);
bPathExists = NavSys->TestPathSync(Query, EPathFindingMode::Hierarchical);
}
UE_VLOG(GetOwner(), LogNavigation, Log, TEXT("GeneratePathTo: out of nodes, HPA* path: %s"),
bPathExists ? TEXT("exists, trying reversed search") : TEXT("doesn't exist"));
// reverse search
if (bPathExists)
{
DECLARE_SCOPE_CYCLE_COUNTER(TEXT("Pathfinding: reversed test time"), STAT_Navigation_PathReverseTime, STATGROUP_Navigation);
TSharedPtr<FNavigationQueryFilter> Filter = QueryFilter.IsValid() ? QueryFilter->GetCopy() : MyNavData->GetDefaultQueryFilter()->GetCopy();
Filter->SetBacktrackingEnabled(true);
FPathFindingQuery ReversedQuery(MyNavData, Query.EndLocation, Query.StartLocation, Filter);
Result = NavSys->FindPathSync(*MyNavAgent->GetNavAgentProperties(), Query, Mode);
}
}
if (Result.IsSuccessful() || Result.IsPartial())
{
CurrentGoal = NavMeshGoalLocation;
SetPath(Result.Path);
if (IsFollowing() == true)
{
// make sure ticking is enabled (and it shouldn't be enabled before _first_ async path finding)
SetComponentTickEnabledAsync(true);
}
NotifyPathUpdate();
return true;
}
else
{
UE_VLOG(GetOwner(), LogNavigation, Display, TEXT("Failed to generate path to destination"));
}
}
else
{
UE_VLOG(GetOwner(), LogNavigation, Display, TEXT("Destination not on navmesh (and nowhere near!)"));
}
}
else
{
UE_VLOG(GetOwner(), LogNavigation, Display, TEXT("UNavigationComponent::GeneratePathTo: NavAgentProps == NULL (Probably Pawn died)"));
}
return false;
}
