void UNavigationComponent::OnSmartLinkBroadcast(class USmartNavLinkComponent* NearbyLink)
{
// update only when agent is actually moving
if (NearbyLink == NULL || !Path.IsValid() ||
PathFollowComp == NULL || PathFollowComp->GetStatus() == EPathFollowingStatus::Idle)
{
return;
}
UNavigationSystem* NavSys = UNavigationSystem::GetCurrent(GetWorld());
if (NavSys == NULL || MyNavAgent == NULL || GetNavData() == NULL)
{
return;
}
const bool bHasLink = Path->ContainsSmartLink(NearbyLink);
const bool bIsEnabled = NearbyLink->IsEnabled();
if (bIsEnabled == bHasLink)
{
// enabled link, path already use it - or - disabled link and path it's not using it
return;
}
const FVector GoalLocation = Path->PathPoints.Last().Location;
FPathFindingResult Result = NavSys->FindPathSync(*MyNavAgent->GetNavAgentProperties(),
FPathFindingQuery(GetNavData(), MyNavAgent->GetNavAgentLocation(), GoalLocation, StoredQueryFilter),
bDoHierarchicalPathfinding ? EPathFindingMode::Hierarchical : EPathFindingMode::Regular);
if (Result.IsSuccessful() || Result.IsPartial())
{
const bool bNewHasLink = Result.Path->ContainsSmartLink(NearbyLink);
if ((bIsEnabled && !bHasLink && bNewHasLink) ||
(!bIsEnabled && bHasLink && !bNewHasLink))
{
SetPath(Result.Path);
NotifyPathUpdate();
}
}
}
FNavLocation UNavigationComponent::ProjectPointToNavigation(const FVector& Location) const
{
FNavLocation OutLocation(Location);
if (GetNavData() != NULL)
{
if (!MyNavData->ProjectPoint(Location, OutLocation, MyNavData->GetDefaultQueryExtent()))
{
OutLocation = FNavLocation();
}
}
return OutLocation;
}
bool UNavigationComponent::GeneratePath(const FVector& FromLocation, const FVector& GoalLocation, TSharedPtr<const FNavigationQueryFilter> QueryFilter)
{
if (GetWorld() == NULL || GetWorld()->GetNavigationSystem() == NULL || GetOuter() == NULL || MyNavAgent == NULL || GetNavData() == NULL)
{
return false;
}
const FNavLocation NavMeshGoalLocation = ProjectPointToNavigation(GoalLocation);
const EPathFindingMode::Type Mode = bDoHierarchicalPathfinding ? EPathFindingMode::Hierarchical : EPathFindingMode::Regular;
FPathFindingResult Result = GetWorld()->GetNavigationSystem()->FindPathSync(*MyNavAgent->GetNavAgentProperties()
, FPathFindingQuery(GetNavData(), FromLocation, NavMeshGoalLocation.Location, QueryFilter)
, Mode);
if (Result.IsSuccessful() || Result.IsPartial())
{
CurrentGoal = NavMeshGoalLocation.Location;
SetPath(Result.Path);
return true;
}
return false;
}
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;
}
bool UNavigationComponent::AsyncGeneratePath(const FVector& FromLocation, const FVector& GoalLocation, TSharedPtr<const FNavigationQueryFilter> QueryFilter)
{
if (GetWorld()->GetNavigationSystem() == NULL || GetOuter() == NULL || MyNavAgent == NULL || GetNavData() == 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.
const FNavAgentProperties* NavAgentProps = MyNavAgent->GetNavAgentProperties();
if (NavAgentProps != NULL)
{
FVector NavMeshGoalLocation;
if (ProjectPointToNavigation(GoalLocation, NavMeshGoalLocation) == QuerySuccess)
{
const EPathFindingMode::Type Mode = bDoHierarchicalPathfinding ? EPathFindingMode::Hierarchical : EPathFindingMode::Regular;
AsynPathQueryID = GetWorld()->GetNavigationSystem()->FindPathAsync(*MyNavAgent->GetNavAgentProperties()
, FPathFindingQuery(MyNavData, FromLocation, NavMeshGoalLocation, QueryFilter)
, FNavPathQueryDelegate::CreateUObject(this, &UNavigationComponent::AsyncGeneratePath_OnCompleteCallback)
, Mode);
}
else
{
// projecting destination point failed
AsynPathQueryID = INVALID_NAVQUERYID;
UE_VLOG(GetOwner(), LogNavigation, Warning, TEXT("AI trying to generate path to point off of navmesh"));
UE_VLOG_LOCATION(GetOwner(), GoalLocation, 30, FColor::Red, TEXT("Invalid pathing GoalLocation"));
if (MyNavData != NULL)
{
UE_VLOG_BOX(GetOwner(), FBox(GoalLocation-MyNavData->GetDefaultQueryExtent(),GoalLocation+MyNavData->GetDefaultQueryExtent()), FColor::Red, TEXT_EMPTY);
}
}
}
else
{
AsynPathQueryID = INVALID_NAVQUERYID;
}
return AsynPathQueryID != INVALID_NAVQUERYID;
}
bool UNavigationComponent::ProjectPointToNavigation(const FVector& Location, FNavLocation& ProjectedLocation) const
{
return GetNavData() != NULL && MyNavData->ProjectPoint(Location, ProjectedLocation, MyNavData->GetDefaultQueryExtent());
}
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();
}
}
