www.pudn.com > Improved-AStar-on-3D-Terrain.rar > data.m, change:2012-11-01,size:16580b

```clc
clear

%初始化地形
h=  [1000  800  900  1000  900  700  1100  900  1000  1200  1100
800  600  800  1000  900  800  600   1100  1200  1300  1400
700  700  900  700   900  800  600   600   900   1100  1000
500  600  700  900   800  900   800   900   1000  800   900
800  600  900  1000  900  1000  700   1000  1200  900   800
500  700  800  1100  900   800  1100  1000   900   1200  1100
700  600  900  1300  1200  1400  1200  1000  1200  800  1200
900  1100  900  1200 1500  1200  1300  900   1200  1100  1300
600  800  900  800   1000  1200  900   1100   1000  900  900
900  800  600  900   800  800   800   1100   800  1100  1000
600  900  1000  800  800  600   900   1200   900  1100  600];
h=h./10;

%% 第1轮分解，形成21*21矩阵
%X:1,3,5...21
for i=1:11
for j=1:11
h1(2*i-1,j)=h(i,j);
end
end

%X:2,4,6...20
for i=1:10
for j=1:11
h1(2*i,j)=(h1(2*i-1,j)+h1(2*i+1,j))/2;
end
end

%Y:1,3,5...21
for i=1:21
for j=1:11
h2(i,2*j-1)=h1(i,j);
end
end

%Y:2,4,6...20
for i=1:21
for j=1:10
h2(i,2*j)=(h2(i,2*j-1)+h2(i,2*j+1))/2;
end
end

%% 第2轮分解，形成41*41矩阵
%X:1,3,5...41
for i=1:21
for j=1:21
h3(2*i-1,j)=h2(i,j);
end
end

%X:2,4,6...40
for i=1:20
for j=1:21
h3(2*i,j)=(h3(2*i-1,j)+h3(2*i+1,j))/2;
end
end

%Y:1,3,5...41
for i=1:41
for j=1:21
h4(i,2*j-1)=h3(i,j);
end
end

%Y:2,4,6...40
for i=1:41
for j=1:20
h4(i,2*j)=(h4(i,2*j-1)+h4(i,2*j+1))/2;
end
end
z=h4; % 初始地形，地形栅格顶点的高程

%%
x=1:41;
y=1:41;
figure('name','地形');
mesh(x*5,y*5,z);%生成三维网格图,同mesh(x,y,z(x,y))和mesh(x,y,z(y,x))
axis([0,210,0,210,0,200]);
hold on
xlabel('X:m');
ylabel('Y:m');
zlabel('Z:m');

zx=zeros(40,40);%计算地形栅格的中心点高程
gx=zeros(40,40);%以四领域计算地形栅格的中心点高程
tabu=ones(40,40);

%轮式车辆
dimao=[
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.8	1	0.1	0.1	0.8	0.8	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2	0.2	0.2
0.8	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	1	0.1	0.1	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2
0.8	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	1	0.1	0.1	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2
0.4	0.4	0.4	0.4	0.4	0.4	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	1	0.1	0.1	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.2	0.2	0.2	0.2	0.2
0.4	1	1	0.4	0.4	0	0	0	0.4	0.8	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	1	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.1	0.2	0.2	0.2
1	1	1	0.4	0	0	0	0	0.4	0.8	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	1	1	0.4	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.1	0.2	0.2	0.2
1	0.4	0.4	1	0	0	0	0	0   0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.1	0.1	0.1	1	1	0.4	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.1	0.2	0.2	0.2
0.8	0.4	0.4	1	0	0	0	0	0	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	1	1	0.1	0.1	1	1	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.2	0.2	0.1	0.1	0.1	0.1	0.2	0.2	0.2
0.8	0.4	0.4	0.4	1	0	0	0	0	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	1	1	1	1	1	1	0.4	0.4	0	0.1	0.1	0.1	0.8	0.8	0.2	0.2	0.1	0.1	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	1	1	0	1	0	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	1	0.1	0.1	1	1	1	0	0	0	0.1	0.1	0.1	0.8	0.8	0.8	0.8	0.1	0.1	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	0.4	1	1	1	0	0	0.4	0.4	0.4	0.4	0.8	0.8	1	0.1	0.1	0.1	0.1	0.1	0.1	0	0	0	0	0.1	0.1	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.4	0.4	0.4	0.4	1	0.4	1	0	0.4	0.4	0.4	0.4	0.8	0.8	1	0.1	0.1	0.1	0.1	0.1	0.1	0	0	0	0	0.1	0.1	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.4	0.4	1	0	0.4	0.4	0.4	0.4	0.8	1	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.4	0	0	0.1	0.1	0.1	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.8	0.4	0.4	1	0.4	0.4	0.4	0.4	0.8	1	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.4	0	0	0.1	0.1	0.1	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.4	0.4	0.4	0.8	0.8	0.4	1	0.4	0.4	0.4	0.4	1	1	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.4	0.4	1	0.4	0.4	0.4	1	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.4	1	1	1	1	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.1	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0.8	0	0.8	0.8	0.4	0.4	0.4	0.4	1	0.8	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.4	0.4	0.4	0.8	0.8	0.1	0.1	0.1	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0.8	0	0.8	0.8	0.4	0.4	0.4	0.4	1	1	0.8	0.8	0.8	0.1	0.1	0.1	0.1	0.1	0.1	0.4	0.4	0.4	0.8	0.8	0.1	0.1	0.1	0.1	0.8	0.8	0.8	0.8	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.4	0.4	0.4	0	0	1	1	1	1	0	0	0	0	0	0.4	0.4	0.4	0.1	0.1	0.1	0.1	0.1	0.1	0.8	0.8	0.8	0.8	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.4	0.4	0.4	0.4	0	0	1	1	1	0	0	0	0	0	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.2	0.2	0.2
0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.4	0.4	0.8	0.8	0.8	0	0	0.8	1	0	0	0	0.1	0	0	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.4	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0	0	0.8	1	0	0	0.1	0.1	0.1	0.8	0.8	0	0	0	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.1	0.1	0.1	0.1	0.8	0.8	0	0	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	1	1	1	0.8	0.8	0.8	0.8	0.8	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	1	1	0.8	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0	0	0	0	0	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0	0	0	0	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0	0	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0	0	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	1	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.2	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	1	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8
];

%履带式车辆
% dimao=[
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.9	0.9	0.9	0.3	0.3	0.3	0.5	0.5	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.9	1	0.3	0.3	0.9	0.9	0.3	0.3	0.3	0.5	0.5	0.5	0.5	0.5	0.5	0.5
% 0.9	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	1	0.3	0.3	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.5	0.5	0.5	0.5	0.5
% 0.9	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	1	0.3	0.3	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.5	0.5	0.5	0.5	0.5
% 0.8	0.8	0.8	0.8	0.8	0.8	0	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	1	0.3	0.3	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.5	0.5	0.5	0.5	0.5
% 0.8	1	1	0.8	0.8	0	0	0	0.8	0.9	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	1	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.3	0.5	0.5	0.5
% 1	1	1	0.8	0	0	0	0	0.8	0.9	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.3	0.3	1	1	0.8	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.3	0.5	0.5	0.5
% 1	0.8	0.8	1	0	0	0	0	0   0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.3	0.3	0.3	1	1	0.8	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.3	0.5	0.5	0.5
% 0.9	0.8	0.8	1	0	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	1	1	0.3	0.3	1	1	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.5	0.5	0.3	0.3	0.3	0.3	0.5	0.5	0.5
% 0.9	0.8	0.8	0.8	1	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	1	1	1	1	1	1	0.8	0.8	0	0.3	0.3	0.3	0.9	0.9	0.5	0.5	0.3	0.3	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.8	0.8	1	1	0	1	0	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	1	0.3	0.3	1	1	1	0	0	0	0.3	0.3	0.3	0.9	0.9	0.9	0.9	0.3	0.3	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.8	0.8	0.8	1	1	1	0	0	0.8	0.8	0.8	0.8	0.9	0.9	1	0.3	0.3	0.3	0.3	0.3	0.3	0	0	0	0	0.3	0.3	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.8	0.8	0.8	0.8	1	0.8	1	0	0.8	0.8	0.8	0.8	0.9	0.9	1	0.3	0.3	0.3	0.3	0.3	0.3	0	0	0	0	0.3	0.3	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.8	0.8	0.8	0.8	0.8	1	0	0.8	0.8	0.8	0.8	0.9	1	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.8	0	0	0.3	0.3	0.3	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.8	0.8	0.8	0.9	0.8	0.8	1	0.8	0.8	0.8	0.8	0.9	1	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.8	0	0	0.3	0.3	0.3	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.8	0.8	0.8	0.9	0.9	0.8	1	0.8	0.8	0.8	0.8	1	1	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.8	0.8	1	0.8	0.8	0.8	1	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.8	1	1	1	1	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.3	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0.9	0	0.9	0.9	0.8	0.8	0.8	0.8	1	0.9	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.8	0.8	0.8	0.9	0.9	0.3	0.3	0.3	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0.9	0	0.9	0.9	0.8	0.8	0.8	0.8	1	1	0.9	0.9	0.9	0.3	0.3	0.3	0.3	0.3	0.3	0.8	0.8	0.8	0.9	0.9	0.3	0.3	0.3	0.3	0.9	0.9	0.9	0.9	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.8	0.8	0.8	0	0	1	1	1	1	0	0	0	0	0	0.8	0.8	0.8	0.3	0.3	0.3	0.3	0.3	0.3	0.9	0.9	0.9	0.9	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.8	0.8	0.8	0.8	0	0	1	1	1	0	0	0	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.5	0.5	0.5
% 0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.8	0.8	0.9	0.9	0.9	0	0	0.9	1	0	0	0	0.3	0	0	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.8	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0	0	0.9	1	0	0	0.3	0.3	0.3	0.9	0.9	0	0	0	0	0	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.3	0.3	0.3	0.3	0.9	0.9	0	0	0	0	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	1	1	1	0.9	0.9	0.9	0.9	0.9	0	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0	0	0	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	1	1	0.9	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0	0	0	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0	0	0	0	0	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0	0	0	0	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0	0	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0	0	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0	0	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	1	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% 0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.5	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	1	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9	0.9
% ];

%针对轮式车辆
for i=1:40
for j=1:40
A=5*[j,j,j+1,j+1];
B=5*[i,i+1,i+1,i];
C=[z(j,i),z(j,i+1),z(j+1,i+1),z(j+1,i)];
if  dimao(j,i)==1%硬质路面
fill3(B,A,C,'B');
elseif  dimao(j,i)==0.8%土质路面0.9
fill3(B,A,C,[0.8,0.8,0.8]);
elseif  dimao(j,i)==0.4%草地0.8
fill3(B,A,C,'G');
elseif  dimao(j,i)==0.2%沙地0.5
fill3(B,A,C,'Y');
elseif dimao(j,i)==0.1%树林0.3
fill3(B,A,C,'M');
elseif  dimao(j,i)==0%沼泽/湖泊
fill3(B,A,C,'R');
end
end
end

%针对履带式车辆
%     for i=1:40
%         for j=1:40
%             A=5*[j,j,j+1,j+1];
%             B=5*[i,i+1,i+1,i];
%             C=[z(j,i),z(j,i+1),z(j+1,i+1),z(j+1,i)];
%             if  dimao(j,i)==1%硬质路面
%                  fill3(B,A,C,'B');
%             elseif  dimao(j,i)==0.9%土质路面
%                  fill3(B,A,C,[0.8,0.8,0.8]);
%             elseif  dimao(j,i)==0.8%草地
%                  fill3(B,A,C,'G');
%             elseif  dimao(j,i)==0.5%沙地
%                  fill3(B,A,C,'Y');
%             elseif dimao(j,i)==0.3%树林
%                  fill3(B,A,C,'M');
%             elseif  dimao(j,i)==0%沼泽/湖泊
%                  fill3(B,A,C,'R');
%             end
%         end
%     end

for j=1:40
for i=1:40
zx(j,i)=(z(j,i)+z(j,i+1)+z(j+1,i)+z(j+1,i+1))/4;
gx(j,i)=2*(abs(z(j,i)-zx(j,i))+abs(z(j+1,i)-zx(j,i))+abs(z(j,i+1)-zx(j,i))+abs(z(j+1,i+1)-zx(j,i)))/(sqrt(2)*5);
if gx(j,i)>5 ||dimao(j,i)==0
plot3(i*5+2.5,j*5+2.5,zx(j,i),'KX');
tabu(j,i)=0;
end
end
end

% startx=8;starty=27;
% endx=35;endy=17;

%     plot3(5*startx+2.5,5*starty+2.5,zx(starty,startx),'--o','LineWidth',2,...
%                            'MarkerEdgeColor','k',...
%                            'MarkerFaceColor','g',...
%                            'MarkerSize',10);
%     plot3(5*endx+2.5,5*endy+2.5,zx(endy,endx),'--o','LineWidth',2,...
%                            'MarkerEdgeColor','k',...
%                            'MarkerFaceColor','g',...
%                            'MarkerSize',10);
%     text(5*startx+2.5,5*starty+2.5,zx(starty,startx)+10,'S');
%     text(5*endx+2.5,5*endy+2.5,zx(endy,endx)+10,'T');

save HeightData z zx tabu dimao %把HeightData保存为HeightData.mat文件```