FBoxSphereBounds UNavMeshRenderingComponent::CalcBounds(const FTransform& LocalToWorld) const
{
FBox BoundingBox(0);
#if WITH_RECAST
ARecastNavMesh* NavMesh = Cast<ARecastNavMesh>(GetOwner());
if (NavMesh)
{
BoundingBox = NavMesh->GetNavMeshBounds();
if (NavMesh->bDrawOctree)
{
const UNavigationSystem* NavSys = UNavigationSystem::GetCurrent(GetWorld());
const FNavigationOctree* NavOctree = NavSys ? NavSys->GetNavOctree() : NULL;
if (NavOctree)
{
for (FNavigationOctree::TConstIterator<> NodeIt(*NavOctree); NodeIt.HasPendingNodes(); NodeIt.Advance())
{
const FOctreeNodeContext& CorrentContext = NodeIt.GetCurrentContext();
BoundingBox += CorrentContext.Bounds.GetBox();
}
}
}
}
#endif
return FBoxSphereBounds(BoundingBox);
}
void ANavigationTestingActor::SearchPathTo(ANavigationTestingActor* Goal)
{
#if WITH_EDITORONLY_DATA
if (EdRenderComp)
{
EdRenderComp->MarkRenderStateDirty();
}
#endif // WITH_EDITORONLY_DATA
if (Goal == NULL)
{
return;
}
UNavigationSystem* NavSys = GetWorld()->GetNavigationSystem();
check(NavSys);
const double StartTime = FPlatformTime::Seconds();
FPathFindingQuery Query = BuildPathFindingQuery(Goal);
EPathFindingMode::Type Mode = bUseHierarchicalPathfinding ? EPathFindingMode::Hierarchical : EPathFindingMode::Regular;
FPathFindingResult Result = NavSys->FindPathSync(NavAgentProps, Query, Mode);
const double EndTime = FPlatformTime::Seconds();
const float Duration = (EndTime - StartTime);
PathfindingTime = Duration * 1000000.0f; // in micro seconds [us]
bPathIsPartial = Result.IsPartial();
bPathExist = Result.IsSuccessful();
bPathSearchOutOfNodes = bPathExist ? Result.Path->DidSearchReachedLimit() : false;
LastPath = Result.Path;
PathCost = bPathExist ? Result.Path->GetCost() : 0.0f;
if (bPathExist)
{
LastPath->AddObserver(PathObserver);
if (OffsetFromCornersDistance > 0.0f)
{
((FNavMeshPath*)LastPath.Get())->OffsetFromCorners(OffsetFromCornersDistance);
}
}
#if WITH_RECAST && WITH_EDITORONLY_DATA
if (bGatherDetailedInfo && !bUseHierarchicalPathfinding)
{
ARecastNavMesh* RecastNavMesh = Cast<ARecastNavMesh>(MyNavData);
if (RecastNavMesh && RecastNavMesh->HasValidNavmesh())
{
PathfindingSteps = RecastNavMesh->DebugPathfinding(Query, DebugSteps);
}
}
#endif
}
FBoxSphereBounds UNavMeshRenderingComponent::CalcBounds(const FTransform & LocalToWorld) const
{
FBox BoundingBox(0);
#if WITH_RECAST
ARecastNavMesh* NavMesh = Cast<ARecastNavMesh>(GetOwner());
if (NavMesh)
{
BoundingBox = NavMesh->GetNavMeshBounds();
}
#endif
return FBoxSphereBounds(BoundingBox);
}
void FGameplayDebuggerCategory_Navmesh::CollectData(APlayerController* OwnerPC, AActor* DebugActor)
{
#if WITH_RECAST
ARecastNavMesh* NavData = nullptr;
APawn* PlayerPawn = OwnerPC ? OwnerPC->GetPawnOrSpectator() : nullptr;
APawn* DebugActorAsPawn = Cast<APawn>(DebugActor);
APawn* DestPawn = DebugActorAsPawn ? DebugActorAsPawn : PlayerPawn;
if (DestPawn)
{
UNavigationSystem* NavSys = UNavigationSystem::GetCurrent(OwnerPC->GetWorld());
const FNavAgentProperties& NavAgentProperties = DestPawn->GetNavAgentPropertiesRef();
NavData = Cast<ARecastNavMesh>(NavSys->GetNavDataForProps(NavAgentProperties));
}
if (NavData)
{
// add 3x3 neighborhood of target
const FVector TargetLocation = DestPawn->GetActorLocation();
TArray<int32> TileSet;
int32 TileX = 0;
int32 TileY = 0;
const int32 DeltaX[] = { 0, 1, 1, 0, -1, -1, -1, 0, 1 };
const int32 DeltaY[] = { 0, 0, 1, 1, 1, 0, -1, -1, -1 };
int32 TargetTileX = 0;
int32 TargetTileY = 0;
NavData->GetNavMeshTileXY(TargetLocation, TargetTileX, TargetTileY);
for (int32 Idx = 0; Idx < ARRAY_COUNT(DeltaX); Idx++)
{
const int32 NeiX = TargetTileX + DeltaX[Idx];
const int32 NeiY = TargetTileY + DeltaY[Idx];
NavData->GetNavMeshTilesAt(NeiX, NeiY, TileSet);
}
const int32 DetailFlags =
(1 << static_cast<int32>(ENavMeshDetailFlags::PolyEdges)) |
(1 << static_cast<int32>(ENavMeshDetailFlags::FilledPolys)) |
(1 << static_cast<int32>(ENavMeshDetailFlags::NavLinks));
NavmeshRenderData.GatherData(NavData, DetailFlags, TileSet);
}
#endif // WITH_RECAST
}
bool UCrowdManager::SetAgentMovePath(const UCrowdFollowingComponent* AgentComponent, const FNavMeshPath* Path,
int32 PathSectionStart, int32 PathSectionEnd, const FVector& PathSectionEndLocation) const
{
SCOPE_CYCLE_COUNTER(STAT_AI_Crowd_AgentUpdateTime);
bool bSuccess = false;
#if WITH_RECAST
const FCrowdAgentData* AgentData = ActiveAgents.Find(AgentComponent);
ARecastNavMesh* RecastNavData = Cast<ARecastNavMesh>(MyNavData);
if (AgentData && AgentData->bIsSimulated && AgentData->IsValid() &&
DetourCrowd && RecastNavData &&
Path && (Path->GetPathPoints().Num() > 1) &&
Path->PathCorridor.IsValidIndex(PathSectionStart) && Path->PathCorridor.IsValidIndex(PathSectionEnd))
{
FVector TargetPos = PathSectionEndLocation;
if (PathSectionEnd < (Path->PathCorridor.Num() - 1))
{
RecastNavData->GetPolyCenter(Path->PathCorridor[PathSectionEnd], TargetPos);
}
TArray<dtPolyRef> PathRefs;
for (int32 Idx = PathSectionStart; Idx <= PathSectionEnd; Idx++)
{
PathRefs.Add(Path->PathCorridor[Idx]);
}
const INavigationQueryFilterInterface* NavFilter = Path->GetFilter().IsValid() ? Path->GetFilter()->GetImplementation() : MyNavData->GetDefaultQueryFilterImpl();
const dtQueryFilter* DetourFilter = ((const FRecastQueryFilter*)NavFilter)->GetAsDetourQueryFilter();
DetourCrowd->updateAgentFilter(AgentData->AgentIndex, DetourFilter);
DetourCrowd->updateAgentState(AgentData->AgentIndex, false);
const FVector RcTargetPos = Unreal2RecastPoint(TargetPos);
bSuccess = DetourCrowd->requestMoveTarget(AgentData->AgentIndex, PathRefs.Last(), &RcTargetPos.X);
if (bSuccess)
{
bSuccess = DetourCrowd->setAgentCorridor(AgentData->AgentIndex, PathRefs.GetData(), PathRefs.Num());
}
}
#endif
return bSuccess;
}
void UCrowdManager::UpdateNavData()
{
if (MyNavData == NULL)
{
UNavigationSystem* NavSys = Cast<UNavigationSystem>(GetOuter());
if (NavSys)
{
for (int32 Idx = 0; Idx < NavSys->NavDataSet.Num(); Idx++)
{
ARecastNavMesh* RecastNavData = Cast<ARecastNavMesh>(NavSys->NavDataSet[Idx]);
if (RecastNavData && RecastNavData->IsSupportingDefaultAgent())
{
MyNavData = RecastNavData;
RecastNavData->OnNavMeshUpdate.AddUObject(this, &UCrowdManager::OnNavMeshUpdate);
OnNavMeshUpdate();
break;
}
}
}
}
}
void FNavMeshPath::DescribeSelfToVisLog(FVisualLogEntry* Snapshot) const
{
if (IsStringPulled())
{
// draw path points only for string pulled paths
Super::DescribeSelfToVisLog(Snapshot);
}
// draw corridor
#if WITH_RECAST
FVisualLogShapeElement CorridorPoly(EVisualLoggerShapeElement::Polygon);
CorridorPoly.SetColor(FColorList::Cyan);
CorridorPoly.Category = LogNavigation.GetCategoryName();
CorridorPoly.Points.Reserve(PathCorridor.Num() * 6);
const FVector CorridorOffset = NavigationDebugDrawing::PathOffset * 1.25f;
int32 NumAreaMark = 1;
ARecastNavMesh* NavMesh = Cast<ARecastNavMesh>(GetNavigationDataUsed());
NavMesh->BeginBatchQuery();
TArray<FVector> Verts;
for (int32 Idx = 0; Idx < PathCorridor.Num(); Idx++)
{
Verts.Reset();
NavMesh->GetPolyVerts(PathCorridor[Idx], Verts);
CorridorPoly.Points.Reset();
CorridorPoly.Points.Append(Verts);
Snapshot->ElementsToDraw.Add(CorridorPoly);
const uint8 AreaID = NavMesh->GetPolyAreaID(PathCorridor[Idx]);
const UClass* AreaClass = NavMesh->GetAreaClass(AreaID);
if (AreaClass && AreaClass != UNavigationSystem::GetDefaultWalkableArea())
{
FVector CenterPt = FVector::ZeroVector;
for (int32 VIdx = 0; VIdx < Verts.Num(); VIdx++)
{
CenterPt += Verts[VIdx];
}
CenterPt /= Verts.Num();
FVisualLogShapeElement AreaMarkElem(EVisualLoggerShapeElement::Segment);
AreaMarkElem.SetColor(FColorList::Orange);
AreaMarkElem.Category = LogNavigation.GetCategoryName();
AreaMarkElem.Thicknes = 2;
AreaMarkElem.Description = AreaClass->GetName();
AreaMarkElem.Points.Add(CenterPt + CorridorOffset);
AreaMarkElem.Points.Add(CenterPt + CorridorOffset + FVector(0,0,100.0f + NumAreaMark * 50.0f));
Snapshot->ElementsToDraw.Add(AreaMarkElem);
NumAreaMark = (NumAreaMark + 1) % 5;
}
}
NavMesh->FinishBatchQuery();
//Snapshot->ElementsToDraw.Add(CorridorElem);
#endif
}
void UCrowdManager::DrawDebugPath(const dtCrowdAgent* CrowdAgent) const
{
ARecastNavMesh* NavMesh = Cast<ARecastNavMesh>(MyNavData);
if (NavMesh == NULL)
{
return;
}
NavMesh->BeginBatchQuery();
const dtPolyRef* Path = CrowdAgent->corridor.getPath();
TArray<FVector> Verts;
for (int32 Idx = 0; Idx < CrowdAgent->corridor.getPathCount(); Idx++)
{
Verts.Reset();
NavMesh->GetPolyVerts(Path[Idx], Verts);
uint16 PolyFlags = 0;
uint16 AreaFlags = 0;
NavMesh->GetPolyFlags(Path[Idx], PolyFlags, AreaFlags);
const FColor PolyColor = AreaFlags != 1 ? CrowdDebugDrawing::Path : CrowdDebugDrawing::PathSpecial;
for (int32 VertIdx = 0; VertIdx < Verts.Num(); VertIdx++)
{
const FVector Pt0 = Verts[VertIdx];
const FVector Pt1 = Verts[(VertIdx + 1) % Verts.Num()];
DrawDebugLine(GetWorld(), Pt0 + CrowdDebugDrawing::Offset * 0.5f, Pt1 + CrowdDebugDrawing::Offset * 0.5f, PolyColor, false
, /*LifeTime*/-1.f, /*DepthPriority*/0
, /*Thickness*/CrowdDebugDrawing::LineThickness);
}
}
NavMesh->FinishBatchQuery();
}
void LogPathPartHelper(AActor* LogOwner, FNavMeshPath* NavMeshPath, int32 StartIdx, int32 EndIdx)
{
#if ENABLE_VISUAL_LOG && WITH_RECAST
ARecastNavMesh* NavMesh = Cast<ARecastNavMesh>(NavMeshPath->GetNavigationDataUsed());
if (NavMesh == NULL ||
!NavMeshPath->PathCorridor.IsValidIndex(StartIdx) ||
!NavMeshPath->PathCorridor.IsValidIndex(EndIdx))
{
return;
}
NavMesh->BeginBatchQuery();
FVector SegmentStart = NavMeshPath->GetPathPoints()[0].Location;
FVector SegmentEnd(FVector::ZeroVector);
if (StartIdx > 0)
{
NavMesh->GetPolyCenter(NavMeshPath->PathCorridor[StartIdx], SegmentStart);
}
for (int32 Idx = StartIdx + 1; Idx < EndIdx; Idx++)
{
NavMesh->GetPolyCenter(NavMeshPath->PathCorridor[Idx], SegmentEnd);
UE_VLOG_SEGMENT_THICK(LogOwner, LogCrowdFollowing, Log, SegmentStart, SegmentEnd, FColor::Yellow, 2, TEXT_EMPTY);
SegmentStart = SegmentEnd;
}
if (EndIdx == (NavMeshPath->PathCorridor.Num() - 1))
{
SegmentEnd = NavMeshPath->GetPathPoints()[1].Location;
}
else
{
NavMesh->GetPolyCenter(NavMeshPath->PathCorridor[EndIdx], SegmentEnd);
}
UE_VLOG_SEGMENT_THICK(LogOwner, LogCrowdFollowing, Log, SegmentStart, SegmentEnd, FColor::Yellow, 2, TEXT_EMPTY);
NavMesh->FinishBatchQuery();
#endif // ENABLE_VISUAL_LOG && WITH_RECAST
}
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 UEnvQueryGenerator_PathingGrid::ProjectAndFilterNavPoints(TArray<FNavLocation>& Points, FEnvQueryInstance& QueryInstance) const
{
Super::ProjectAndFilterNavPoints(Points, QueryInstance);
#if WITH_RECAST
UObject* DataOwner = QueryInstance.Owner.Get();
PathToItem.BindData(DataOwner, QueryInstance.QueryID);
ScanRangeMultiplier.BindData(DataOwner, QueryInstance.QueryID);
bool bPathToItem = PathToItem.GetValue();
float RangeMultiplierValue = ScanRangeMultiplier.GetValue();
ARecastNavMesh* NavMeshData = const_cast<ARecastNavMesh*>(static_cast<const ARecastNavMesh*>(FEQSHelpers::FindNavigationDataForQuery(QueryInstance)));
if (NavMeshData == nullptr || DataOwner == nullptr)
{
return;
}
TArray<FVector> ContextLocations;
QueryInstance.PrepareContext(GenerateAround, ContextLocations);
FSharedNavQueryFilter NavigationFilterCopy = NavigationFilter ?
UNavigationQueryFilter::GetQueryFilter(*NavMeshData, DataOwner, NavigationFilter)->GetCopy() :
NavMeshData->GetDefaultQueryFilter()->GetCopy();
NavigationFilterCopy->SetBacktrackingEnabled(!bPathToItem);
{
TArray<NavNodeRef> Polys;
TArray<FNavLocation> HitLocations;
const FVector ProjectionExtent(ProjectionData.ExtentX, ProjectionData.ExtentX, (ProjectionData.ProjectDown + ProjectionData.ProjectUp) / 2);
for (int32 ContextIdx = 0; ContextIdx < ContextLocations.Num() && Points.Num(); ContextIdx++)
{
float CollectDistanceSq = 0.0f;
for (int32 Idx = 0; Idx < Points.Num(); Idx++)
{
const float TestDistanceSq = FVector::DistSquared(Points[Idx].Location, ContextLocations[ContextIdx]);
CollectDistanceSq = FMath::Max(CollectDistanceSq, TestDistanceSq);
}
const float MaxPathDistance = FMath::Sqrt(CollectDistanceSq) * RangeMultiplierValue;
Polys.Reset();
FRecastDebugPathfindingData NodePoolData;
NodePoolData.Flags = ERecastDebugPathfindingFlags::Basic;
NavMeshData->GetPolysWithinPathingDistance(ContextLocations[ContextIdx], MaxPathDistance, Polys, NavigationFilterCopy, nullptr, &NodePoolData);
for (int32 Idx = Points.Num() - 1; Idx >= 0; Idx--)
{
bool bHasPath = PathGridHelpers::HasPath(NodePoolData, Points[Idx].NodeRef);
if (!bHasPath && Points[Idx].NodeRef != INVALID_NAVNODEREF)
{
// try projecting it again, maybe it will match valid poly on different height
HitLocations.Reset();
FVector TestPt(Points[Idx].Location.X, Points[Idx].Location.Y, ContextLocations[ContextIdx].Z);
NavMeshData->ProjectPointMulti(TestPt, HitLocations, ProjectionExtent, TestPt.Z - ProjectionData.ProjectDown, TestPt.Z + ProjectionData.ProjectUp, NavigationFilterCopy, nullptr);
for (int32 HitIdx = 0; HitIdx < HitLocations.Num(); HitIdx++)
{
const bool bHasPathTest = PathGridHelpers::HasPath(NodePoolData, HitLocations[HitIdx].NodeRef);
if (bHasPathTest)
{
Points[Idx] = HitLocations[HitIdx];
Points[Idx].Location.Z += ProjectionData.PostProjectionVerticalOffset;
bHasPath = true;
break;
}
}
}
if (!bHasPath)
{
Points.RemoveAt(Idx);
}
}
}
}
#endif // WITH_RECAST
}
void UEnvQueryTest_PathfindingBatch::RunTest(FEnvQueryInstance& QueryInstance) const
{
UObject* QueryOwner = QueryInstance.Owner.Get();
BoolValue.BindData(QueryOwner, QueryInstance.QueryID);
PathFromContext.BindData(QueryOwner, QueryInstance.QueryID);
SkipUnreachable.BindData(QueryOwner, QueryInstance.QueryID);
FloatValueMin.BindData(QueryOwner, QueryInstance.QueryID);
FloatValueMax.BindData(QueryOwner, QueryInstance.QueryID);
ScanRangeMultiplier.BindData(QueryOwner, QueryInstance.QueryID);
bool bWantsPath = BoolValue.GetValue();
bool bPathToItem = PathFromContext.GetValue();
bool bDiscardFailed = SkipUnreachable.GetValue();
float MinThresholdValue = FloatValueMin.GetValue();
float MaxThresholdValue = FloatValueMax.GetValue();
float RangeMultiplierValue = ScanRangeMultiplier.GetValue();
UNavigationSystem* NavSys = QueryInstance.World->GetNavigationSystem();
if (NavSys == nullptr)
{
return;
}
ANavigationData* NavData = FindNavigationData(*NavSys, QueryOwner);
ARecastNavMesh* NavMeshData = Cast<ARecastNavMesh>(NavData);
if (NavMeshData == nullptr)
{
return;
}
TArray<FVector> ContextLocations;
if (!QueryInstance.PrepareContext(Context, ContextLocations))
{
return;
}
TArray<FNavigationProjectionWork> TestPoints;
TArray<float> CollectDistanceSq;
CollectDistanceSq.Init(0.0f, ContextLocations.Num());
FSharedNavQueryFilter NavigationFilter = FilterClass != nullptr
? UNavigationQueryFilter::GetQueryFilter(*NavMeshData, FilterClass)->GetCopy()
: NavMeshData->GetDefaultQueryFilter()->GetCopy();
NavigationFilter->SetBacktrackingEnabled(!bPathToItem);
const dtQueryFilter* NavQueryFilter = ((const FRecastQueryFilter*)NavigationFilter->GetImplementation())->GetAsDetourQueryFilter();
{
// scope for perf timers
// can't use FEnvQueryInstance::ItemIterator yet, since it has built in scoring functionality
for (int32 ItemIdx = 0; ItemIdx < QueryInstance.Items.Num(); ItemIdx++)
{
if (QueryInstance.Items[ItemIdx].IsValid())
{
const FVector ItemLocation = GetItemLocation(QueryInstance, ItemIdx);
TestPoints.Add(FNavigationProjectionWork(ItemLocation));
for (int32 ContextIdx = 0; ContextIdx < ContextLocations.Num(); ContextIdx++)
{
const float TestDistanceSq = FVector::DistSquared(ItemLocation, ContextLocations[ContextIdx]);
CollectDistanceSq[ContextIdx] = FMath::Max(CollectDistanceSq[ContextIdx], TestDistanceSq);
}
}
}
NavMeshData->BatchProjectPoints(TestPoints, NavMeshData->GetDefaultQueryExtent(), NavigationFilter);
}
TArray<FRecastDebugPathfindingData> NodePoolData;
NodePoolData.SetNum(ContextLocations.Num());
{
// scope for perf timer
TArray<NavNodeRef> Polys;
for (int32 ContextIdx = 0; ContextIdx < ContextLocations.Num(); ContextIdx++)
{
const float MaxPathDistance = FMath::Sqrt(CollectDistanceSq[ContextIdx]) * RangeMultiplierValue;
Polys.Reset();
NodePoolData[ContextIdx].Flags = ERecastDebugPathfindingFlags::PathLength;
NavMeshData->GetPolysWithinPathingDistance(ContextLocations[ContextIdx], MaxPathDistance, Polys, NavigationFilter, nullptr, &NodePoolData[ContextIdx]);
}
}
int32 ProjectedItemIdx = 0;
if (GetWorkOnFloatValues())
{
NodePoolHelpers::PathParamFunc Func[] = { nullptr, NodePoolHelpers::GetPathCost, NodePoolHelpers::GetPathLength };
FEnvQueryInstance::ItemIterator It(this, QueryInstance);
for (It.IgnoreTimeLimit(); It; ++It, ProjectedItemIdx++)
{
for (int32 ContextIndex = 0; ContextIndex < ContextLocations.Num(); ContextIndex++)
{
const float PathValue = Func[TestMode](NodePoolData[ContextIndex], TestPoints[ProjectedItemIdx], NavQueryFilter);
It.SetScore(TestPurpose, FilterType, PathValue, MinThresholdValue, MaxThresholdValue);
if (bDiscardFailed && PathValue >= BIG_NUMBER)
{
It.ForceItemState(EEnvItemStatus::Failed);
}
}
}
}
else
{
FEnvQueryInstance::ItemIterator It(this, QueryInstance);
for (It.IgnoreTimeLimit(); It; ++It, ProjectedItemIdx++)
{
for (int32 ContextIndex = 0; ContextIndex < ContextLocations.Num(); ContextIndex++)
{
const bool bFoundPath = NodePoolHelpers::HasPath(NodePoolData[ContextIndex], TestPoints[ProjectedItemIdx]);
It.SetScore(TestPurpose, FilterType, bFoundPath, bWantsPath);
}
}
}
}
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
}
void UGameplayDebuggingComponent::ServerCollectNavmeshData_Implementation(FVector_NetQuantize10 TargetLocation)
{
#if WITH_RECAST
UNavigationSystem* NavSys = UNavigationSystem::GetCurrent(GetWorld());
ARecastNavMesh* NavData = GetNavData();
if (NavData == NULL)
{
NavmeshRepData.Empty();
return;
}
const double Timer1 = FPlatformTime::Seconds();
TArray<int32> Indices;
int32 TileX = 0;
int32 TileY = 0;
const int32 DeltaX[] = { 0, 1, 1, 0, -1, -1, -1, 0, 1 };
const int32 DeltaY[] = { 0, 0, 1, 1, 1, 0, -1, -1, -1 };
NavData->BeginBatchQuery();
// add 3x3 neighborhood of target
int32 TargetTileX = 0;
int32 TargetTileY = 0;
NavData->GetNavMeshTileXY(TargetLocation, TargetTileX, TargetTileY);
for (int32 i = 0; i < ARRAY_COUNT(DeltaX); i++)
{
const int32 NeiX = TargetTileX + DeltaX[i];
const int32 NeiY = TargetTileY + DeltaY[i];
if (FMath::Abs(NeiX - TileX) > 1 || FMath::Abs(NeiY - TileY) > 1)
{
NavData->GetNavMeshTilesAt(NeiX, NeiY, Indices);
}
}
const FNavDataConfig& NavConfig = NavData->GetConfig();
FColor NavMeshColors[RECAST_MAX_AREAS];
NavMeshColors[RECAST_DEFAULT_AREA] = NavConfig.Color.DWColor() > 0 ? NavConfig.Color : NavMeshRenderColor_RecastMesh;
for (uint8 i = 0; i < RECAST_DEFAULT_AREA; ++i)
{
NavMeshColors[i] = NavData->GetAreaIDColor(i);
}
TArray<uint8> UncompressedBuffer;
FMemoryWriter ArWriter(UncompressedBuffer);
int32 NumIndices = Indices.Num();
ArWriter << NumIndices;
for (int32 i = 0; i < NumIndices; i++)
{
FRecastDebugGeometry NavMeshGeometry;
NavMeshGeometry.bGatherPolyEdges = false;
NavMeshGeometry.bGatherNavMeshEdges = false;
NavData->GetDebugGeometry(NavMeshGeometry, Indices[i]);
NavMeshDebug::FTileData TileData;
const FBox TileBoundingBox = NavData->GetNavMeshTileBounds(Indices[i]);
TileData.Location = TileBoundingBox.GetCenter();
for (int32 VertIndex = 0; VertIndex < NavMeshGeometry.MeshVerts.Num(); ++VertIndex)
{
const NavMeshDebug::FShortVector SV = NavMeshGeometry.MeshVerts[VertIndex] - TileData.Location;
TileData.Verts.Add(SV);
}
for (int32 iArea = 0; iArea < RECAST_MAX_AREAS; iArea++)
{
const int32 NumTris = NavMeshGeometry.AreaIndices[iArea].Num();
if (NumTris)
{
NavMeshDebug::FAreaPolys AreaPolys;
for (int32 AreaIndicesIndex = 0; AreaIndicesIndex < NavMeshGeometry.AreaIndices[iArea].Num(); ++AreaIndicesIndex)
{
AreaPolys.Indices.Add(NavMeshGeometry.AreaIndices[iArea][AreaIndicesIndex]);
}
AreaPolys.Color = NavMeshColors[iArea];
TileData.Areas.Add(AreaPolys);
}
}
TileData.Links.Reserve(NavMeshGeometry.OffMeshLinks.Num());
for (int32 iLink = 0; iLink < NavMeshGeometry.OffMeshLinks.Num(); iLink++)
{
const FRecastDebugGeometry::FOffMeshLink& SrcLink = NavMeshGeometry.OffMeshLinks[iLink];
NavMeshDebug::FOffMeshLink Link;
Link.Left = SrcLink.Left - TileData.Location;
Link.Right = SrcLink.Right - TileData.Location;
Link.Color = ((SrcLink.Direction && SrcLink.ValidEnds) || (SrcLink.ValidEnds & FRecastDebugGeometry::OMLE_Left)) ? DarkenColor(NavMeshColors[SrcLink.AreaID]) : NavMeshRenderColor_OffMeshConnectionInvalid;
Link.PackedFlags.Radius = (int8)SrcLink.Radius;
Link.PackedFlags.Direction = SrcLink.Direction;
Link.PackedFlags.ValidEnds = SrcLink.ValidEnds;
TileData.Links.Add(Link);
}
ArWriter << TileData;
}
NavData->FinishBatchQuery();
const double Timer2 = FPlatformTime::Seconds();
const int32 HeaderSize = sizeof(int32);
NavmeshRepData.Init(0, HeaderSize + FMath::TruncToInt(1.1f * UncompressedBuffer.Num()));
const int32 UncompressedSize = UncompressedBuffer.Num();
int32 CompressedSize = NavmeshRepData.Num() - HeaderSize;
uint8* DestBuffer = NavmeshRepData.GetData();
FMemory::Memcpy(DestBuffer, &UncompressedSize, HeaderSize);
DestBuffer += HeaderSize;
FCompression::CompressMemory((ECompressionFlags)(COMPRESS_ZLIB | COMPRESS_BiasMemory),
(void*)DestBuffer, CompressedSize, (void*)UncompressedBuffer.GetData(), UncompressedSize);
NavmeshRepData.SetNum(CompressedSize + HeaderSize, false);
const double Timer3 = FPlatformTime::Seconds();
UE_LOG(LogGDT, Log, TEXT("Preparing navmesh data: %.1fkB took %.3fms (collect: %.3fms + compress %d%%: %.3fms)"),
NavmeshRepData.Num() / 1024.0f, 1000.0f * (Timer3 - Timer1),
1000.0f * (Timer2 - Timer1),
FMath::TruncToInt(100.0f * NavmeshRepData.Num() / UncompressedBuffer.Num()), 1000.0f * (Timer3 - Timer2));
#endif
if (World && World->GetNetMode() != NM_DedicatedServer)
{
OnRep_UpdateNavmesh();
}
}
