三次B样条拟合及一阶导、二阶导、曲率计算

1.三次钳位B样条拟合

公式如下图：

展开结果为：
r(u)=[(1-3u+3u2-u3)Pi + (4-6u2+3u3)Pi+1 + (1+3u+3u2-3u3)Pi+2 + (u3)Pi+3 ]/6
其中u取值范围为0到1,将4个控制点的坐标带入上式之后,生成的是Pi+1到Pi+2之间的一段平滑曲线。如果希望某个点必须出现在拟合后的曲线上，则需要让该点在控制点集中连续出现三次。
比如第一个点（0,0）,使P0P1P2都为（0,0）,后续的点正常排列。则使用上述参数化方程可得到通过这一系列点的拟合曲线,且该曲线必定通过第一个点。
拟合结果：

2.参数方程求切线、法线

以上式B样条参数方程为目标曲线，其一阶导和二阶导为：
r’(u)=[(-1+2u-u2)Pi + (-4u+3u2)Pi+1 + (1+2u-3u2)Pi+2 + (u2)Pi+3 ]/2
r’’(u)=[(1-u)Pi + (-2+3u)Pi+1 + (1-3u)Pi+2 + (u)Pi+3 ]
则dy/dx,d2y/dx2为：
f’=y’(u)/x’(u)
f’’=(x’y’’-x’’y’)/(x’)3
f曲率=(x’y’’-x’’y’)/(x’2+y’2)3/2
f’垂线=-1/y’
其中x(u)、y(u)将控制点的x、y坐标带入可得

最开始的代码没有保存，后来算曲率什么的改了很多，比较乱

% function [  ] = B_Spline( x,y )
%三次钳位B样条拟合
close all;
clear all;
clc;
x=[0.5,3,5,6,4,2];
y=[0.5,0,2,4,5,4];
% x=[0,1,2,3,4,5];
% y=[0,1,1,1,1,1];
plot(x,y,'o');
hold on;

x=[x(1),x(1),x(1),x(2:end),x(end),x(end)];%对起始点和结尾点进行重复处理，同一个点需重复出现3次，拟合后的曲线必定经过该点
y=[y(1),y(1),y(1),y(2:end),y(end),y(end)];

u=linspace(0,1,5);%参数u的取值范围为[0,1],5等分即表示会取两个原始点之间的拟合曲线上的5个点。
final_x=x(1);
final_y=y(1);
r1=[];
r=[];
for i=1:(length(x)-3)%通过B样条参数方程计算拟合后曲线的离散点
    px=(1-3.*u+3.*u.^2-u.^3).*x(i) + (4-6.*u.^2+3.*u.^3).*x(i+1) + (1+3.*u+3.*u.^2-3.*u.^3).*x(i+2) + u.^3.*x(i+3);
    py=(1-3.*u+3.*u.^2-u.^3).*y(i) + (4-6.*u.^2+3.*u.^3).*y(i+1) + (1+3.*u+3.*u.^2-3.*u.^3).*y(i+2) + u.^3.*y(i+3);
    px=px/6;
    py=py/6;
    final_x=[final_x,px];
    final_y=[final_y,py];
%     plot(px,py);
%     hold on;
    
    px1=(-1+2.*u-u.^2).*x(i) + (-4.*u+3.*u.^2).*x(i+1) + (1+2.*u-3.*u.^2).*x(i+2) + u.^2.*x(i+3);%B样条参数方程的一阶导
    py1=(-1+2.*u-u.^2).*y(i) + (-4.*u+3.*u.^2).*y(i+1) + (1+2.*u-3.*u.^2).*y(i+2) + u.^2.*y(i+3);
    px1=px1/2;
    py1=py1/2;
    px2=(1-u).*x(i) + (-2+3.*u).*x(i+1) + (1-3.*u).*x(i+2) + (u).*x(i+3);
    py2=(1-u).*y(i) + (-2+3.*u).*y(i+1) + (1-3.*u).*y(i+2) + (u).*y(i+3);
    tmpr1=abs(px1.*py2 + px2.*py1)./(px1.^2 + py1.^2).^1.5
    tmpr=1./(abs(px1.*py2 + px2.*py1)./(px1.^2 + py1.^2).^1.5)
    r1=[r1,tmpr1];
    r=[r,tmpr];
    
%     for j=1:length(px)%求拟合后曲线上每个点的垂线
%         k=-px1(j)/py1(j);0
%         lx=px(j)-0.1:0.1:px(j)+0.1;
%         ly=k.*(lx-px(j))+py(j);
%         plot(lx,ly);
%         hold on;
%     end
end

    plot(final_x,final_y);
    hold on;
    figure(2);
    plot(r);
    hold on;

% for i=1:(length(x))
%     plot(x(i),y(i));
%     hold on;
% end
% end