%% 算法开始,输入两坐标点
p0=[-191.4037,11.3508,7.6182];u0=[0.011639,-0.036377,0.99927];
p1=[-191.4057,13.5159,7.9093];u1=[0.006393,-0.068922,0.99960];
u=[u0;u1];L=410;T=0.2;v=1.0923;Q=p1-p0;
%% 插值算出中间密化点
pa=zeros(11,3);pa(1,:)=p0;pa(11,:)=p1;
for i=1:9
pa(1,:)=p0+Q*i*v*T/norm(Q);
end
disp(pa);
C=zeros(11,1);A=zeros(11,1);pa1=zeros(11,3);
%% 算出首末两点后置处理点
mx=-25;my=-15;mz=-25;
C(1,1)=acos(u(1,3)/L);
A(1,1)=atan(u(1,1)/u(1,2));
X=cos(C(1,:)*pi/180)*(pa(1,3)-mx)-sin(C(1,:)*pi/180)*(pa(1,2)-my)-my;
Y=sin(C(1,:)*pi/180)*cos(A(1,1)*pi/180)*(pa(1,3)-mx)+cos(A(1,1)*pi/180)*cos(C(1,:)*pi/180)*(pa(1,2)-my)-sin(A(1,1)*pi/180)*(pa(1,3)-mz)-my;
Z=sin(A(1,1)*pi/180)*sin(C(1,:)*pi/180)*(pa(1,3)-mx)+sin(A(1,1)*pi/180)*cos(C(1,:)*pi/180)*(pa(1,2)-my)+cos(A(1,1)*pi/180)*(pa(1,3)-mz)-mz;
pa1(1,:)=[X,Y,Z];
X=cos(C(2,:)*pi/180)*(pa(1,3)-mx)-sin(C(2,:)*pi/180)*(pa(1,2)-my)-my;
Y=sin(C(2,:)*pi/180)*cos(A(2,1)*pi/180)*(pa(1,3)-mx)+cos(A(2,1)*pi/180)*cos(C(2,:)*pi/180)*(pa(1,2)-my)-sin(A(2,1)*pi/180)*(pa(1,3)-mz)-my;
Z=sin(A(2,1)*pi/180)*sin(C(2,:)*pi/180)*(pa(1,3)-mx)+sin(A(2,1)*pi/180)*cos(C(2,:)*pi/180)*(pa(1,2)-my)+cos(A(2,1)*pi/180)*(pa(1,3)-mz)-mz;
pa1(11,:)=[X,Y,Z]; C(11,:)=acos(u(2,3)/L); A(11,:)=atan(u(2,1)/u(2,2));
disp([A,C])
%% 算出所有后置处理点
B1=linspace(A(1,1),A(11,1),10)'; B2=linspace(C(1,1),C(11,1),10)';
for i=1:9
A(i+1,1)=B1(i,1);C(i+1,1)=B2(i,1);
X=cos(C(1,:)*pi/180)*(pa(1,3)-mx)-sin(C(1,:)*pi/180)*(pa(1,2)-my)-my;
Y=sin(C(1,:)*pi/180)*cos(A(1,1)*pi/180)*(pa(1,3)-mx)+cos(A(1,1)*pi/180)*cos(C(1,:)*pi/180)*(pa(1,2)-my)-sin(A(1,1)*pi/180)*(pa(1,3)-mz)-my;
Z=sin(A(1,1)*pi/180)*sin(C(1,:)*pi/180)*(pa(1,3)-mx)+sin(A(1,1)*pi/180)*cos(C(1,:)*pi/180)*(pa(1,2)-my)+cos(A(1,1)*pi/180)*(pa(1,3)-mz)-mz;
pa1(i+1,:)=[X,Y,Z];
end
x=pa1(:,1)';y=pa1(:,2)';z=pa1(:,3)'; x1=[pa1(1,1),pa1(11,1)]; y1=[pa1(1,2),pa1(11,2)];z1=[pa1(1,3),pa1(11,3)];
plot3(x,y,z,'p')
disp([pa1 A C])