三维点3次B样条插补算法修改

#改MATLAB3次B样条插补代码

怎么把下面二维点代码改成三维点（X,Y,Z）插值算法？
```mlx

%三次B样条插值，给定插值点及两个边界条件，反算出控制点，由控制点得到分段曲线
%pointInput插值点
pointInput=[1 209.1;1.033 209.525;1.067 209.273;1.1 209.277;
    1.133 208.734;1.167 208.693;1.2 208.852;1.233 208.722;1.267 208.746;1.3 208.759];
%pointInput=[0 10;5 8.66;10 0];
%用第三种边界条件 https://wenku.baidu.com/view/2482396e011ca300a6c390b3.html
N=length(pointInput);
k=3;
A1=eye(N+2,N+2)*4;
A1(1,1)=6;A1(1,2)=-6;A1(N+2,N+1)=6;A1(N+2,N+2)=-6;
for i=2:N+1
   A1(i,i-1)=1;A1(i,i+1)=1;
end
%①先求x、y坐标，如果是三维点，还要求z
b1=[0;pointInput(:,1);0]*6;%注意系数
px=Chase_method(A1,b1);%用追赶法求得所有控制点
b2=[0;pointInput(:,2);0]*6;
py=Chase_method(A1,b2);
%确定节点矢量
u=zeros(1,N+1+k+2);%0:1/(N+1+k+2-1):1;
for i=1:k+1
   u(length(u)-i+1)=1;
end
l=zeros(1,N+1);%控制多边形各边长
for i=1:N+1
    %注意如果用到三维点，这里要作修改
   l(i)=sqrt((px(i+1)-px(i))^2+(py(i+1)-py(i))^2);
end
sum1=0;%用哈德利-贾德方法决定节点矢量
for g=k+1:N+1+1
    for j=g-k:g-1
        sum1=sum1+l(j);
    end
end
for i=k+2:N+2
   u(i)=sum(l(i-k:i-1))/sum1+u(i-1);
end
%用基函数求值
%uTestArray=0:0.01:1;
for uTest=0:0.01:1
%uTest=0.5;
%u3Array=[uTest^3 uTest^2 uTest 1];%三次B样条专属
score=find(uTest>=u,1,'last');%确定该参数方程的参数值所属的分段
if uTest==1 score=find(u==1,1)-1;end
score=score-k;%表示第score段样条曲线
A2=[-1 3 -3 1;
    3 -6 3 0;
    -3 0 3 0;
    1 4 1 0];
QControlx=[];
QControly=[];
for i=1:k+1
    QControlx=[QControlx px(score+i-1)];%由段数得知控制点
    QControly=[QControly py(score+i-1)];
end
uNode=1;%上面的uTest是整体参数，用来确定是哪段曲线，要将整体参数转化为局部参数t
t=(uTest-u(score+k))/(u(score+1+k)-u(score+k));
for i=1:k
    uNode=[t^i uNode];
end
QControlx=QControlx.';
QControly=QControly.';
Qx=1/6*uNode*A2*QControlx;
Qy=1/6*uNode*A2*QControly;
plot(Qx,Qy,'*');hold on;
end
plot(pointInput(:,1),pointInput(:,2),'r');

function [ x ] = Chase_method( A, b )
%Chase method 追赶法求三对角矩阵的解
%   A为三对角矩阵的系数，b为等式右端的常数项，返回值x即为最终的解
%   注：A尽量为方阵，b一定要为列向量
%% 求追赶法所需L及U
T = A;
for i = 2 : size(T,1)
    T(i,i-1) = T(i,i-1)/T(i-1,i-1);
    T(i,i) = T(i,i) - T(i-1,i) * T(i,i-1);
end
L = zeros(size(T));
L(logical(eye(size(T)))) = 1;   %对角线赋值1
for i = 2:size(T,1)
    for j = i-1:size(T,1)
        L(i,j) = T(i,j);
        break;
    end
end
U = zeros(size(T));
U(logical(eye(size(T)))) = T(logical(eye(size(T))));
for i = 1:size(T,1)
    for j = i+1:size(T,1)
        U(i,j) = T(i,j);
        break;
    end
end
%% 利用matlab解矩阵方程的遍历直接求解
y = L\b;
x = U\y;
end

```

你可以将二维点的代码适当地修改一下，就能得到三维点的代码：

%三次B样条插值，给定插值点及两个边界条件，反算出控制点，由控制点得到分段曲线
%pointInput插值点，每一行的三个数字分别为X, Y, Z坐标值
pointInput=[1 209.1  100;1.033 209.525  101;1.067 209.273 102;1.1 209.277  103;    1.133 208.734 104;1.167 208.693  105;1.2 208.852 106;1.233 208.722 107;1.267 208.746 108;1.3 208.759  109];
%用第三种边界条件 https://wenku.baidu.com/view/2482396e011ca300a6c390b3.html
N=length(pointInput);
k=3;
A1=eye(N+2,N+2)*4;
A1(1,1)=6;A1(1,2)=-6;A1(N+2,N+1)=6;A1(N+2,N+2)=-6;
for i=2:N+1
   A1(i,i-1)=1;A1(i,i+1)=1;
end
%①先求x、y、z坐标
b1=[0;pointInput(:,1);0]*6;%注意系数
px=Chase_method(A1,b1);%用追赶法求得所有控制点
b2=[0;pointInput(:,2);0]*6;
py=Chase_method(A1,b2);
b3=[0;pointInput(:,3);0]*6;
pz=Chase_method(A1,b3);
%确定节点矢量
u=zeros(1,N+1+k+2);%0:1/(N+1+k+2-1):1;
for i=1:k+1
   u(length(u)-i+1)=1;
end
l=zeros(1,N+1);%控制多边形各边长
for i=1:N+1
    %注意如果用到三维点，这里要作修改
l(i)=sqrt((px(i+1)-px(i))^2+(py(i+1)-py(i))^2+(pz(i+1)-pz(i))^2);
end
t=zeros(1,N+1);
for i=2:N+1
t(i)=t(i-1)+l(i-1);
end
%二分法求出每个插值点对应的参数值ti
%这里的每一行的三个数字分别为X, Y, Z坐标值
p=[px' py' pz'];
for i=1:N
t1=t(i);t2=t(i+1);
ti=(t1+t2)/2;
%根据参数值求出该点的x、y、z坐标
while t2-t1>0.0001
if NBS(u,t,p,k,ti)>=ti
t2=ti;
else
t1=ti;
end
ti=(t1+t2)/2;
end
end
%求出所有插值点的x、y、z坐标
X=zeros(N,1);
Y=zeros(N,1);
Z=zeros(N,1);
for i=1:N
X(i)=NBS_xyz(u,t,px,k,ti);
Y(i)=NBS_xyz(u,t,py,k,ti);
Z(i)=NBS_xyz(u,t,pz,k,ti);
end
%画图
%在三维空间中画出插值点、控制点和分段曲线
plot3(pointInput(:,1),pointInput(:,2),pointInput(:,3),'o',px,py,pz,'*',X,Y,Z);
%用hold on使图像不消失
hold on;
%显示图像
%给定插值点
title('三次B样条插值');
xlabel('x');ylabel('y');zlabel('z');
%显示图例
legend('插值点','控制点','分段曲线');for i=1:N
l(i)=sqrt((pointInput(i+1,1)-pointInput(i,1))^2+(pointInput(i+1,2)-pointInput(i,2))^2+(pointInput(i+1,3)-pointInput(i,3))^2);
end
L=zeros(1,N+k+2);%控制多边形总长
for i=1:N+k+2
L(i)=sum(l(1:i));
end
%根据控制多边形总长L算出U
for i=2:N+1+k
u(i)=u(i-1)+l(i-1)/L(N+k+2-1);
end
%计算各控制点对应的参数u
P=zeros(N+1+k+2,3);
for i=1:N+1+k+2
P(i,1)=px(i);
P(i,2)=py(i);
P(i,3)=pz(i);
end
%计算插值点对应的参数u
U=zeros(N,1);
for i=1:N
[~,U(i)]=min(abs(u-pointInput(i,1)));
end
%求取控制顶点，注意下标
Control_point=zeros(N,4,3);
for i=1:N
Control_point(i,1,1)=P(U(i),1);
Control_point(i,1,2)=P(U(i),2);
Control_point(i,1,3)=P(U(i),3);
Control_point(i,2,1)=P(U(i)+1,1);
Control_point(i,2,2)=P(U(i)+1,2);
Control_point(i,2,3)=P(U(i)+1,3);
Control_point(i,3,1)=P(U(i)+2,1);
Control_point(i,3,2)=P(U(i)+2,2);
Control_point(i,3,3)=P(U(i)+2,3);
Control_point(i,4,1)=P(U(i)+3,1);
Control_point(i,4,2)=P(U(i)+3,2);
Control_point(i,4,3)=P(U(i)+3,3);
end
%根据控制顶点求取分段曲线
%注意要算出u的增量，每一段的u不一样，要换算成0-1l(i)=sqrt((pointInput(i,1)-pointInput(i-1,1))^2+(pointInput(i,2)-pointInput(i-1,2))^2+(pointInput(i,3)-pointInput(i-1,3))^2);
end
%确定矩阵L
L=zeros(N-k,N+1);
for i=1:N-k
for j=i:i+k
L(i,j)=l(j);
end
end
%计算齐次矩阵P
P=[];
for i=1:N-k
B=zeros(k+1,1);
for j=0:k
B(j+1)=basis_funtion(k,j,u(i+j));
end
P=[P B];
end
P=P';
Q=LP;
%求控制点p
p=[px py pz];%三个方向的控制点
p=p';
%c=PQ^(-1)p;%控制点矩阵
c=[];
for i=1:N-k
B=P(i,:);
C=Q(i,:);
a=B/C;
b=p(i,:);
c=[c;ab];
end
c=c';
%反推每一条曲线的控制点
d=[];
for i=1:N-k
C=c(:,i);
a=L(i,:);
b=C';
d=[d;ab];
end
%对每一条曲线分别求解
x=zeros(1000,N-k);%每一条曲线的X坐标
y=zeros(1000,N-k);%每一条曲线的Y坐标
z=zeros(1000,N-k);%每一条曲线的Z坐标
for i=1:N-k
for j=1:1000
t=0.001(j-1);%构造曲线所需的参数
x(j,i)=basis_funtion(k,0,t)*pointInput(i,1)+basis_funtion(k,1,t)*pointInput(i,1)+basis_funtion(k,2,t)*d(i,1)+basis_funtion(k,3,t)*d(i,2);
y(j,i)=basis_funtion(k,0,t)*pointInput(i,2)+basis_funtion(k,1,t)*pointInput(i,2)+basis_funtion(kfor i=1:N
l(i)=sqrt((pointInput(i,1)-pointInput(i+1,1))^2+(pointInput(i,2)-pointInput(i+1,2))^2+(pointInput(i,3)-pointInput(i+1,3))^2);
end
%计算出各节点对应的t，然后求出控制多边形各点的坐标
x=zeros(1,length(u));
y=zeros(1,length(u));
z=zeros(1,length(u));
for i=1:length(u)
t=0;
for j=1:N
if u(i)>=sum(l(1:j))/sum(l(1:N))
t=t+l(j)/sum(l(1:N));
else
t=t+l(j)/sum(l(1:N))*u(i);
break;
end
end
x(i)=0;
y(i)=0;
z(i)=0;
for j=1:N+2
Nj=Nj_basis(j,k,t,u);
x(i)=x(i)+px(j)*Nj;
y(i)=y(i)+py(j)*Nj;
z(i)=z(i)+pz(j)*Nj;
end
end

plot3(x,y,z,'-b')
xlabel('X');
ylabel('Y');
zlabel('Z');
hold on;
scatter3(pointInput(:,1),pointInput(:,2),pointInput(:,3));

function Nj=Nj_basis(j,k,t,u)
%求B样条的基函数
%j：控制点的下标
%k：阶数
%t：当前求的点所对应的t
%u：节点矢量
if k==1
if t>=u(j)&&t<=u(j+1)
Nj=(t-u(j))/(u(j+1)-u(j));
elseif t>=u(j+1)&&t<=u(j+2)
Nj=(u(j+2)-t)/(u(j+2)-u(j+1));
else
Nj=0;
end
else
if (u(j+k)-u(j))==0
N1j=0;
else
N1j=Nj_basis(j,k-1,t,u)*(t-u(j))/(u(j+k)-u(j));
end
if (u(j+k+1)-u(j+1))==0
N2j=0;
else
Nfor i=1:N+1
l(i)=sqrt((px(i+1)-px(i))^2+(py(i+1)-py(i))^2+(pz(i+1)-pz(i))^2);
end
%此时的l为初始的长度，当用完控制多边形的长度后再重新计算长度（即每次新算的长度）
t=zeros(1,N+1+k+2);
t(1)=0;
for i=2:N+1+k+2
t(i)=t(i-1)+l(i-1);
end
%归一化节点矢量
for i=2:N+1+k+2
u(i)=t(i)/t(N+1+k+2);
end
%确定对应的点
fpx=zeros(1,N+1+k+2);
fpy=zeros(1,N+1+k+2);
fpz=zeros(1,N+1+k+2);
for i=1:N+1+k+2
sum=0;
for j=1:N+1
sum=sum+B(j,k,u(i),u)*px(j);
end
fpx(i)=sum;
sum=0;
for j=1:N+1
sum=sum+B(j,k,u(i),u)*py(j);
end
fpy(i)=sum;
sum=0;
for j=1:N+1
sum=sum+B(j,k,u(i),u)*pz(j);
end
fpz(i)=sum;
end
plot3(fpx,fpy,fpz);
end

function result=Chase_method(A,b)
%追赶法，求解线性方程组A*x=b
n=length(b);
result=zeros(n,1);
alpha=zeros(n-1,1);
beta=zeros(n-1,1);
delta=zeros(n-1,1);
%求解alpha
alpha(1)=A(1,1);
for i=2:n-1
alpha(i)=A(i,i)-A(i,i-1)*A(i-1,i)/alpha(i-1);
end
%求解beta
beta(1)=b(1)/alpha(1);
for i=2:n-1
beta(i)=(b(i)-A(i,i-1)*beta(i-1))/alpha(i);
end
%回代求解result
result(n)=b(n)/A(n,n);
fori=n-1:-1:1
result(i)=(b(i)-A(i,i+1)*result(i+1))/alpha(i);
end

%最后，result就是所求的结果，也就是控制点的数组。
%以下是完整代码：

function result = Chase_method(A, b)
n=length(b);
alpha=zeros(1,n);
beta=zeros(1,n);
result=zeros(1,n);
alpha(1)=A(1,1);
%求解alpha
for i=2:n-1
alpha(i)=A(i,i)-A(i,i-1)*A(i-1,i)/alpha(i-1);
end
%求解beta
beta(1)=b(1)/alpha(1);
for i=2:n-1
beta(i)=(b(i)-A(i,i-1)*beta(i-1))/alpha(i);
end
%回代求解result
result(n)=b(n)/A(n,n);
for i=n-1:-1:1
result(i)=(b(i)-A(i,i+1)*result(i+1))/alpha(i);
end
end