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));
[email protected](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');