//Matlab語言 functionA*(start,goal) closedset := the empty set //已经被估算的節點集合 openset := set containing the initial node //將要被估算的節點集合，初始只包含start came_from := empty map g_score[start] := 0 //g(n) h_score[start] := heuristic_estimate_of_distance(start, goal) //通過估計函數 估計h(start) f_score[start] := h_score[start] //f(n)=h(n)+g(n)，由於g(n)=0，所以省略 while openset is not empty //當將被估算的節點存在時，執行循環 x := the node in openset having the lowest f_score[] value //在將被估計的集合中找到f(x)最小的節點 if x = goal //若x為終點，執行 return reconstruct_path(came_from,goal) //返回到x的最佳路徑 remove x from openset //將x節點從將被估算的節點中刪除 add x to closedset //將x節點插入已經被估算的節點 for each y in neighbor_nodes(x) //循環遍歷與x相鄰節點 if y in closedset //若y已被估值，跳過 continue tentative_g_score := g_score[x] + dist_between(x,y) //從起點到節點y的距離
if y not in openset //若y不是將被估算的節點 tentative_is_better := true //暫時判斷為更好 elseif tentative_g_score < g_score[y] //如果起點到y的距離小於y的實際距離 tentative_is_better := true //暫時判斷為更好 else tentative_is_better := false //否則判斷為更差 if tentative_is_better = true //如果判斷為更好 came_from[y] := x //將y設為x的子節點 g_score[y] := tentative_g_score //更新y到原點的距離 h_score[y] := heuristic_estimate_of_distance(y, goal) //估計y到終點的距離 f_score[y] := g_score[y] + h_score[y] add y to openset //將y插入將被估算的節點中 return failure functionreconstruct_path(came_from,current_node) if came_from[current_node] is set p = reconstruct_path(came_from,came_from[current_node]) return (p + current_node) else return current_node
