void push(Stack *s,int m) // 入栈 { s->printout[++(s->top)] = m; }
int pop(Stack *s) // 出栈 { return s->printout[s->top–]; }
int in[MAX_LEN]; // 若in[i] = 1,则说明顶点vi已在集合S中 int dist[MAX_LEN]; // dist[i]是"顶点vs"到"顶点i"的最短路径的长度 int prev[MAX_LEN]; // prev[i]的值是"顶点vs"到"顶点i"的最短路径所经历的全部顶点中,位于"顶点i"之前的那个顶点
若当前处理节点的相邻格子已经在Open List中,则检查这条路径是否更优,即计算经由当前处理节点到达那个方格是否具有更小的 G值。如果没有,不做任何操作。相反,如果G值更小,则把那个方格的父节点设为当前处理节点 ( 我们选中的方格 ) ,然后重新计算那个方格的 F 值和 G 值。
若当前处理节点的相邻格子不在Open List中,那么把它加入,并将它的父节点设置为该节点。
按照上述规则我们继续搜索,选择起点右边的方格作为当前处理节点。它的外框用蓝线打亮,被放入了close list 中。然后我们检查与它相邻的方格。它右侧的3个方格是墙壁,我们忽略。它左边的方格是起点,在 close list 中,我们也忽略。其他4个相邻的方格均在 open list 中,我们需要检查经由当前节点到达那里的路径是否更好。我们看看上面的方格,它现在的G值为14 ,如果经由当前方格到达那里, G值将会为20( 其中10为从起点到达当前方格的G值,此外还要加上从当前方格纵向移动到上面方格的G值10) ,因此这不是最优的路径。看图就会明白直接从起点沿对角线移动到那个方格比先横向移动再纵向移动要好。
当把4个已经在 open list 中的相邻方格都检查后,没有发现经由当前节点的更好路径,因此不做任何改变。接下来要选择下一个待处理的节点。因此再次遍历open list ,现在open list中只有 7 个方格了,我们需要选择F值最小的那个。这次有两个方格的F值都是54,选哪个呢?没什么关系。从速度上考虑,选择最后加入 open list 的方格更快。因此选择起点右下方的方格,如下图所示。
接下来把起点右下角F值为54的方格作为当前处理节点,检查其相邻的方格。我们发现它右边是墙(墙下面的一格也忽略掉,假定墙角不能直接穿越),忽略之。这样还剩下 5 个相邻的方格。当前方格下面的 2 个方格还没有加入 open list ,所以把它们加入,同时把当前方格设为他们的父亲。在剩下的 3 个方格中,有 2 个已经在 close list 中 ( 一个是起点,一个是当前方格上面的方格,外框被加亮的 ) ,我们忽略它们。最后一个方格,也就是当前方格左边的方格,检查经由当前方格到达那里是否具有更小的 G 值。没有,因此我们准备从 open list 中选择下一个待处理的方格。
不断重复这个过程,直到把终点也加入到了 open list 中,此时如下图所示。注意在起点下方 2 格处的方格的父亲已经与前面不同了。之前它的G值是28并且指向它右上方的方格。现在它的 G 值为 20 ,并且指向它正上方的方格。这是由于在寻路过程中的某处使用新路径时G值更小,因此父节点被重新设置,G和F值被重新计算。
function A*(start, goal)
// The set of nodes already evaluated.
closedSet := {}
// The set of currently discovered nodes still to be evaluated. Initially, only the start node is known.
openSet := {start}
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the most efficient previous step.
cameFrom := the empty map
</span><span style="color: #008000">//</span><span style="color: #008000"> For each node, the cost of getting from the start node to that node.</span>
gScore := map with <span style="color: #0000ff">default</span><span style="color: #000000"> value of Infinity
</span><span style="color: #008000">//</span><span style="color: #008000"> The cost of going from start to start is zero.</span>
gScore[start] := <span style="color: #800080">0</span>
<span style="color: #008000">//</span><span style="color: #008000"> For each node, the total cost of getting from the start node to the goal
</span><span style="color: #008000">//</span><span style="color: #008000"> by passing by that node. That value is partly known, partly heuristic.</span>
fScore := map with <span style="color: #0000ff">default</span><span style="color: #000000"> value of Infinity
</span><span style="color: #008000">//</span><span style="color: #008000"> For the first node, that value is completely heuristic.</span>
fScore[start] :=<span style="color: #000000"> heuristic_cost_estimate(start, goal)
</span><span style="color: #0000ff">while</span> openSet <span style="color: #0000ff">is</span><span style="color: #000000"> not empty
current :</span>= the node <span style="color: #0000ff">in</span><span style="color: #000000"> openSet having the lowest fScore[] value
</span><span style="color: #0000ff">if</span> current =<span style="color: #000000"> goal
</span><span style="color: #0000ff">return</span><span style="color: #000000"> reconstruct_path(cameFrom, current)
openSet.Remove(current)
closedSet.Add(current)
</span><span style="color: #0000ff">for</span><span style="color: #000000"> each neighbor of current
</span><span style="color: #0000ff">if</span> neighbor <span style="color: #0000ff">in</span><span style="color: #000000"> closedSet
</span><span style="color: #0000ff">continue</span> <span style="color: #008000">//</span><span style="color: #008000"> Ignore the neighbor which is already evaluated.
</span><span style="color: #008000">//</span><span style="color: #008000"> The distance from start to a neighbor</span>
tentative_gScore := gScore[current] +<span style="color: #000000"> dist_between(current, neighbor)
</span><span style="color: #0000ff">if</span> neighbor not <span style="color: #0000ff">in</span> openSet <span style="color: #008000">//</span><span style="color: #008000"> Discover a new node</span>
openSet.Add(neighbor) else if tentative_gScore >= gScore[neighbor] continue // This is not a better path.
</span><span style="color: #008000">//</span><span style="color: #008000"> This path is the best until now. Record it!</span>
cameFrom[neighbor] :=<span style="color: #000000"> current
gScore[neighbor] :</span>=<span style="color: #000000"> tentative_gScore
fScore[neighbor] :</span>= gScore[neighbor] +<span style="color: #000000"> heuristic_cost_estimate(neighbor, goal)
</span><span style="color: #0000ff">return</span><span style="color: #000000"> failure
function reconstruct_path(cameFrom, current) total_path :=
[current]
while current
in
cameFrom.Keys: current :=
cameFrom[current] total_path.append(current)
return total_path
openSet </span>=<span style="color: #000000"> []
heappush(openSet, (fscore[start], start)) </span><span style="color: #008000">#</span><span style="color: #008000"> 往堆中插入一条新的值 </span>
<span style="color: #008000">#</span><span style="color: #008000"> while openSet is not empty</span>
<span style="color: #0000ff">while</span><span style="color: #000000"> openSet:
</span><span style="color: #008000">#</span><span style="color: #008000"> current := the node in openSet having the lowest fScore value</span>
current = heappop(openSet)[1] <span style="color: #008000">#</span><span style="color: #008000"> 从堆中弹出fscore最小的节点 </span>
<span style="color: #0000ff">if</span> current ==<span style="color: #000000"> goal:
</span><span style="color: #0000ff">return</span><span style="color: #000000"> reconstruct_path(came_from, current)
close_set.add(current)
</span><span style="color: #0000ff">for</span> i, j <span style="color: #0000ff">in</span> directions: <span style="color: #008000">#</span><span style="color: #008000"> 对当前节点的 8 个相邻节点一一进行检查</span>
neighbor = current[0] + i, current[1] +<span style="color: #000000"> j
</span><span style="color: #008000">#</span><span style="color: #008000"># 判断节点是否在地图范围内,并判断是否为障碍物</span>
<span style="color: #0000ff">if</span> 0 <= neighbor[0] <<span style="color: #000000"> array.shape[0]:
</span><span style="color: #0000ff">if</span> 0 <= neighbor[1] < array.shape[1<span style="color: #000000">]:
</span><span style="color: #0000ff">if</span> array[neighbor[0]][neighbor[1]] == 1: <span style="color: #008000">#</span><span style="color: #008000"> 1为障碍物</span>
<span style="color: #0000ff">continue</span>
<span style="color: #0000ff">else</span><span style="color: #000000">:
</span><span style="color: #008000">#</span><span style="color: #008000"> array bound y walls</span>
<span style="color: #0000ff">continue</span>
<span style="color: #0000ff">else</span><span style="color: #000000">:
</span><span style="color: #008000">#</span><span style="color: #008000"> array bound x walls</span>
<span style="color: #0000ff">continue</span>
<span style="color: #008000">#</span><span style="color: #008000"> Ignore the neighbor which is already evaluated.</span>
<span style="color: #0000ff">if</span> neighbor <span style="color: #0000ff">in</span><span style="color: #000000"> close_set:
</span><span style="color: #0000ff">continue</span>
<span style="color: #008000">#</span><span style="color: #008000"> The distance from start to a neighbor via current</span>
tentative_gScore = gscore[current] +<span style="color: #000000"> dist_between(current, neighbor)
</span><span style="color: #0000ff">if</span> neighbor <span style="color: #0000ff">not</span> <span style="color: #0000ff">in</span> [i[1] <span style="color: #0000ff">for</span> i <span style="color: #0000ff">in</span> openSet]: <span style="color: #008000">#</span><span style="color: #008000"> Discover a new node</span>
heappush(openSet, (fscore.get(neighbor, numpy.inf), neighbor)) elif tentative_gScore >= gscore.get(neighbor, numpy.inf): # This is not a better path. continue
<span style="color: #008000">#</span><span style="color: #008000"> This path is the best until now. Record it!</span>
came_from[neighbor] =<span style="color: #000000"> current
gscore[neighbor] </span>=<span style="color: #000000"> tentative_gScore
fscore[neighbor] </span>= tentative_gScore +<span style="color: #000000"> heuristic_cost_estimate(neighbor, goal)
</span><span style="color: #0000ff">return</span><span style="color: #000000"> False
if name == “main”: nmap = numpy.array([ [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,0,1,1,1,1,1,1,1,1,1,1,1,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,0,1,1,1,1,1,1,1,1,1,1,1,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0], [1,1,1,1,1,1,1,1,1,1,1,1,0,1], [0,0,0,0,0,0,0,0,0,0,0,0,0,0]])
openSet </span>=<span style="color: #000000"> []
heappush(openSet, (fscore[start], start)) </span><span style="color: #008000">#</span><span style="color: #008000"> 往堆中插入一条新的值 </span>
<span style="color: #008000">#</span><span style="color: #008000"> while openSet is not empty</span>
<span style="color: #0000ff">while</span><span style="color: #000000"> openSet:
</span><span style="color: #008000">#</span><span style="color: #008000"> current := the node in openSet having the lowest fScore value</span>
current = heappop(openSet)[1] <span style="color: #008000">#</span><span style="color: #008000"> 从堆中弹出fscore最小的节点 </span>
<span style="color: #0000ff">if</span> current ==<span style="color: #000000"> goal:
</span><span style="color: #0000ff">return</span><span style="color: #000000"> reconstruct_path(came_from, current)
close_set.add(current)
</span><span style="color: #0000ff">for</span> i, j <span style="color: #0000ff">in</span> directions: <span style="color: #008000">#</span><span style="color: #008000"> 对当前节点的 8 个相邻节点一一进行检查</span>
neighbor = current[0] + i, current[1] +<span style="color: #000000"> j
</span><span style="color: #008000">#</span><span style="color: #008000"># 判断节点是否在地图范围内,并判断是否为障碍物</span>
<span style="color: #0000ff">if</span> 0 <= neighbor[0] <<span style="color: #000000"> array.shape[0]:
</span><span style="color: #0000ff">if</span> 0 <= neighbor[1] < array.shape[1<span style="color: #000000">]:
</span><span style="color: #0000ff">if</span> array[neighbor[0]][neighbor[1]] == 0: <span style="color: #008000">#</span><span style="color: #008000"> 0为障碍物</span>
<span style="color: #0000ff">continue</span>
<span style="color: #0000ff">else</span><span style="color: #000000">:
</span><span style="color: #008000">#</span><span style="color: #008000"> array bound y walls</span>
<span style="color: #0000ff">continue</span>
<span style="color: #0000ff">else</span><span style="color: #000000">:
</span><span style="color: #008000">#</span><span style="color: #008000"> array bound x walls</span>
<span style="color: #0000ff">continue</span>
<span style="color: #008000">#</span><span style="color: #008000"> Ignore the neighbor which is already evaluated.</span>
<span style="color: #0000ff">if</span> neighbor <span style="color: #0000ff">in</span><span style="color: #000000"> close_set:
</span><span style="color: #0000ff">continue</span>
<span style="color: #008000">#</span><span style="color: #008000"> The distance from start to a neighbor via current</span>
tentative_gScore = gscore[current] +<span style="color: #000000"> dist_between(current, neighbor)
</span><span style="color: #0000ff">if</span> neighbor <span style="color: #0000ff">not</span> <span style="color: #0000ff">in</span> [i[1] <span style="color: #0000ff">for</span> i <span style="color: #0000ff">in</span> openSet]: <span style="color: #008000">#</span><span style="color: #008000"> Discover a new node</span>
heappush(openSet, (fscore.get(neighbor, np.inf), neighbor)) elif tentative_gScore >= gscore.get(neighbor, np.inf): # This is not a better path. continue
<span style="color: #008000">#</span><span style="color: #008000"> This path is the best until now. Record it!</span>
came_from[neighbor] =<span style="color: #000000"> current
gscore[neighbor] </span>=<span style="color: #000000"> tentative_gScore
fscore[neighbor] </span>= tentative_gScore +<span style="color: #000000"> heuristic_cost_estimate(neighbor, goal)
</span><span style="color: #0000ff">return</span><span style="color: #000000"> False
【推荐】华为云11.11普惠季 血拼风暴 一促即发
【拼团】腾讯云服务器拼团活动又双叒叕来了!
【推荐】腾讯云新注册用户域名抢购1元起
· 从谷歌出来的人,为什么领导力那么强?
· 瓜子又融了1.62亿美金,可二手车的拐点还没到来
· 深交所向乐视网发问询函 聚焦控制权变化、关联交易等问题
· 这家创企正建立农作物数据库 帮助印度农民用AI种庄稼
· 恒大造车路线的偶然与必然
» 更多新闻...