function zp=GridZ(pt,x,y,s0)
%移动曲面拟合法内插(x,y)处的高程
%pt为数据点矩阵，s0为单点平均占用面积
N=length(pt); %点数
n0=8; %搜索点数
%初始化各值
n=1; m=1; %计数器
xp=0; yp=0; d2=0; %原点在(x,y)时数据点坐标和与(x,y)距离的平方
ptin.x=0; ptin.y=0; ptin.z=0; din2=0; %落入搜索范围内的数据点坐标值和与(x,y)距离的平方
ptout.x=0; ptout.y=0; ptout.z=0; dout2=0; %搜索区外的数据点坐标和距离
P=0; %权阵
for k=1:N
xp(k)=pt(k,1)-x; yp(k)=pt(k,2)-y;
d2(k)=xp(k)^2+yp(k)^2;
if d2(k)<s0*n0/pi %搜索半径为n0个点占用范围(当作正方形)的对角线的一半
ptin.x(n)=xp(k);
ptin.y(n)=yp(k);
ptin.z(n)=pt(k,3);
din2(n)=d2(k);
n=n+1;
else %落入搜索范围外
ptout.x(m)=xp(k);
ptout.y(m)=yp(k);
ptout.z(m)=pt(k,3);
dout2(m)=d2(k);
m=m+1;
end
end
nin1=n-1;
nin=nin1;
while nin<8 %若点数不足8个则继续搜索区外
n0=n0+(8-nin); %扩大搜索区
for l=1:N-nin1
if dout2(l)<s0*n0/2
ptin.x(n)=ptout.x(l);
ptin.y(n)=ptout.y(l);
ptin.z(n)=ptout.z(l);
din2(n)=dout2(l);
dout2(l)=inf; %将本次采用的点的距离置无穷以避免重复搜索
n=n+1;
end
end
nin=n-1;
end
P=diag([1./din2]); %权阵
M=[ptin.x.^2;ptin.x.*ptin.y;ptin.y.^2;ptin.x;ptin.y;ones(1,n-1)];
X=inv(M*P*M')*M*P*ptin.z';
zp=X(6); %待定点高程
