最短経路探索(A-Starアルゴリズム)

ダイクストラ法との違いはヒューリスティックコストという推測値を利用して、ゴールに近いポイント順に処理をしていき、ゴールにたどり着いた時点で計算を止めるので、すべてを計算してしまうダイクストラに比べるとコストが低くなる。

Input0にConvertline SOPでポリライン化した地形を、Input1に始点と終点を含むポリラインを差す。

// RunOver: Detail
// input0: Polyline Graph
// input1: Polyline

vector start = point(1, "P", 0);
vector goal = point(1, "P", 1);

// 始点
vector cross;
vector uv;
vector origin = start + set(0, 1, 0) * 50000;
vector dir = set(0, -1, 0) * 100000;
int hitId = intersect(1, origin, dir, cross, uv);
if(hitId >= 0)
    i@startId = nearpoint(0, cross);
else
    i@startId = nearpoint(0, start);

// 終点
origin = goal + set(0, 1, 0) * 50000;
dir = set(0, -1, 0) * 100000;
hitId = intersect(1, origin, dir, cross, uv);
if(hitId >= 0)
    i@goalId = nearpoint(0, cross);
else
    i@goalId = nearpoint(0, goal);

ポイントから隣接するエッジからの情報を格納する。

// RunOver: Points

// 隣り合うポイント
int neighbours[] = neighbours(0, @ptnum);
i[]@connectedPoint = neighbours;

// 隣り合うポリライン
int connectedPrim[] = pointprims(0, @ptnum);
i[]@connectedPrim = connectedPrim;

// ポリラインの長さ
f[]@distance;
for(int i = 0; i < len(neighbours); i++)
{
    vector pos = point(0, "P", neighbours[i]);
    f[]@distance[i] = length(pos - @P);
}

Python SOPでA-Starの経路探索を記述する。

#
# A*の経路探索
#

node = hou.pwd()
geo = node.geometry()

import time

# Start
start_time = time.time()

# 地点クラス
class Node:
    def __init__(self, connect, dist, position):
        
        self.index = -1
    
        # 直前のインデックス
        self.prevPt = -1
        # フラグ
        self.flag = 0
        # コスト(一応大きな数を入れておく)
        self.totalCost = 100000.0
        # 接続する道路
        self.connect = connect
        # 道路のコスト(距離)
        self.dist = dist
        # 座標
        self.position = position
        # 推測コスト
        self.heuristicCost = 0.0
        # それまでのコスト+推測コスト
        self.score = 0.0
        
start = geo.intAttribValue('startId')
goal = geo.intAttribValue('goalId')

# ルートリスト
openList = []

# ポイント総数
nPoints = len(geo.points())

# 分岐するルートと距離の情報を格納する
connect = []
dist = []
position = []

for pt in geo.points():
    connect.append(pt.intListAttribValue("connectedPoint"))
    dist.append(pt.floatListAttribValue("distance"))
    pos = pt.position()
    position.append(pos)
    
# ノードクラスのリストを初期化
node = []
for i in range(nPoints):
    node.append(Node(connect[i], dist[i], position[i]))
    node[i].index = i
    
#
# 計算開始
#

# startからのルートをリストに入れる
node[start].totalCost = 0.0
node[start].heuristicCost = (node[goal].position - node[start].position).length()
node[start].score = node[start].heuristicCost;
node[start].prevPt = start
node[start].flag = 1

openList.append(start)    

# 比較用のクラスリスト
classList = []
classList.append(node[start])
    
# リストが空になるまで続ける
while len(openList) > 0:

    # scoreをラムダ式で昇順に並べ替え
    classList = sorted(classList, key=lambda x: x.score)

    # コストの低いものを取得してリストから削除する
    currentPt = classList[0].index
    #print "current:" + str(currentPt) + ":" + str(node[currentPt].score)
    openList.remove(currentPt)
    classList.pop(0)
    
    for j in range(len(node[currentPt].connect)):
        
        nextPt = node[currentPt].connect[j]
        cost = node[currentPt].totalCost + node[currentPt].dist[j]
        
        # もし探索済みで、新しいルートのほうがコストが低い場合は更新する
        if node[nextPt].flag == 1:
            if cost < node[nextPt].totalCost:
                node[nextPt].totalCost = cost
                node[nextPt].score = node[nextPt].totalCost + node[nextPt].heuristicCost
                node[nextPt].prevPt = currentPt
        else:
            node[nextPt].totalCost = cost
            node[nextPt].heuristicCost = (node[goal].position - node[nextPt].position).length()
            node[nextPt].score = node[nextPt].totalCost + node[nextPt].heuristicCost
            node[nextPt].prevPt = currentPt
            node[nextPt].flag = 1
            openList.append(nextPt)
            classList.append(node[nextPt])
            
            # ゴールだった場合
            if nextPt == goal:
                #print "num search:" + str(i)
                break
            
    else:
        continue
    break
    
# ルートを検索
if node[goal].prevPt != -1:
    route = []
    
    pt = goal
    while(pt != start):
        route.append(pt)
        pt = node[pt].prevPt
            
    route.append(start)
    route.reverse()
            
    #print route
    
    # Detailに保存
    geo.addArrayAttrib(hou.attribType.Global, 'route', hou.attribData.Int, 1)
    geo.setGlobalAttribValue('route', route)
    
    geo.addAttrib(hou.attribType.Global, 'dist', 0.0)
    geo.setGlobalAttribValue('dist', node[goal].totalCost)
    
    geo.addAttrib(hou.attribType.Global, 'start', 0)
    geo.setGlobalAttribValue('start', start)
    
    geo.addAttrib(hou.attribType.Global, 'goal', 0)
    geo.setGlobalAttribValue('goal', goal)
else:
    print ('No Route Found.')

# End
elapsed_time = time.time() - start_time
#print ("A-star_route::time:{0}".format(elapsed_time) + "[sec]")

Python SOPの結果からラインを生成するVEXコード。

// RunOver: Detail
// input0: None
// input1: Python SOP

int route[] = detail(1, "route", 0);

int prim = addprim(0, "polyline");

for(int i = 0; i < len(route); i++)
{
    vector pos = point(1, "P", route[i]);
    int pt = addpoint(0, pos);
    addvertex(0, prim, pt);
}

環境:Houdini 19.5.752

タイトルとURLをコピーしました