chazhi5.cpp

实现三维空间点的样条插值算法

//#include   "stdio.h"
#include   "chazhi5.h" 
#include   <iostream>
#include   <vector>
//#include   <windows.h>
//#include "mat.h" 
using namespace std; 
void   tangent(double   *p,double   *dp,int   n)   
{   
    int   i;   
    double   *a,*b,*c,*d;
    //p=new   double[n];
    //dp=new   double[n];	
    a=new   double[n];   
    b=new   double[n];   
    c=new   double[n];   
    d=new   double[n];   
    for(i=0;i<n-2;i++)
	{
	   double pp = p[i+2];
       d[i]=3*(p[i+2]-p[i]);   
       d[n-2]=3*(p[0]-p[n-2]);   
       d[n-1]=3*(p[1]-p[n-1]);   
       a[0]=1;   
       b[0]=0;   
       c[0]=0;   
       a[1]=0;   
       b[1]=1;   
       c[1]=0;
	}
    for(i=0;i<n-2;i++)   
	{   
       a[i+2]=-a[i]-4*a[i+1];   
       b[i+2]=-b[i]-4*b[i+1];   
       c[i+2]=-c[i]-4*c[i+1]+d[i];   
	}   
    double   m1=a[0]+a[n-2]+4*a[n-1];   
    double   m2=b[0]+b[n-2]+4*b[n-1];   
    double   n1=4*a[0]+a[1]+a[n-1];   
    double   n2=4*b[0]+b[1]+b[n-1];   
    double   p1=d[n-2]-c[0]-c[n-2]-4*c[n-1];   
    double   p2=d[n-1]-4*c[0]-c[1]-c[n-1];   
    double   Delt=m1*n2-m2*n1;   
    dp[0]=(p1*n2-p2*m2)/Delt;   
    dp[1]=(m1*p2-n1*p1)/Delt;   
    for(i=0;i<n-2;i++)
	{
        dp[i+2]=d[i]-dp[i]-4*dp[i+1];
	}
}   
    
void   spline(double   *p,double   *dp,double (*pa)[4],int   n)   
{   
    //p=new   double[n];
    //dp=new   double[n];
    //pa=new double[n][4];
	int   i;   
    for(i=0;i<n-1;i++)   
	{   
       pa[i][0]=2*p[i]-2*p[i+1]+dp[i]+dp[i+1];   
       pa[i][1]=-3*p[i]+3*p[i+1]-2*dp[i]-dp[i+1];   
       pa[i][2]=dp[i];   
       pa[i][3]=p[i];   
	}   
    pa[n-1][0]=2*p[n-1]-2*p[0]+dp[n-1]+dp[0];   
    pa[n-1][1]=-3*p[n-1]+3*p[0]-2*dp[n-1]-dp[0];   
    pa[n-1][2]=dp[n-1];   
    pa[n-1][3]=p[n-1];   
}   
    
double   val(double   (*pa)[4],int   i,double   t)   
{   
	//pa=new   double[n][4];   
	double   ft=pa[i][0]*t*t*t+pa[i][1]*t*t+pa[i][2]*t+pa[i][3];   
    return   ft;   
}   
    
int   main(int   argc,   char*   argv[])   
{   
    //=5;   
    //double   x[n]={293.22, 293, 292.29, 289.32, 285.61};
	//double   y[n]={271.44, 270.99, 271.82, 274.52, 277.37};
    //double   z[n]={1, 4.99, 18.03, 30.17, 42.74};
	vector<Point3D> V;
	Point3D P1,P2,P3,P4,P5;
	P1.x = 293.22;
	P1.y = 271.44;
	P1.z = 1;
	V.push_back(P1);
	double g0 = V[0].x;
	//V.clear();
    P2.x = 293;
	P2.y = 270.99;
	P2.z = 4.99;
	V.push_back(P2);
	double g1 = V[1].x;
    P3.x = 292.29;
	P3.y = 271.82;
	P3.z = 18.03;
	V.push_back(P3);
	double g2 = V[2].x;
    P4.x = 289.32;
	P4.y = 274.52;
	P4.z = 30.17;
	V.push_back(P4);
	double g3 = V[3].x;
    P5.x = 285.61;
	P5.y = 277.37;
	P5.z = 42.74;
	V.push_back(P5);
	double g4 = V[4].x;
	int n = V.size ();
	//double x[n],y[n],z[n];
	double *px,*py,*pz;
    px=new   double[n];
    py=new   double[n];
    pz=new   double[n];
	for(int j = 0; j < n; j++)
	{
		px[j] = V[j].x;
		py[j] = V[j].y;
		pz[j] = V[j].z;
	}
		
	
    //double   dx[n],dy[n],dz[n];
	double *dx,*dy,*dz;
    dx=new   double[n];
    dy=new   double[n];
    dz=new   double[n];
    double   s1,s2,s3;
    tangent(px,dx,n);   
    tangent(py,dy,n);
	tangent(pz,dz,n);
    
    //double   ax[n][4],ay[n][4],az[n][4];
	double (*ax)[4],(*ay)[4],(*az)[4];
    ax=new   double[n][4];
    ay=new   double[n][4];
    az=new   double[n][4];
    spline(px,dx,ax,n);   
    spline(py,dy,ay,n);
    spline(pz,dz,az,n);	
    vector<Point3D> V1;
	Point3D P;
	//short array[3][125];
	//short *p;
	//int nSize = 3 * 125;
	//p = new short[nSize];
	//p = &array[0][0];

	
  //FILE   *fp=fopen("d:\\spline.txt","w");   
    for(int   i=0;i<n;i++)
	{
        for(double  t=0;t<=1;t+=0.04)
		{
	       s1 = val(ax,i,t);
           s2 = val(ay,i,t);
		   s3 = val(az,i,t);
		   P.x = s1;
	       P.y = s2;
	       P.z = s3;
		   V1.push_back(P);
		   //*(p + (i*25+(int)(t/0.04))*125) = s1;
		   //*(p + (i*25+(int)(t/0.04))*125 + 1) = s2;
		   //*(p + (i*25+(int)(t/0.04))*125 + 2) = s3;
		   //array[0][(i*25+(int)(t/0.04))] = (short)s1;
		   //array[1][(i*25+(int)(t/0.04))] = (short)s2;
		   //array[2][(i*25+(int)(t/0.04))] = (short)s3;
		   
		}
		
			
	}
	cout<<V1[124].x<<endl;
	cout<<V1[124].y<<endl;
	cout<<V1[124].z<<endl;
}