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

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 =
    (
        -- ２頂点間の頂点配列を取得
        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]
    )
)