Bezier-Blending C2-continuous

clc;clear;close;

r=0.5;

q=[
0 1 2 4 5 6;
0 2 3 3 2 0;
0 1 0 0 2 2];

qb=[
0.79 1.28 1.71 2.5 3.5 4.2  4.79 5.22;
1.59 2.28 2.71 3   3   2.79 2.20 1.55;
0.79 0.71 0.28 0   0   0.40 1.59 2];

qx=q(1,:);
qy=q(2,:);
qz=q(3,:);

t=[0.4 0.81 0.4;
   0.57 0.57 -0.57;
   0.57 0.57 -0.57;
   1 0 0;
   1 0 0;
   0.4 -0.4 0.81;
   0.4 -0.4 0.81;
   0.44 -0.89 0];

jj=0;
for k=1:4
     a=256-49*(t(1+jj,:)+t(2+jj,:))*(t(1+jj,:)+t(2+jj,:))';
     b=420*(qb(:,2+jj)-qb(:,1+jj))'*(t(1+jj,:)+t(2+jj,:))';
     c=-900*(qb(:,2+jj)-qb(:,1+jj))'*(qb(:,2+jj)-qb(:,1+jj));
     
     fun=@(Q)(a*Q^2+b*Q+c);
     qp(k)=fsolve(fun,0.1);
jj=jj+2;
end
jj=0;
kk=0;
p=zeros(12,6)
for k=1:4
   p(1+kk:3+kk,:)=[qb(:,jj+1), qb(:,jj+1)+(qp(k)/5)*t(1+jj,:)' ,2*(qb(:,jj+1)+(qp(k)/5)*t(1+jj,:)')-qb(:,jj+1),2*(qb(:,jj+2)-(qp(k)/5)*t(2+jj,:)')-qb(:,jj+2) ,qb(:,jj+2)-(qp(k)/5)*t(2+jj,:)'  ,qb(:,jj+2)] 
   jj=jj+2;
   kk=kk+3;
end

pt=0;

for i=1:3
 P=p(1+pt:3+pt,:);
 pp=p(4+pt:6+pt,:);
 pp(:,2)=pp(:,1)+(P(:,6)-P(:,5));
 pp(:,3)=2*pp(:,2)+(P(:,4)-2*P(:,5))-pp(:,1)+P(:,6);
 p(4+pt:6+pt,:)=pp;
 pt=pt+3;
end

m=5;
pt=0;
jj=1;
for k=1:4
for ut=0:0.001:1
%for ut=uk(k):0.01:uk(k+1);
    P=p(1+pt:3+pt,:);
    b=BezierP(P,m,ut);
    db=BezierPDerivatives(P,m,ut);
    ddb=BezierPDerivatives2(P,m,ut);
    bx(jj)=b(1,1);
    by(jj)=b(2,1);
    bz(jj)=b(3,1);
    dbx(jj)=db(1,1);
    dby(jj)=db(2,1);
    dbz(jj)=db(3,1);
    ddbx(jj)=ddb(1,1);
    ddby(jj)=ddb(2,1);
    ddbz(jj)=ddb(3,1);
    jj=jj+1;
end
  pt=pt+3;
end

plot3(bx,by,bz,'r');
hold on;
plot3(qx,qy,qz);
plot3(qx,qy,qz,'o');
xlabel('X轴');       
ylabel('Y轴');        
zlabel('Z轴'); 
figure('name','dbx');
plot(1:jj-1,dbx,'b');
figure('name','dby');
plot(1:jj-1,dby,'b');
figure('name','dbz');
plot(1:jj-1,dbz,'b');

figure('name','ddbx');
plot(1:jj-1,ddbx,'b');
figure('name','ddby');
plot(1:jj-1,ddby,'b');
figure('name','ddbz');
plot(1:jj-1,ddbz,'b');