UFUNCTION(BlueprintCallable, meta = (WorldContext = "WorldContextObject"))
static UNavigationPath* FindPathToLocationWithOffset(UObject* WorldContextObject, const FVector& PathStart, const FVector& PathEnd, const float OffsetDistance, AActor* PathfindingContext = nullptr, TSubclassOf<UNavigationQueryFilter> FilterClass = nullptr);

UNavigationPath* URTSTools::FindPathToLocationWithOffset(UObject* WorldContextObject, const FVector& PathStart, const FVector& PathEnd, const float OffsetDistance, AActor* PathfindingContext, TSubclassOf<UNavigationQueryFilter> FilterClass)
{
    UNavigationSystemV1* NavSys = UNavigationSystemV1::GetCurrent(WorldContextObject);

    UNavigationPath* NavPath = NavSys->FindPathToLocationSynchronously(WorldContextObject, PathStart, PathEnd, PathfindingContext, FilterClass);

    // Return invalid path so we can handle invalidity in blueprint
    if (!NavPath->IsValid())
    {
        return NavPath;
    }

    // Offset path points from corners only if distance is valid
    if (OffsetDistance > 0)
    {
        const FNavPathSharedPtr NavPathPtr = NavPath->GetPath();

        NavPathPtr.Get()->CastPath<FNavMeshPath>()->OffsetFromCorners(OffsetDistance);

        UNavigationPath* OffsetPath = NewObject<UNavigationPath>(NavSys);
        OffsetPath->SetPath(NavPathPtr);

        // Filter out duplicate points as OffsetFromCorners tends to create doubles
        TArray<FVector> FilteredPoints;
        for (auto Point : OffsetPath->PathPoints)
        {
            FilteredPoints.AddUnique(Point);
        }

        OffsetPath->PathPoints = FilteredPoints;

        return OffsetPath;
    }
    else
    {
        return NavPath;
    }
}