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);
}
}
}
}