function [path, ilosc_w ] = A_gwiazdka(ms,ws,start,meta) m = size(ms); closed = Inf; open = [start, 0]; cost = Inf(m,1); cost(start) = 0; prev = Inf(m,1); ilosc_w=0; while ~isempty(open) open = sortrows(open, 2); x = open(1,1); open = open(2:end, 1:2); closed(end + 1) = x; ilosc_w=ilosc_w+1; if(x == meta) pathcost = cost(x); path = reconstructpath(prev, x); return; end for y = 1:m if(find(closed == y)) continue end vek1=[ws(meta,2)-ws(y,2),ws(meta,3)-ws(y,3)]; vek2=[ws(meta,2)-ws(x,2),ws(meta,3)-ws(x,3)]; dl1=sqrt(vek1(1,1)*vek1(1,1)+vek1(1,2)*vek1(1,2)); dl2=sqrt(vek2(1,1)*vek2(1,1)+vek2(1,2)*vek2(1,2)); nc = cost(x) + ms(x,y) + dl2; if nc < cost(y) + dl1 cost(y) = nc; prev(y) = x; end open = [open;[y, cost(y)+dl1]]; end end end pathcost = []; path = []; function path = reconstructpath(prev, v) if (prev(v) == Inf) path = v; return; end path = [reconstructpath(prev, prev(v)), v]; end end