% 北太天元上實(shí)現(xiàn)A星算法,找到兩點(diǎn)之間的最短路徑，兩點(diǎn)之間可能有障礙物，

% 兩間之間的道路只能是水平或者豎直，每一步的距離一定要是dL(這里是整數(shù)1)

% 腳步不能落在障礙物上

clear all;

clf

close all

load_plugin("time")

%hold on;

%設(shè)置探索區(qū)域的邊界

Estart_x = 0;?Estart_y = 0;

Weight = 40;?High = 40;

dL = 1;??% 每一步走的最小距離

%% 生成所有的網(wǎng)格點(diǎn)

xL = 0:dL:Weight;

yL = 0:dL:High;

[XL , YL] = meshgrid(xL,yL);??%

C = zeros(size(XL));???%標(biāo)記點(diǎn)是否可以到達(dá)

%--------------------%

%邊界標(biāo)記為100

C(1,:) = 100;??C(:,1) = 100;?

C(end,:) = 100; C(:,end) = 100;

hold on

%%障礙物標(biāo)記為80

%--------------------%

barrier_1 = 5:30; %障礙物的橫坐標(biāo)是從20到30

a1(1:length(barrier_1)) = 20;?% 障礙物的縱坐標(biāo)

C(barrier_1,20)=80; %

plot(barrier_1,a1,'k-*');

axis([Estart_x-5 Weight 0 High]);

axis equal;?

%--------------------%

global startx

global starty

global endx;

global endy;

startx = 1 ;

starty = 2;

endx = 30;

endy = 35;

tic;

%% A星算法計(jì)算最短距離

[dis,road] = Axing_fun(startx,starty,endx,endy,Estart_x,Estart_y,Weight,High,C);

disp("A星算法計(jì)算的時(shí)間是");

toc

fprintf('得到的最短距離是%d\n',dis);

line(road(:,1),road(:,2))

text(road(1,1),road(1,2),'起點(diǎn)')

text(road(end,1),road(end,2),'終點(diǎn)')

hold off

%x 是一個(gè)cell，每一個(gè)元素是一個(gè)1x1的double

function A = cell2mat(x)

???[m,n] = size(x);

???A = zeros(m,n);

???for k=1:m

?????????for l=1:n

????????????A(k,l) = x{k,l};

?????????end

???end

end

function A = sortrows(B, k)

??????[m,n] = size(B);

??????if( m <= 1 || k<0 || k >n)

?????????A = B;

?????????return

??????end

????col_k = cell2mat(B(:,k));???

??????[~, ind] = sort(col_k);

??????A = B(ind,:);

end???