Bezier-C1 continuous

clc;clear;close;

p=[0 0.18 0.49 1;
0 0.58 1.32 2;
0 0.59 1.26 1;
1 1.35 1.47 2;
2 2.47 2.79 3;
1 0.88 0.20 0;
2 2.60 3.39 4;
3 3.24 3.15 3;
0 -0.24 -0.30 0;
4 4.76 4.57 5;
3 2.80 2.66 2;
0 0.38 1.61 2;
5 5.37 5.72 6;
2 1.42 0.67 0;
2 2.33 2.23 2];

p=[0 0.18 0.5023 1;
0 0.5750 1.3306 2;
0 0.6007 1.1630 1;

1 1.3503 1.4745 2;
2 2.4711 2.7946 3;
1 0.8852 0.2054 0;

2 2.60 3.39 4;
3 3.24 3.15 3;
0 -0.24 -0.30 0;

4 4.76 4.57 5;
3 2.80 2.66 2;
0 0.38 1.61 2;

5 5.37 5.72 6;
2 1.42 0.67 0;
2 2.33 2.23 2];

p0=p(:,1);
p1=p(:,2);
p2=p(:,3);
p3=p(:,4);

t=[0.21 0.58 0.87 0.87 0.48 0.36
0.67 0.78 0.34 -0.21 -0.75 -0.88
0.70 -0.19 -0.34 0.43 0.44 -0.30];

tx=t(1,:);
ty=t(2,:);
tz=t(3,:);
pt=0;
uk=zeros(1,size(t,2));

for i=1:size(t,2)-1
    P=p(1+pt:3+pt,:);
    ap {i}= 16 - (t(:,i) + t(:,i+1))'*(t(:,i) + t(:,i+1));
    bp {i}= 12*(P(:,4) - P(:,1))' * (t(:,i) + t(:,i+1));
    cp {i}= -36*(P(:,4) - P(:,1))'*(P(:,4) - P(:,1))
    pt=pt+3;
    
     fun=@(Q)(ap{i}*Q^2+bp{i}*Q+cp{i});
     qp{i}=fsolve(fun,0.1);
     
     pp1{i}=P(:,1)+(1/3)*qp{i}*t(:,i);
     pp2{i}=P(:,4)-(1/3)*qp{i}*t(:,i+1);
     
     ck(i) = 3*norm((P(:,2)-P(:,1)));
     
    
end
pt=0;

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

for i=1:size(t,2)-1
    uk(i+1)=uk(i)+ck(i);
end

uk=uk/uk(size(uk,2));

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,:);

m=3;
u=[0, 2.5, 4.3, 6.4, 9.1, 11.3]/11.3;
%u=[0, 0.22, 0.38, 0.56, 0.79, 1];
jj=1;
pt=0;
for k=1:size(u,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);
    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);
    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');