空间圆弧插补算法，matlab实现封装函数

%圆弧插补函数，输入三个坐标，以及插补点个数，得到一组坐标
function [trajx,trajy,trajz]=Cir_interpol(N,pos1,pos2,pos3)
x1=pos1(1);y1=pos1(2);z1=pos1(3);
x2=pos2(1);y2=pos2(2);z2=pos2(3);
x3=pos3(1);y3=pos3(2);z3=pos3(3);
A1 = y1z2 - y1z3 -z1y2 + z1y3 + y2z3 - y3z2;
B1 = -x1z2 + x1z3 + z1x2 - z1x3 - x2z3 + x3z2;
C1 = x1y2 - x1y3 - y1x2 + y1x3 + x2y3 - x3y2;
D1 = -x1y2z3 + x1y3z2 + x2y1z3 - x3y1z2 -x2y3z1 + x3y2z1;
% fplane = @(x,y,z) A1x + B1y +C1z +D1; %三点确定的面的方程
A2 = 2(x2-x1);
B2 = 2*(y2-y1);
C2 = 2*(z2-z1);
D2 = x1^2+y12+z1^2 - x2^2-y22-z2^2;
A3 = 2*(x3-x1);
B3 = 2*(y3-y1);
C3 = 2*(z3-z1);
D3 = x1^2+y12+z1^2 - x3^2-y32-z3^2;
T = [A1 B1 C1;A2 B2 C2;A3 B3 C3];
D = [D1 D2 D3]’;
r0 = -inv(T)D ;%圆心坐标向量
R = sqrt((pos1(1)-r0(1))^{2+(pos1(2)-r0(2))}2+(pos1(3)-r0(3))^2);%半径
%step2:得到13两点间的间距d
d13=sqrt((pos1(1)-pos3(1))^{2+(pos1(2)-pos3(2))}2+(pos1(3)-pos3(3))^2);
o13=(pos1+pos3)/2;%得到13两点的中点，当弧角180度时，圆心在该点，以该点为衡量，判定优劣弧
%step3:得到13中点与2点间距do2
do2=sqrt((o13(1)-pos2(1))^{2+(o13(2)-pos2(2))}2+(o13(3)-pos2(3))^2);
%ste4:判断优弧还是劣弧
if(do2<d13/2)
theta=2asin(d13/2/R);
else
theta=2pi-2asin(d13/2/R);
end
%step5:寻找该平面的法向量n，并将其单位化，将n1向量绕其旋转得到ni向量，加上圆心坐标即为圆弧插补点坐标
n1=pos3-r0’;
n2=pos1-r0’;
n=cross(n1,n2);
len=sqrt(n(1)²⁺ⁿ⁽²⁾2+n(3)^2);
n=n/len;%得到单位法向量
u=n(1);
v=n(2);
w=n(3);
%插值个数为30个
xx(N)=0;
yy(N)=0;
zz(N)=0;
%计算每一个插值点坐标,向量绕旋转轴n的旋转矩阵T
for c=1:N
tmp=transT((c-1)/(N-1)*theta,u,v,w)*n2’;
xx©=tmp(1);
yy©=tmp(2);
zz©=tmp(3);
end
trajx(N)=0;
trajy(N)=0;
trajz(N)=0;
for t=1:1:N;
trajx(t)=xx(t)+r0(1);
trajy(t)=yy(t)+r0(2);
trajz(t)=zz(t)+r0(3);
end
end