Bezier-C2 continuous

clc;clear;close;

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

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

p=[
0 0.20 0.40 0.51 0.75 1;
0 0.41 0.84 1.11 1.56 2;
0 0.20 0.57 1.03 1.10 1;

1 1.17 1.34 1.47 1.71 2;
2 2.30 2.60 2.75 2.89 3;
1 0.92 0.77 0.42 0.18 0;

2 2.34 2.76 3.24 3.66 4;
3 3.12 3.18 3.15 3.08 3;
0 -0.22 -0.37 -0.42 -0.25 0;

4 4.39 4.68 4.65 4.80 5;
3 2.89 2.77 2.74 2.40 2;
0 0.30 0.74 1.34 1.74 2;

5 5.17 5.37 5.56 5.79 6;
2 1.64 1.23 0.86 0.40 0;
2 2.22 2.32 2.12 2.00 2]

pt=0;

for i=1:size(q,2)-2
 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));
 p(4+pt:6+pt,:)=pp;
 pt=pt+3;
 
end

m=5;
pt=0;
jj=1;
for k=1:size(q,2)-1
for ut=0:0.01: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');