polyfit.txt

任意阶的多项式曲线拟合方程

#define TINY 1.0e-20
 
/* Given a matrix a[1..n][1..n], htis routine replaces it by the LU decmposition of a rwowise permutation of itself.
a and n are input. a is output, arranged as follows. index[1..n] is an output vector that records the row permutation
effected by the partial pivoting; d is outpu as +/-1 depending on whether the number of row interchanges was even or odd, 
respectively. This routine is used in combination iwth lubksb to sovle linear equations or invert a matrix
 |1   0   0   0|  |c11 c12 c13 c14| |a11 a12 a13 a14|            |c11 c12 c13 c14|
 |b21 1   0   0| *|0   c22 c23 c24|=|a21 a22 a23 a24|     output=|b21 c22 c23 c24|
 |b31 b32 1   0|  |0   0   c33 c34| |a31 a32 a33 a34|            |b31 b32 c33 c34| 
 |b41 b42 b43 1|  |0   0   0   c44| |a41 a42 a43 a44|            |b41 b42 b43 c44|
*/
int ludcmp(double **a, int n, int *index, float *d)
{
 int i, imax, j, k;
 double big, dum, sum, temp;
 double *vv;   //vv storing the implicit scaling of each row
 
 vv=(double *)calloc(n+1, sizeof(double));
 *d=1.0;
 for (i=1; i<=n; i++){
  big=0.0;
  for(j=1; j<=n; j++)
   if((temp=fabs(a[i][j]))>big) big=temp;
  if(big==0.0) 
   return -1;
  vv[i]=double(1.0)/big;   //save the scaling
 }
 for(j=1; j<=n; j++){
  for(i=1; i<j; i++){
   sum=a[i][j];
   for(k=1; k<i; k++) sum-=a[i][k]*a[k][j];
   a[i][j]=sum;
  }
  big=0.0;
  for(i=j; i<=n; i++){
   sum=a[i][j];
   for(k=1; k<j; k++)
    sum-= a[i][k]*a[k][j];
   a[i][j]=sum;
   if((dum=vv[i]*fabs(sum))>=big){
    //is the figure of merit ofr the pivot better than the best so far?
    big=dum;
    imax=i;
   }
  }
  if(j!=imax){ //do we need to interchange rows?
   for(k=1; k<=n; k++){  //yse, do so...
    dum=a[imax][k];
    a[imax][k]=a[j][k];
    a[j][k]=dum;
   }
   *d = -(*d);
   vv[imax]=vv[j];
  }
  index[j]=imax;
  if(a[j][j]==0.0) a[j][j]=double(TINY);
  //if the pivot element is zero the matrix is singular(at least to the precision of the algorithm)
  //for some applications on singular matrices, it is desirable to substitue TINY for zero
  if(j!=n) {  //now, finally,divide by the pivot element
   dum=double(1.0)/a[j][j];
   for (i=j+1; i<=n; i++) a[i][j]*=dum;
  }
 }
 free(vv);
 
 return 0;
}
    
/* This function solves the set of n linear equations A.X=B. Here a[1..n][1..n] is input, not as 
the matrix A but rather as its LU decomposition, determined by the routine ludcmp. index[1..n] is 
input as the permutation vector returned by ludcmp. b[1..n] is input as the right-hand side vector
B, and returns with the solution vector X. a, n and index are not modified by this routine and can
be left in place for successive calls iwth different right-hand sides b. This routine takes into 
account the possibility that b will begin with many zero elements so it it efficient for use in 
matrix inversion.
  |a11 a12 a13| |x1|   |b1|
  |a21 a22 a23|*|x2| = |b2|   
  |a31 a32 a33| |x3|   |b3|
*/
int lubksb(double **a, int n, int *index, double b[])
{
 int i, ii=0, ip, j;
 double sum;
 
 for(i=1; i<=n; i++){   //when ii is set to a positie value, it will become the index 
  ip=index[i];       // of the first nonvanishing element of b. We now do the forward
  sum=b[ip];        //substitution. The only new wrinkle is to unscrambel the permutation
  b[ip]=b[i];       //as we go.
  if (ii)
   for(j=ii; j<=i-1; j++) sum-= a[i][j]*b[j];
  else if (sum) ii=i;   //a nonzero element was encountered, so from now on we will have to
  b[i]=sum;            //do the sums in the loop above.
 }
 for(i=n; i>=1; i--){  //Now we do the backsubstitution
  sum=b[i];
  for(j=i+1; j<=n;j++) sum-= a[i][j]*b[j];
  b[i]=sum/a[i][i];  //store a component of the solution vector X
 }                      //All done
 return 0;
}
/* Fit y=a0+a1*x+a2*power(x,2)+a3*power(x,3)+...am*power(x,m);
  B*A=C
     | b[0]  b[1]   b[2]   ... b[m]   |
 B=  | b[1]  b[2]   b[3]   ... b[m+1] |
       ...
     | b[m]  b[m+1] b[m+2] ... b[2*m] |
 A=  [a[0], a[1], a[2], ..., a[m]]
 b[k]=sum( power(Xi,k))
 c[k]=sum(Yi*power(Xi,k))
*/
__declspec(dllexport) void _stdcall FitPolynomialCurve(TFPoint arrPts[], int nData, float a[], int m)
{
 int i, k,row,col;
 double *b=new double[m+m+1];
 memset(b,0,(m+m+1)*sizeof(double));
 double **bb=new double*[m+2];
 for(i=0; i<m+2; i++){
  bb[i]=new double[m+2];
  memset(bb[i],0,(m+2)*sizeof(double));
 }
 double *c=new double[m+2];
 memset(c,0,(m+2)*sizeof(double));
 
 double value;
 for(i=0; i<nData; i++){    
  for(k=0, value=1; k<=m+m; k++){
   b[k]+=value;
   if(k<=m) c[k+1]+=value*arrPts[i].y;
   value*=arrPts[i].x;
  }
 }
 for(row=0; row<=m; row++)
  for(col=0;col<=m; col++)
   bb[row+1][col+1]=b[row+col];
 
 int *index=new int[m+2];
 float d;
 ludcmp(bb,m+1,index,&d);
 lubksb(bb,m+1,index,c);
 for(i=0; i<m+1; i++)
  a[i]=c[i+1];
 
 delete [] b;
 delete [] index;
 for(i=0; i<m+2; i++)
  delete [] bb[i];
 delete [] bb;
 delete [] c;
 
 return ;
}