2頂点間の最短経路を探索する (A*アルゴリズム)

投稿者: | 2019/06/03

今回はポリゴン上の2頂点間における最短経路探索について。
標準関数でありそうな機能ですが、意外にも見つからなかったので自前で実装してみました。

ゲームキャラのルーティングや、カーナビのルート探査などでよく使われているらしいA*(Aスター)アルゴリズムを使用しています。

本当は2時間くらいかけて書いた詳細なアルゴリズムの説明があったのですが、Wordpressの設定ミスで消滅してしまったので気力が尽きました。

A*アルゴリズムについて

A*は最短経路問題解決アルゴリズムの一種で、ダイクストラ法の派生アルゴリズムです。
標準的なダイクストラ法では、スタート地点から放射状に周辺ノードを探査していくのに対し、A*法では出来るだけゴールに近いルートを優先的に探査する事によって、処理コストを低減しています。

詳細についてはA*のWikiを参照してください。(丸投げ)

MaxScriptへの最適化

基本的にWikiのアルゴリズムを流用していますが、MaxScript用にいくつか最適化しています。

  • 頂点IDを2つ渡し、それぞれをスタート地点とゴール地点とする。
  • 2頂点間を最小距離で移動するルートを探索する。(ステップ数は考えない)
  • CLOSEリストを使用せず、代わりにノードにisClosedプロパティを持たせる。
  • 作成済みノードを頂点IDで索引する為に、頂点数と同じ大きさのプール配列を使用する。
  • 経路は頂点IDのみを記憶し、エッジIDは保持しない。(理由後述)

コード (Editable_Poly版)

ダウンロードリンク
TRLib_PolyAStar.ms (5.1 KB)

コード(長いので非表示にしています)
struct AStarNode
(
    public id,
    public pos,
    public parent,
    public cost,
    public heuristic,
    public isClosed = false
)

struct PolyAStarOp
(
    private _epoly,
    private _openList,
    private _nodeTable,
    
    -- ▽ OpenList内の最もスコアの小さいノードを検索します。
    --    Return: インデックス。_openListが空なら-1。
    private fn _findMinScoreNodeFromOpenList =
    (
        local minCost = 3.402823e+38  -- とにかく大きな数字
        local minScore = 3.402823e+38
        local index = -1
        local i = 0
        for n in this._openList do
        (
            local cost = n.cost
            local score = n.heuristic + cost
            i += 1
            
            -- スコアが小さいか、スコアが同じかつコストが小さければ更新
            if score < minScore or (score == minScore and cost < minCost) do
            (
                minCost = cost
                minScore = score
                index = i
            )
        )
        return index
    ),
    
    -- ▽ 指定した頂点位置のノードをOpenします。
    private fn _openNode vertId parentNode goalPos =
    (
        -- 頂点座標を取得しコストを計算する
        local pos = polyop.getVert this._epoly vertId
        local cost = (length (pos - parentNode.pos)) + parentNode.cost
        
        -- プールから既存のノードを取得
        local theNode = this._nodeTable[vertId]
        
        if theNode == undefined then
        (
            -- まだノードが作られていなければ新規作成
            theNode = AStarNode vertId pos parentNode cost (length (goalPos - pos))
            
            -- Openリスト及びノードプールに追加
            this._nodeTable[vertId] = theNode
            append this._openList theNode
        )
        else if cost < theNode.cost then
        (
            -- 既にノードがあり、かつ新しいコストの方が小さければ経路を置換
            theNode.cost = cost
            theNode.parent = parentNode
            
            -- 既にCloseされている場合Openに戻す
            if theNode.isClosed do
            (
                theNode.isClosed = false
                append this._openList theNode
            )
        )
    ),
    
    -- ▼ ポリゴン上の2頂点間にある頂点配列を取得します。
    public fn getVertsBetweenTwoVerts epoly vert1 vert2 =
    (
        -- Meshから初期情報を取得
        local numVerts = polyop.getNumVerts epoly
        local startPos = polyop.getVert epoly vert2
        local goalPos = polyop.getVert epoly vert1
        local dist = length (goalPos - startPos)
        
        -- 親なし、コスト0でスタートノードを作成する
        local startNode = AStarNode vert2 startPos undefined 0.0 dist
        
        -- インスタンス変数を初期化
        this._epoly = epoly
        this._openList = #(startNode)
        this._nodeTable = #()
        this._nodeTable[numVerts] = undefined
        this._nodeTable[vert2] = startNode
        
        -- 関数をキャッシュ
        local getEUV = polyop.getEdgesUsingVert
        local getVUE = polyop.getVertsUsingEdge
        
        -- ゴールノードが見つかるかOpenリストが空になるまでループ
        local goalNode = undefined
        while goalNode == undefined and this._openList.count != 0 do
        (
            -- OpenListから最もコストの小さいノードを検索する
            local idx = this._findMinScoreNodeFromOpenList()
            local theNode = this._openList[idx]
            
            -- ノードがゴールなら終了
            if theNode.id == vert1 then
                goalNode = theNode
            else
            (
                -- ノードをOpenリストから削除しCloseする
                deleteItem this._openList idx
                theNode.isClosed = true
                
                -- 頂点に隣接する頂点リストを取得
                local verts = getVUE epoly (getEUV epoly theNode.id)
                verts[theNode.id] = false
                
                -- 隣接する頂点ノードをOpenする
                for vi in verts do
                    this._openNode vi theNode goalPos
            )
        )
        
        -- 頂点配列を取得
        local resVerts = #()
        while goalNode != undefined do
        (
            append resVerts goalNode.id
            goalNode = goalNode.parent
        )
        
        return resVerts
    ),
    
    -- ▼ ポリゴン上の2頂点間にあるエッジ配列を取得します。
    --    mode: #dist, #steps
    public fn getEdgesBetweenTwoVerts epoly vert1 vert2 =
    (
        -- 2頂点間の頂点配列を取得
        local verts = this.getVertsBetweenTwoVerts epoly vert1 vert2
        
        -- 各頂点に隣接するエッジリストを取得
        local vertEdges = for vi in verts collect
            polyop.getEdgesUsingVert epoly vi
        
        -- ルート上のエッジリストを作成して返す
        for i = 1 to vertEdges.count - 1 collect
            ((vertEdges[i] * vertEdges[i + 1]) as array)[1]
    )
)

テストコード

編集可能ポリゴン上で2頂点を選択し以下を実行します。

-- 編集可能ポリゴンから選択頂点を取得
local epoly = $.baseObject
local selVerts = for v in polyop.getVertSelection epoly collect v

-- オペレーション構造体を作成
local op = PolyAStarOp()

-- 2頂点間の経路頂点を取得
local resVerts = op.getVertsBetweenTwoVerts epoly selVerts[1] selVerts[2]
polyop.setVertSelection epoly resVerts

-- 2頂点間の経路エッジを取得
local resEdges = op.getEdgesBetweenTwoVerts epoly selVerts[1] selVerts[2]
polyop.setEdgeSelection epoly resEdges

経路上の頂点が選択された状態になります。
また、エッジモードに変更すればエッジが選択されているのを確認出来ます。

ただし頂点間に経路が存在しない(2頂点が別の要素上にある)場合、戻り値配列が空のため選択は解除されます。

エッジ取得について

エッジ取得を別関数にしたのは、隣接要素の取得関数(polyop.getVertsUsingEdge等)の処理コストが大きく、毎回取得していると遅くなってしまうからです。

探査コードの以下の部分はエッジを個別に処理せず、直接頂点IDへ再変換しています。
そこからノード自身の頂点IDを引く事によって、隣接頂点のみを取得しています。

-- 頂点に隣接する頂点リストを取得
local verts = getVUE epoly (getEUV epoly theNode.id)
verts[theNode.id] = false

移動コストの計算

移動コストの計算は_openNodeの先頭で行っています。ここの計算式を変更すれば、探査条件を変更する事が出来ます。

-- 頂点座標を取得しコストを計算する
local pos = polyop.getVert this._epoly vertId
local cost = (length (pos - parentNode.pos)) + parentNode.cost

例えば、実距離ではなく経由したノード数をコストにすれば、極力ステップ数を減らすような経路を選択させる事もできます。ただし、単純に定数を加算しただけだと、ゴールまでの距離によって相対的なコストの重さが変化してしまいます。
オブジェクトを拡大しただけで経路が変わってしまうので注意が必要です。

コード (Mesh版)

Editable_Polyではなく、Meshを処理したい場合はこちらを使用してください。
例えばモディファイヤのビューポートメッシュで処理したい場合など、ノードの.meshプロパティからTriMeshを取得して処理する事が出来ます。

コードはほぼ同じなのでDLリンクだけを置いておきます。
TRLib_MeshAStar.ms (5.6 KB)

不可視エッジ

基本的な内容はEPoly版と同じですが、Meshでは不可視エッジの概念がある為、表示されているエッジだけを辿る必要があります。

以下のコードはmeshop.getEdgesUsingVertで取得したエッジリストから、不可視エッジを取り除いています。

-- vertIdを使用する全てのエッジを取得する
local edges = meshop.getEdgesUsingVert theMesh vertId

-- 不可視エッジをフィルタリング
for ei in edges do 
    edges[ei] = getEdgeVis theMesh ((ei - 1) / 3 + 1) ((mod (ei - 1) 3) + 1)

getEdgeVisはフェースIDとフェース内でのインデックス(1~3)を指定する必要がありますが、Meshの場合、エッジIDからこの2つを逆算する事が出来ます。

隣り合うフェースに挟まれたエッジ

Meshの場合、1つのフェースが常に3つのエッジを専有しており、隣り合うフェースの間にあるエッジは2つのエッジが重なっています。つまり、2つの頂点間にあるエッジは、常に1つか2つ存在する事になります。
結果として、Mesh版のgetEdgesBetweenTwoVertsの戻り値はBitArrayの配列になっています。

EPoly版と違い、戻り値を直接setEdgeSelection等に食わせるとエラーになるので注意が必要です。

Mesh版の処理速度

残念な事に、Mesh版はPoly版に比べて大分処理速度が遅いです。
正確には計測していませんが、おおよそ2~10倍程度速度差が出ます。

上記のエッジフィルタリングも1つの原因ですが、どうもTriMeshそのものが若干遅いみたいです。
Editable_Meshであればもう少し速度が出るみたいなのですが、snapshotを作成するコストを考えると、TriMeshをそのまま使うかEditable_Meshに変換するかは、今回に限らず問題になりそうです。

最後に

自分なりにコードを最適化したつもりですが、経路探索の仕組み上それなりに重いです。
特にポリゴン数が多いモデルで、非常に遠い距離で実行するとかなり時間がかかるはずです。(仕組み上、処理時間が無限大になる事はありませんが…。)

ここはこうした方がもっと早くなる等ありましたら、教えて頂けると大変有り難いです。

コメントを残す

メールアドレスが公開されることはありません。