NURBS

#include <bits/stdc++.h>
using namespace std;
#define DB double
const DB eps=1e-14;
const int NN=305;
DB A[NN][NN],u[NN],v[NN],P[NN][4],Q[NN][4],O[4],N[10];
int n,m,p,K=2;
DB dist(DB a[],DB b[]){    //求两点的欧式距离 
    DB x=0.0;
    for (int k=0;k<K;++k) x+=(a[k]-b[k])*(a[k]-b[k]);
    return sqrt(x);
}
void V0(){    //直接指定输入 
    for (int i=0;i<=n;++i) scanf("%lf",&v[i]);
}
void V1(){    //均匀参数化 
    for (int i=0;i<=n;++i) v[i]=1.0*i/n;
}
void V2(){    //弦长参数化 
    DB d=0;
    for (int i=0;i<n;++i) d+=dist(Q[i],Q[i+1]);
    v[0]=0.0; v[n]=1.0;
    for (int i=1;i<n;++i) v[i]=v[i-1]+dist(Q[i],Q[i-1])/d;
}
void V3(){    //向心参数化 
    DB d=0;
    for (int i=0;i<n;++i) d+=sqrt(dist(Q[i],Q[i+1]));
    v[0]=0.0; v[n]=1.0;
    for (int i=1;i<n;++i) v[i]=v[i-1]+sqrt(dist(Q[i],Q[i-1]))/d;
}
void U0(){    //直接指定输入 
    for (int i=0;i<=m;++i) scanf("%lf",&u[i]);
}
void U1(){    //等距分布 
    for (int i=0;i<=p;++i) u[i]=0.0;
    for (int i=m-p;i<=m;++i) u[i]=1.0;
    for (int i=1;i<=n-p;++i) u[i]=1.0*i/(n-p+1);
}
void U2(){    //对参数取平均法 
    for (int i=0;i<=p;++i) u[i]=0.0;
    for (int i=m-p;i<=m;++i) u[i]=1.0;
    DB x=0.0;
    for (int i=1;i<p;++i) x+=v[i];
    for (int i=1;i<=n-p;++i){
        x+=v[i+p-1];
        u[i+p]=x/p;
        x-=v[i];
    }
}
int Find_i(DB v){    //求参数所在的节点区间[ui,ui+1) 
    int l=p,r=n,m;
    while (l<r){
        m=(l+r)/2;
        if (v<u[m+1]-eps) r=m; else l=m+1;
    }
    return l;
}
void Get_A(){        //线性方程组的系数矩阵(n+1)*(n+1) 
    for (int j=0,i=p;j<=n;++j){
        for (int k=0;k<=n;++k) A[j][k]=0.0;
        while (i<n&&u[i+1]<=v[j]+eps) ++i;
        N[0]=1.0;
        for (int k=1;k<=p;++k){
            N[k]=0.0;
            for (int t=k-1;t>=0;--t){
                N[t]/=u[i-t+p]-u[i-t];
                N[t+1]+=N[t]*(u[i-t+p]-v[j]);
                N[t]*=v[j]-u[i-t];
            }
        }
        for (int k=0;k<=p;++k) A[j][i-k]=N[k];
    }
}
void Get_P(){        //求控制点{Pi} 
    for (int i=0;i<=n;++i)
    for (int k=0;k<K;++k) P[i][k]=Q[i][k];
    DB x;
    for (int i=0,j;i<=n;++i){
        for (j=i;j<=n;++j) if (fabs(A[j][i])>eps) break;
        if (j>n) {puts("det=0!?"); exit(0);}
        if (j!=i){
            for (int k=i;k<=n;++k) swap(A[j][k],A[i][k]);
            for (int k=0;k<K;++k) swap(P[j][k],P[i][k]);
        }
        x=A[j][i];
        for (int k=i;k<=n;++k) A[i][k]/=x;
        for (int k=0;k<K;++k) P[i][k]/=x;
        for (j=i+1;j<=n;++j)
        if (fabs(A[j][i])>eps){
            x=-A[j][i];
            for (int k=i;k<=n;++k) A[j][k]+=x*A[i][k];
            for (int k=0;k<K;++k) P[j][k]+=x*P[i][k];
        }
    }
    for (int i=n;i>=0;--i){
        for (int j=0;j<i;++j)
        if (fabs(A[j][i])>eps){
            x=-A[j][i]; A[j][i]=0.0;
            for (int k=0;k<K;++k) P[j][k]+=x*P[i][k];
        }
    }
}
void Calc_O(DB v,DB O[],int i=-1){    //指定参数，求曲线上的点坐标 
    if (i==-1) i=Find_i(v);
    N[0]=1.0;
    for (int k=1;k<=p;++k){
        N[k]=0.0;
        for (int t=k-1;t>=0;--t){
            N[t]/=u[i-t+k]-u[i-t];
            N[t+1]+=N[t]*(u[i-t+k]-v);
            N[t]*=v-u[i-t];
        }
    }
    for (int k=0;k<K;++k){
        O[k]=0.0;
        for (int t=0;t<=p;++t) O[k]+=P[i-t][k]*N[t];
    }
}
void Print(DB a[]){                    //输出一个点 
    printf("%.3lf",a[0]);
    for (int k=1;k<K;++k) printf(",%.3lf",a[k]);
    printf("\n");
}
void CAD_OUT(){                        //输出CAD曲线(折线) 
    freopen("CAD.txt","w",stdout);
    //for (int i=0;i<=n;++i) printf("point "),Print(Q[i]);
    DB v0=0.0,v1;
    printf("line "); 
    Print(P[0]);
    for (int t=1,i=p;;++t){
        v1=v0+1.0/1000;
        if (v1>1.0-eps) break;
        while (i<n&&u[i+1]<=v1+eps) ++i;
        Calc_O(v1,O,i);
        Print(O);
        v0=v1;
    }
    Print(P[n]);
}
int main(){    
    //freopen("2DSlope.txt","r",stdin);
    printf("输入NURBS曲线的次数(一般为3): ");
    scanf("%d",&p);
    printf("输入数据点: ");
    scanf("%d",&n); --n; m=n+p+1;
    for (int i=0;i<=n;++i)
    for (int k=0;k<K;++k) scanf("%lf",&Q[i][k]);
    V2();
    U2(); 
    Get_A();
    Get_P();
    CAD_OUT();
    return 0;
}