Modify pathfinding cost - DetourNavMeshQuery subclass

Development World Creation
, , , ,
18

I guess the following could help somebody to save some time. This was my approach to manipulate pathfinding based on data from the game world (seems to work)…

Use case was to block a specific area for doing a single path finding query (Flanking ability).

Snippet from actor component at AIController which is responsible for flanking:

TArray<FVector> UFlankingComponent::FindFlankPathToLocation(FVector PathEndLocation, FVector ActualEnemyLocation)
{
    if (!AIController || !AIController->GetPawn())
    {
        return TArray<FVector>();
    }
    
    FVector PathStartLocation = AIController->GetPawn()->GetActorLocation();

    UNavigationSystemV1* NavSystem = UNavigationSystemV1::GetCurrent(AIController->GetWorld());
    ANavigationData* NavData = NavSystem->GetNavDataForProps(AIController->GetNavAgentPropertiesRef(), AIController->GetNavAgentLocation());

    AGoapShooterCharacter* Char = Cast<AGoapShooterCharacter>(AIController->GetPawn());
    Char->FlankEnemyLocation = ActualEnemyLocation; // temporarily save it there, so it can be accessed by the following QueryFilter
    FSharedConstNavQueryFilter QueryFilter = UNavigationQueryFilter::GetQueryFilter(*NavData, Char,
        UNavQueryFilter_EfficientAvoidFlanking::StaticClass());
    
    FPathFindingQuery Query(Char, *NavData, PathStartLocation,PathEndLocation, QueryFilter);
    Query.SetAllowPartialPaths(false);

    FPathFindingResult PathResult = NavSystem->FindPathSync(Query);

    if (!PathResult.IsSuccessful() || !PathResult.Path.IsValid())
    {
        return TArray<FVector>();
    }
    
    TArray<FVector> FoundPathPoints;
    for (const FNavPathPoint& Point : PathResult.Path->GetPathPoints())
    {
        FoundPathPoints.Add(Point.Location);
    }
    
    return FoundPathPoints;
}

NavQueryFilter_EfficientAvoidFlanking.h

UCLASS()
class GOAPSHOOTER_API UNavQueryFilter_EfficientAvoidFlanking : public UNavigationQueryFilter
{
	GENERATED_BODY()

public:
	UNavQueryFilter_EfficientAvoidFlanking();

	virtual void InitializeFilter(const ANavigationData& NavData, const UObject* Querier, FNavigationQueryFilter& Filter) const override;
};

NavQueryFilter_EfficientAvoidFlanking.cpp

UNavQueryFilter_EfficientAvoidFlanking::UNavQueryFilter_EfficientAvoidFlanking()
{
	bIsMetaFilter = false;
	bInstantiateForQuerier = true;
}

void UNavQueryFilter_EfficientAvoidFlanking::InitializeFilter(const ANavigationData& NavData, const UObject* Querier,
	FNavigationQueryFilter& Filter) const
{
	const AGoapShooterCharacter* Char = Cast<AGoapShooterCharacter>(Querier);
	const FFlankRecastQueryFilter FlankFilterImplementation(Char->GetActorLocation(), Char->FlankEnemyLocation);
	Filter.SetFilterImplementation(&FlankFilterImplementation);
	Super::InitializeFilter(NavData, Querier, Filter);
}

Somewhere accessable:

struct FFlankRecastQueryFilter : public FRecastQueryFilter
{
	// default constructor , dont use
	FFlankRecastQueryFilter() : FFlankRecastQueryFilter(FVector::ZeroVector, FVector::ZeroVector)
	{}
	
	FFlankRecastQueryFilter(const FVector& AILocation, const FVector& EnemyLocation)
		: FRecastQueryFilter(true)
		, AILocation(AILocation)
		, EnemyLocation(EnemyLocation)
	{
	}
	
	FVector AILocation;
	FVector EnemyLocation;
	
	virtual bool passVirtualFilter(const dtPolyRef ref, const dtMeshTile* tile, const dtPoly* poly) const override
	{
		// Standard inline filter (area flags, include/exclude, etc.)
		if (!passInlineFilter(ref, tile, poly)) {
			return false;
		}
		
		// Reject if any vertex of the nav mesh overlaps the avoidance area
		const int VertCount = poly->vertCount;
		for (int i = 0; i < VertCount; ++i)
		{
			// Recast coords -> Unreal world point
			const double* RecastPoint = &tile->verts[ poly->verts[i] * 3 ]; 
			const FVector PointToTest = Recast2UnrealPoint(RecastPoint);

			bool bInside = IsPointInQuad2D(PointToTest, BuildAvoidanceArea(AILocation, EnemyLocation));

			if (bInside)
			{
				return false;
			}
		}

		return true;
	}

	// virtual dtReal getVirtualCost(const dtReal* pa, const dtReal* pb,
	// 	const dtPolyRef prevRef, const dtMeshTile* prevTile, const dtPoly* prevPoly,
	// 	const dtPolyRef curRef, const dtMeshTile* curTile, const dtPoly* curPoly,
	// 	const dtPolyRef nextRef, const dtMeshTile* nextTile, const dtPoly* nextPoly) const override
	// {
	// 	
	// 	dtReal BaseCost = getInlineCost(pa, pb,
	// 		prevRef, prevTile, prevPoly,
	// 		curRef, curTile, curPoly,
	// 		nextRef, nextTile, nextPoly);
	// 	
	// 	// could manipulate costs here instead of blocking the area...
	// 		FVector PointA(pa[0], pa[1], pa[2]);
	// 		FVector PointB(pb[0], pb[1], pb[2]);
	// 		PointA = Recast2UnrealPoint(PointA);
	// 		PointB = Recast2UnrealPoint(PointB);
	//
	// 	return BaseCost;
	// }

	virtual INavigationQueryFilterInterface* CreateCopy() const override
	{
		return new FFlankRecastQueryFilter(AILocation, EnemyLocation);
	};
};
2 Likes