PavilionDV7の雑多なやつ

Qiitaから移行しました。UE4に関する記事から興味のあることまで色々書きます。

【UE4】経路探索のコスト計算をカスタマイズする

UE4

はじめに

この記事ではC++を使ってRecast NavMeshをカスタマイズしたコスト計算を使うように拡張して経路探索をコントロールする方法を紹介します。

C++での拡張以外にもNavModifierVolumeと動的なナビメッシュの生成を組み合わせて経路探索をコントロールすることが出来ます。自分が過去に書いた記事（【UE4】EQSを使ってInfluence Mapを作った - Qiita）では影響マップの値を基にNavModiferVolumeを動的に生成し、迂回する経路を生成させる手順を解説しています。

プロジェクトはこちら

  • プロジェクトURLを貼る

バージョン：UE4.25.3

https://1drv.ms/u/s!Au-8FqgREBKZjEYMWUcjGPnO5Kex?e=bEIgS2

コスト計算のカスタマイズと、それを用いて経路探索するまでの手順

  1. dtQueryFilterを継承して自作QueryFilterクラスを作る
  2. 自作QueryFilterクラスでgetVirtualCost関数をオーバーライドしデバッグ表示と適当なテキストを表示させる
  3. INavigationQueryFilterInterfaceと1番で作成した自作QueryFilterを継承し自作RecastQueryFilterを作成
  4. 自作RecastQueryFilterクラスで自作QueryFilterを経路探索時に使用するように指定する
  5. ARecastNavMeshを継承して自作RecastNavMeshクラスを作る
  6. 自作RecastNavMeshで経路探索時に自作RecastQueryFilterクラスを使うように指定する

複雑に見えますが重要なことは2番のみで、それ以外はコピペするだけでOKです。

コスト計算をカスタマイズする

1. dtQueryFilterクラスを継承して自作QueryFilterクラスを作る

dtQuryFilterクラスには経路探索時のナビメッシュポリゴンのフィルタリングや移動コストの計算が実装されているため、まずはdtQueryFilterクラスを継承してカスタマイズしたコスト計算を実行させる足がかりにします。

class プロジェクト名_API MyDetourQueryFilter : public dtQueryFilter
{
private:
    // デバッグ時に必要
    class UWorld* World;

public:
    MyDetourQueryFilter(bool inIsVirtual = true);
    virtual ~MyDetourQueryFilter() {}

    void SetWorld(UWorld* NewWorld)
    {
        World = NewWorld;
    }

protected:
    /// virtual scoring function implementation (defaults to getInlineCost). @see getCost for parameter description
    virtual float getVirtualCost(const float* pa, const float* 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;


};

2. 自作QueryFilterクラスでgetVirtualCost関数をオーバーライドしデバッグ表示と適当なテキストを表示させる

dtQueryFilterクラスが実行する移動コストの計算においてはデフォルト実装のgetInlineCost関数とオーバーライド可能（カスタム可能）なgetVirtualCost関数の２つがあります。目的はコスト計算のカスタマイズなのでgetVirtualCost関数をオーバーライドします。

ここではきちんとカスタマイズしたコスト計算処理が呼び出されるか確認するため簡単なデバッグ表示をさせるまでに留めておきます。

// ~~~~~~~~~~ BEGIN MyDetourQueryFilter ~~~~~~~~~~

MyDetourQueryFilter::MyDetourQueryFilter(bool inIsVirtual/* = true*/) 
    : dtQueryFilter(inIsVirtual)
{}


// あるポリゴン内に含まれる線分の始点から終点までの移動コストを求める
// isVirtualCostフラグがTRUEの場合に呼ばれる。これは移動コストのカスタムを目的としたフラグである。
// 
// @param[in]      pa          前のポリゴンと現在のポリゴンのエッジの開始位置. [(x、y、z)]
// @param[in]      pb          現在および次のポリゴンのエッジの終了位置。 [(x、y、z)]
// @param[in]      prevRef     前のポリゴンの参照ID。 [オプション]
// @param[in]      prevTile    前のポリゴンを含むタイル。 [オプション]
// @param[in]      prevPoly    前のポリゴン。 [オプション]
// @param[in]      curRef      現在のポリゴンの参照ID。
// @param[in]      curTile     現在のポリゴンを含むタイル。
// @param[in]      curPoly     現在のポリゴン。
// @param[in]      nextRef     次のポリゴンの参照ID。 [オプション]
// @param[in]      nextTile    次のポリゴンを含むタイル。 [オプション]
// @param[in]      nextPoly    次のポリゴン。 [オプション]
float MyDetourQueryFilter::getVirtualCost(const float* pa, const float* 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
{
    // 経路探索時のコスト計算をカスタマイズしたい場合はこちらの評価式を変更する.


#if WITH_FIXED_AREA_ENTERING_COST

    FVector Begin = Recast2UnrealPoint(pa);
    FVector End = Recast2UnrealPoint(pb);
    DrawDebugSphere(World, Begin, 15.f, 8, FColor::Magenta, true);
    DrawDebugSphere(World, End, 15.f, 8, FColor::Magenta, true);
    DrawDebugDirectionalArrow(World, Begin, End, 100.f, FColor::Cyan, true, -1.f, 0, 4.f);


    return getInlineCost(pa, pb,
                                     prevRef, prevTile, prevPoly,
                                     curRef, curTile, curPoly,
                                     nextRef, nextTile, nextPoly);
#else
    return dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()];
#endif // #if WITH_FIXED_AREA_ENTERING_COST

}
// ~~~~~~~~~~ END MyDetourQueryFilter ~~~~~~~~~~

カスタマイズしたコスト計算処理を呼び出すようにする

3. INavigationQueryFilterInterfaceと1番で作成した自作QueryFilterを継承し自作RecastQueryFilterを作成

実装すべき関数が多いが99%は「FRecastQueryFilter」からコピペで完了します。

// ~~~~~~~~~~ BEGIN FMyRecastQueryFilter ~~~~~~~~~~
class プロジェクト名_API FMyRecastQueryFilter : public INavigationQueryFilterInterface, public MyDetourQueryFilter
{
public:
    FMyRecastQueryFilter(bool bIsVirtual = true);
    virtual ~FMyRecastQueryFilter() {}

    virtual void Reset() override;

    virtual void SetAreaCost(uint8 AreaType, float Cost) override;
    virtual void SetFixedAreaEnteringCost(uint8 AreaType, float Cost) override;
    virtual void SetExcludedArea(uint8 AreaType) override;
    virtual void SetAllAreaCosts(const float* CostArray, const int32 Count) override;
    virtual void GetAllAreaCosts(float* CostArray, float* FixedCostArray, const int32 Count) const override;
    virtual void SetBacktrackingEnabled(const bool bBacktracking) override;
    virtual bool IsBacktrackingEnabled() const override;
    virtual float GetHeuristicScale() const override;
    virtual bool IsEqual(const INavigationQueryFilterInterface* Other) const override;
    virtual void SetIncludeFlags(uint16 Flags) override;
    virtual uint16 GetIncludeFlags() const override;
    virtual void SetExcludeFlags(uint16 Flags) override;
    virtual uint16 GetExcludeFlags() const override;
    virtual FVector GetAdjustedEndLocation(const FVector& EndLocation) const override { return EndLocation; }
    virtual INavigationQueryFilterInterface* CreateCopy() const override;

    const MyDetourQueryFilter* GetAsDetourQueryFilter() const { return this; }

    /** note that it results in losing all area cost setup. Call it before setting anything else */
    void SetIsVirtual(bool bIsVirtual = true, UWorld* NewWorld = nullptr);

};
// ~~~~~~~~~~ END FMyRecastQueryFilter ~~~~~~~~~~

4. 自作RecastQueryFilterクラスで自作QueryFilterを経路探索時に使用するように指定する

// ~~~~~~~~~~ BEGIN FMyRecastQueryFilter ~~~~~~~~~~
FMyRecastQueryFilter::FMyRecastQueryFilter(bool bIsVirtual)
    : MyDetourQueryFilter(bIsVirtual)
{
    SetExcludedArea(RECAST_NULL_AREA);
}

void FMyRecastQueryFilter::Reset()
{
    // 自作QueryFilterクラスを指定する
    MyDetourQueryFilter* Filter = static_cast<MyDetourQueryFilter*>(this);
    Filter = new(Filter) MyDetourQueryFilter(isVirtual);
    SetExcludedArea(RECAST_NULL_AREA);
}
void FMyRecastQueryFilter::SetAreaCost(uint8 AreaType, float Cost)
{
    setAreaCost(AreaType, Cost);
}
void FMyRecastQueryFilter::SetFixedAreaEnteringCost(uint8 AreaType, float Cost)
{
#if WITH_FIXED_AREA_ENTERING_COST
    setAreaFixedCost(AreaType, Cost);
#endif // WITH_FIXED_AREA_ENTERING_COST
}
void FMyRecastQueryFilter::SetExcludedArea(uint8 AreaType)
{
    setAreaCost(AreaType, DT_UNWALKABLE_POLY_COST);
}
void FMyRecastQueryFilter::SetAllAreaCosts(const float* CostArray, const int32 Count)
{
    // @todo could get away with memcopying to m_areaCost, but it's private and would require a little hack
    // need to consider if it's wort a try (not sure there'll be any perf gain)
    if (Count > RECAST_MAX_AREAS)
    {
        UE_LOG(LogNavigation, Warning, TEXT("FRecastQueryFilter: Trying to set cost to more areas than allowed! Discarding redundant values."));
    }

    const int32 ElementsCount = FPlatformMath::Min(Count, RECAST_MAX_AREAS);
    for (int32 i = 0; i < ElementsCount; ++i)
    {
        setAreaCost(i, CostArray[i]);
    }
}
void FMyRecastQueryFilter::GetAllAreaCosts(float* CostArray, float* FixedCostArray, const int32 Count) const
{
    const float* DetourCosts = getAllAreaCosts();
    const float* DetourFixedCosts = getAllFixedAreaCosts();

    FMemory::Memcpy(CostArray, DetourCosts, sizeof(float) * FMath::Min(Count, RECAST_MAX_AREAS));
    FMemory::Memcpy(FixedCostArray, DetourFixedCosts, sizeof(float) * FMath::Min(Count, RECAST_MAX_AREAS));
}
void FMyRecastQueryFilter::SetBacktrackingEnabled(const bool bBacktracking)
{
    setIsBacktracking(bBacktracking);
}
bool FMyRecastQueryFilter::IsBacktrackingEnabled() const
{
    return getIsBacktracking();
}
float FMyRecastQueryFilter::GetHeuristicScale() const
{
    return getHeuristicScale();
}
bool FMyRecastQueryFilter::IsEqual(const INavigationQueryFilterInterface* Other) const
{
    // @NOTE: not type safe, should be changed when another filter type is introduced
    return FMemory::Memcmp(this, Other, sizeof(FMyRecastQueryFilter)) == 0;
}
void FMyRecastQueryFilter::SetIncludeFlags(uint16 Flags)
{
    setIncludeFlags(Flags);
}
uint16 FMyRecastQueryFilter::GetIncludeFlags() const
{
    return getIncludeFlags();
}
void FMyRecastQueryFilter::SetExcludeFlags(uint16 Flags)
{
    setExcludeFlags(Flags);
}
uint16 FMyRecastQueryFilter::GetExcludeFlags() const
{
    return getExcludeFlags();
}
INavigationQueryFilterInterface* FMyRecastQueryFilter::CreateCopy() const
{
    return new FMyRecastQueryFilter(*this);
}
void FMyRecastQueryFilter::SetIsVirtual(bool bIsVirtual, UWorld* NewWorld)
{
    // 自作QueryFilterクラスを指定する
    MyDetourQueryFilter* Filter = static_cast<MyDetourQueryFilter*>(this);
    Filter = new(Filter)MyDetourQueryFilter(bIsVirtual);
    Filter->SetWorld(NewWorld);
    SetExcludedArea(RECAST_NULL_AREA);
}
// ~~~~~~~~~~ END FMyRecastQueryFilter ~~~~~~~~~~

5. ARecastNavMeshを継承して自作RecastNavMeshクラスを作る

UCLASS()
class プロジェクト名_API AMyRecastNavMesh : public ARecastNavMesh
{
    GENERATED_BODY()

public:
    virtual ~AMyRecastNavMesh() {}
    virtual void RecreateDefaultFilter() override;
};

6. 自作RecastNavMeshで経路探索時に自作RecastQueryFilterクラスを使うように指定する

// ~~~~~~~~~~ BEGIN AMyRecastNavMesh ~~~~~~~~~~
void AMyRecastNavMesh::RecreateDefaultFilter()
{
    DefaultQueryFilter->SetFilterType<FMyRecastQueryFilter>();
    DefaultQueryFilter->SetMaxSearchNodes(DefaultMaxSearchNodes);

    // 自作RecastQueryFilterを指定する
    FMyRecastQueryFilter* DetourFilter = static_cast<FMyRecastQueryFilter*>(DefaultQueryFilter->GetImplementation());
    DetourFilter->SetIsVirtual(true, GetWorld());   // 移動コスト計算をカスタムする場合はTRUE
    DetourFilter->setHeuristicScale(HeuristicScale);
    // clearing out the 'navlink flag' from included flags since it would make 
    // dtQueryFilter::passInlineFilter pass navlinks of area classes with
    // AreaFlags == 0 (like NavArea_Null), which should mean 'unwalkable'
    // こちらも忘れずに
    DetourFilter->setIncludeFlags(DetourFilter->getIncludeFlags() & (~AMyRecastNavMesh::GetNavLinkFlag()));

    for (int32 Idx = 0; Idx < SupportedAreas.Num(); Idx++)
    {
        const FSupportedAreaData& AreaData = SupportedAreas[Idx];

        UNavArea* DefArea = nullptr;
        if (AreaData.AreaClass)
        {
            DefArea = ((UClass*)AreaData.AreaClass)->GetDefaultObject<UNavArea>();
        }

        if (DefArea)
        {
            DetourFilter->SetAreaCost(AreaData.AreaID, DefArea->DefaultCost);
            DetourFilter->SetFixedAreaEnteringCost(AreaData.AreaID, DefArea->GetFixedAreaEnteringCost());
        }
    }
}
// ~~~~~~~~~~ END AMyRecastNavMesh ~~~~~~~~~~

実際に使ってみる

実際にカスタマイズしたコスト計算処理を呼び出しますが、その前にエディタ側で自作したRecastNavMeshクラスを使うように設定する必要があります。

  • 手順
  • Project Settingsを開き上部にある検索窓にsupported agentsと打ち込む
  • 表示された項目にある「Supported Agents」の＋ボタンを押す
  • Nav Data Classを自作RecastNavMeshクラスに変更する

f:id:PavilionDV7:20200926144736p:plain

  1. いつもどおりNav Mesh Bounds Volumeを配置してナビメッシュを生成する
  2. 適当なキャラクターを用意しAI Move To等で移動させるとgetVirtualCost関数に実装したデバッグ表示が表示される

f:id:PavilionDV7:20200926144746p:plain

経路探索をコントロールしてみる

コスト計算処理をオーバーライド出来たので次はNav Mesh Modifier Volumeのようにコストに影響を与えるアクター（NavCostModiferアクター）をレベルに配置して経路探索をコントロールさせます。

下準備

コリジョンプロファイルの作成

これからLine Traceを使った処理のためにコリジョンプロファイルを１つ新たに作成します。

f:id:PavilionDV7:20200926144815p:plain

コストに影響を与えるアクターを作る

Actorクラスを継承したNavCostModiferクラスを作成します

UCLASS()
class NAVCOSTMODIFICATION_API ANavCostModifier : public AActor
{
    GENERATED_BODY()
public:
    UPROPERTY(EditInstanceOnly, Category = "Component")
        class UBoxComponent* BoxCollision;

    // 経路探索時のコストに影響を与える何らかの値.
    UPROPERTY(EditInstanceOnly, Category = "Influence")
        float SomeValue;

public:
    // Sets default values for this actor's properties
    ANavCostModifier();
};

// Sets default values
ANavCostModifier::ANavCostModifier()
{
    // Set this actor to call Tick() every frame.  You can turn this off to improve performance if you don't need it.
    PrimaryActorTick.bCanEverTick = false;

    BoxCollision = CreateDefaultSubobject<UBoxComponent>(TEXT("Box"));
    BoxCollision->InitBoxExtent(FVector(100.f));
    BoxCollision->SetGenerateOverlapEvents(false);
    // NavCostModiferにしか反応しないコリジョンプロファイルを設定.（「プロジェクト設定->コリジョン」にて作成する必要がある）
    BoxCollision->SetCollisionProfileName(FName("NavCostModifier"));

    RootComponent = BoxCollision;

    SomeValue = 0.f;
}

続いて作成したNavCostModiferアクターをレベルに配置します。

配置する場所は次の2箇所です

f:id:PavilionDV7:20200926144900p:plain

下準備はこれで終了です。

MyDetourQueryFilterのgetVirtualCost関数を修正する

先程レベルに配置したANavCostModifierアクターを検知してSomeValueをコストに加算するようにしてみます。

float MyDetourQueryFilter::getVirtualCost(const float* pa, const float* 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
{
    // 経路探索時のコスト計算をカスタマイズしたい場合はこちらの評価式を変更する.


#if WITH_FIXED_AREA_ENTERING_COST

    FVector Begin = Recast2UnrealPoint(pa);
    FVector End = Recast2UnrealPoint(pb);
    DrawDebugSphere(World, Begin, 15.f, 8, FColor::Magenta, true);
    DrawDebugSphere(World, End, 15.f, 8, FColor::Magenta, true);
    DrawDebugDirectionalArrow(World, Begin, End, 100.f, FColor::Cyan, true, -1.f, 0, 4.f);

    float TotalSomeValue = 0;
    FCollisionQueryParams Query(FName("InfluenceObjTrace"));
    TArray<FHitResult> HitResult;
    if (World->LineTraceMultiByChannel(HitResult, Begin, End, ECollisionChannel::ECC_GameTraceChannel1, Query))
    {
        for (const FHitResult& Hit : HitResult)
        {
            TotalSomeValue += Cast<ANavCostModifier>(Hit.GetActor())->SomeValue;
        }
    }

    // ポリゴンを跨ぐ際に追加される固定のコスト（Fixed Area Entering Costのこと）
    const float areaChangeCost = nextPoly != 0 && nextPoly->getArea() != curPoly->getArea() ? data.m_areaFixedCost[nextPoly->getArea()] : 0.f;

    // カスタマイズした移動コストの計算結果
    float TraversalCost = dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()] + areaChangeCost + TotalSomeValue;


    // デフォルトで実装されている移動コストの計算結果
    float ActualCost = getInlineCost(pa, pb,
                                     prevRef, prevTile, prevPoly,
                                     curRef, curTile, curPoly,
                                     nextRef, nextTile, nextPoly);

    // デバッグ表示
    FVector TextLocation = (Begin + End) * 0.5f;
    DrawDebugString(World, TextLocation, FString::SanitizeFloat(TraversalCost, 2));
    DrawDebugString(World, TextLocation + FVector(-30.f, 0.f, 0.f), FString::SanitizeFloat(ActualCost, 2));

    return TraversalCost;
#else
    return dtVdist(pa, pb) * data.m_areaCost[curPoly->getArea()];
#endif // #if WITH_FIXED_AREA_ENTERING_COST
}

SomeValueが大きいほどコストに大きく負荷が掛かり、逆にSomeValueがマイナスの場合はコストを軽くすることが出来ます。

ANavCostModifierアクターのSomeValueを弄って動かす

  • デフォルト時
    • 表示されている数値は上段がカスタムしたコスト計算の結果、下段がデフォルト実装のコスト計算の結果を表しています

f:id:PavilionDV7:20201212143746p:plain

  • 右側にあるANavCostModifierアクターのSomeValueを10000にする
    • 右側のルートは掛かるコストが大幅に増加していることがわかります

f:id:PavilionDV7:20201212143807p:plain

  • 右側のSomeValueを0に戻し、左側のSomeValueを-10000にする
    • 結果が負になりコストが掛かっていないに等しい状態になっています

f:id:PavilionDV7:20201212143827p:plain

まとめ

コスト関数のカスタマイズはdtQueryFilterのgetVirtualCost関数をオーバーライドすることによって実現出来ます。

ただし使用する際には処理負荷について注意する必要があります。経路探索は通常、頻繁に実行される処理であるため計算時の処理負荷は常に気にかける必要があります。今回紹介したLine Traceを使った計算は簡単さを重視したものであり、テスト内容自体も非常に短く狭い範囲で経路探索をする内容のため採用することが出来ました。 通常であればLine Traceを使うのは避けたほうが良いでしょう。

もしもレベル上に沼地や水たまりと言った、「通行出来るが可能な限り避けてほしい場所」が固定で存在する場合はいつも通りNav Mesh Modifier Volumeを使って事前計算させるほうが確実に良いです。

用法用量を守って正しくお使いください。

参考資料

Modify pathfinding cost - DetourNavMeshQuery subclass - UE4 AnswerHub

Recast/Detour: Detour