c@241: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
c@241: //---------------------------------------------------------------------------
c@241: 
c@241: #ifndef PolyfitHPP
c@241: #define PolyfitHPP
c@241: //---------------------------------------------------------------------------
c@241: // Least-squares curve fitting class for arbitrary data types
c@241: /*
c@241: 
c@241: { ******************************************
c@241:   ****   Scientific Subroutine Library  ****
c@241:   ****         for C++ Builder          ****
c@241:   ******************************************
c@241: 
c@241:   The following programs were written by Allen Miller and appear in the
c@241:   book entitled "Pascal Programs For Scientists And Engineers" which is
c@241:   published by Sybex, 1981.
c@241:   They were originally typed and submitted to MTPUG in Oct. 1982
c@241:     Juergen Loewner
c@241:     Hoher Heckenweg 3
c@241:     D-4400 Muenster
c@241:   They have had minor corrections and adaptations for Turbo Pascal by
c@241:     Jeff Weiss
c@241:     1572 Peacock Ave.
c@241:     Sunnyvale, CA 94087.
c@241: 
c@241: 
c@241: 2000 Oct 28  Updated for Delphi 4, open array parameters.
c@241:              This allows the routine to be generalised so that it is no longer
c@241:              hard-coded to make a specific order of best fit or work with a
c@241:              specific number of points.
c@241: 2001 Jan 07  Update Web site address
c@241: 
c@241: Copyright © David J Taylor, Edinburgh and others listed above
c@241: Web site:  www.satsignal.net
c@241: E-mail:    davidtaylor@writeme.com
c@241: }*/
c@241: 
c@241:  ///////////////////////////////////////////////////////////////////////////////
c@241:  // Modified by CLandone for VC6 Aug 2004
c@241:  ///////////////////////////////////////////////////////////////////////////////
c@241: 
c@268: #include <iostream>
c@241: 
c@241: using std::vector;
c@241: 
c@241: class TPolyFit
c@241: {
c@241:     typedef vector<vector<double> > Matrix;
c@241: public:
c@241: 
c@241:     static double PolyFit2 (const vector<double> &x,  // does the work
cannam@477:                             const vector<double> &y,
cannam@477:                             vector<double> &coef);
c@241: 
c@241:                    
c@241: private:
c@241:     TPolyFit &operator = (const TPolyFit &);   // disable assignment
c@241:     TPolyFit();                                // and instantiation
c@241:     TPolyFit(const TPolyFit&);                 // and copying
c@241: 
c@241:   
c@241:     static void Square (const Matrix &x,              // Matrix multiplication routine
cannam@477:                         const vector<double> &y,
cannam@477:                         Matrix &a,                    // A = transpose X times X
cannam@477:                         vector<double> &g,         // G = Y times X
cannam@477:                         const int nrow, const int ncol);
c@241:     // Forms square coefficient matrix
c@241: 
c@241:     static bool GaussJordan (Matrix &b,                  // square matrix of coefficients
cannam@477:                              const vector<double> &y, // constant vector
cannam@477:                              vector<double> &coef);   // solution vector
c@241:     // returns false if matrix singular
c@241: 
c@241:     static bool GaussJordan2(Matrix &b,
cannam@477:                              const vector<double> &y,
cannam@477:                              Matrix &w,
cannam@477:                              vector<vector<int> > &index);
c@241: };
c@241: 
c@241: // some utility functions
c@241: 
c@421: struct NSUtility
c@241: {
c@421:     static void swap(double &a, double &b) {double t = a; a = b; b = t;}
c@421:     // fills a vector with zeros.
c@421:     static void zeroise(vector<double> &array, int n) { 
c@421:         array.clear();
c@421:         for(int j = 0; j < n; ++j) array.push_back(0);
c@421:     }
c@421:     // fills a vector with zeros.
c@421:     static void zeroise(vector<int> &array, int n) {
c@421:         array.clear();
c@421:         for(int j = 0; j < n; ++j) array.push_back(0);
c@421:     }
c@421:     // fills a (m by n) matrix with zeros.
c@421:     static void zeroise(vector<vector<double> > &matrix, int m, int n) {
c@421:         vector<double> zero;
c@421:         zeroise(zero, n);
c@421:         matrix.clear();
c@421:         for(int j = 0; j < m; ++j) matrix.push_back(zero);
c@421:     }
c@421:     // fills a (m by n) matrix with zeros.
c@421:     static void zeroise(vector<vector<int> > &matrix, int m, int n) {
c@421:         vector<int> zero;
c@421:         zeroise(zero, n);
c@421:         matrix.clear();
c@421:         for(int j = 0; j < m; ++j) matrix.push_back(zero);
c@421:     }
c@421:     static double sqr(const double &x) {return x * x;}
c@241: };
c@241: 
c@241: 
c@241: // main PolyFit routine
c@241: 
c@241: double TPolyFit::PolyFit2 (const vector<double> &x,
cannam@477:                            const vector<double> &y,
cannam@477:                            vector<double> &coefs)
c@241: // nterms = coefs.size()
c@241: // npoints = x.size()
c@241: {
c@241:     int i, j;
c@241:     double xi, yi, yc, srs, sum_y, sum_y2;
c@241:     Matrix xmatr;        // Data matrix
c@241:     Matrix a;
c@241:     vector<double> g;      // Constant vector
c@241:     const int npoints(x.size());
c@241:     const int nterms(coefs.size());
c@241:     double correl_coef;
c@421:     NSUtility::zeroise(g, nterms);
c@421:     NSUtility::zeroise(a, nterms, nterms);
c@421:     NSUtility::zeroise(xmatr, npoints, nterms);
c@268:     if (nterms < 1) {
c@268:         std::cerr << "ERROR: PolyFit called with less than one term" << std::endl;
c@268:         return 0;
c@268:     }
c@268:     if(npoints < 2) {
c@268:         std::cerr << "ERROR: PolyFit called with less than two points" << std::endl;
c@268:         return 0;
c@268:     }
c@325:     if(npoints != (int)y.size()) {
c@268:         std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl;
c@268:         return 0;
c@268:     }
c@241:     for(i = 0; i < npoints; ++i)
c@241:     {
cannam@477:         //      { setup x matrix }
cannam@477:         xi = x[i];
cannam@477:         xmatr[i][0] = 1.0;         //     { first column }
cannam@477:         for(j = 1; j < nterms; ++j)
cannam@477:             xmatr[i][j] = xmatr [i][j - 1] * xi;
c@241:     }
c@241:     Square (xmatr, y, a, g, npoints, nterms);
c@241:     if(!GaussJordan (a, g, coefs))
cannam@477:         return -1;
c@241:     sum_y = 0.0;
c@241:     sum_y2 = 0.0;
c@241:     srs = 0.0;
c@241:     for(i = 0; i < npoints; ++i)
c@241:     {
cannam@477:         yi = y[i];
cannam@477:         yc = 0.0;
cannam@477:         for(j = 0; j < nterms; ++j)
cannam@477:             yc += coefs [j] * xmatr [i][j];
cannam@477:         srs += NSUtility::sqr (yc - yi);
cannam@477:         sum_y += yi;
cannam@477:         sum_y2 += yi * yi;
c@241:     }
c@241: 
c@241:     // If all Y values are the same, avoid dividing by zero
c@421:     correl_coef = sum_y2 - NSUtility::sqr (sum_y) / npoints;
c@241:     // Either return 0 or the correct value of correlation coefficient
c@241:     if (correl_coef != 0)
cannam@477:         correl_coef = srs / correl_coef;
c@241:     if (correl_coef >= 1)
cannam@477:         correl_coef = 0.0;
c@241:     else
cannam@477:         correl_coef = sqrt (1.0 - correl_coef);
c@241:     return correl_coef;
c@241: }
c@241: 
c@241: 
c@241: //------------------------------------------------------------------------
c@241: 
c@241: // Matrix multiplication routine
c@241: // A = transpose X times X
c@241: // G = Y times X
c@241: 
c@241: // Form square coefficient matrix
c@241: 
c@241: void TPolyFit::Square (const Matrix &x,
cannam@477:                        const vector<double> &y,
cannam@477:                        Matrix &a,
cannam@477:                        vector<double> &g,
cannam@477:                        const int nrow,
cannam@477:                        const int ncol)
c@241: {
c@241:     int i, k, l;
c@241:     for(k = 0; k < ncol; ++k)
c@241:     {
cannam@477:         for(l = 0; l < k + 1; ++l)
cannam@477:         {
cannam@477:             a [k][l] = 0.0;
cannam@477:             for(i = 0; i < nrow; ++i)
cannam@477:             {
cannam@477:                 a[k][l] += x[i][l] * x [i][k];
cannam@477:                 if(k != l)
cannam@477:                     a[l][k] = a[k][l];
cannam@477:             }
cannam@477:         }
cannam@477:         g[k] = 0.0;
cannam@477:         for(i = 0; i < nrow; ++i)
cannam@477:             g[k] += y[i] * x[i][k];
c@241:     }
c@241: }
c@241: //---------------------------------------------------------------------------------
c@241: 
c@241: 
c@241: bool TPolyFit::GaussJordan (Matrix &b,
cannam@477:                             const vector<double> &y,
cannam@477:                             vector<double> &coef)
c@241: //b square matrix of coefficients
c@241: //y constant vector
c@241: //coef solution vector
c@241: //ncol order of matrix got from b.size()
c@241: 
c@241: 
c@241: {
c@241: /*
c@241:   { Gauss Jordan matrix inversion and solution }
c@241: 
c@241:   { B (n, n) coefficient matrix becomes inverse }
c@241:   { Y (n) original constant vector }
c@241:   { W (n, m) constant vector(s) become solution vector }
c@241:   { DETERM is the determinant }
c@241:   { ERROR = 1 if singular }
c@241:   { INDEX (n, 3) }
c@241:   { NV is number of constant vectors }
c@241: */
c@241: 
c@241:     int ncol(b.size());
c@241:     int irow, icol;
c@241:     vector<vector<int> >index;
c@241:     Matrix w;
c@241: 
c@421:     NSUtility::zeroise(w, ncol, ncol);
c@421:     NSUtility::zeroise(index, ncol, 3);
c@241: 
c@241:     if(!GaussJordan2(b, y, w, index))
cannam@477:         return false;
c@241: 
c@241:     // Interchange columns
c@241:     int m;
c@241:     for (int i = 0; i <  ncol; ++i)
c@241:     {
cannam@477:         m = ncol - i - 1;
cannam@477:         if(index [m][0] != index [m][1])
cannam@477:         {
cannam@477:             irow = index [m][0];
cannam@477:             icol = index [m][1];
cannam@477:             for(int k = 0; k < ncol; ++k)
cannam@477:                 swap (b[k][irow], b[k][icol]);
cannam@477:         }
c@241:     }
c@241: 
c@241:     for(int k = 0; k < ncol; ++k)
c@241:     {
cannam@477:         if(index [k][2] != 0)
cannam@477:         {
c@268:             std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl;
c@268:             return false;
cannam@477:         }
c@241:     }
c@241: 
c@241:     for( int i = 0; i < ncol; ++i)
cannam@477:         coef[i] = w[i][0];
c@241:  
c@241:  
c@241:     return true;
cannam@477: }   // end;     { procedure GaussJordan }
c@241: //----------------------------------------------------------------------------------------------
c@241: 
c@241: 
c@241: bool TPolyFit::GaussJordan2(Matrix &b,
cannam@477:                             const vector<double> &y,
cannam@477:                             Matrix &w,
cannam@477:                             vector<vector<int> > &index)
c@241: {
c@241:     //GaussJordan2;         // first half of GaussJordan
c@241:     // actual start of gaussj
c@241:  
c@241:     double big, t;
c@241:     double pivot;
c@241:     double determ;
c@429:     int irow = 0, icol = 0;
c@241:     int ncol(b.size());
c@241:     int nv = 1;                  // single constant vector
c@241:     for(int i = 0; i < ncol; ++i)
c@241:     {
cannam@477:         w[i][0] = y[i];      // copy constant vector
cannam@477:         index[i][2] = -1;
c@241:     }
c@241:     determ = 1.0;
c@241:     for(int i = 0; i < ncol; ++i)
c@241:     {
cannam@477:         // Search for largest element
cannam@477:         big = 0.0;
cannam@477:         for(int j = 0; j < ncol; ++j)
cannam@477:         {
cannam@477:             if(index[j][2] != 0)
cannam@477:             {
cannam@477:                 for(int k = 0; k < ncol; ++k)
cannam@477:                 {
cannam@477:                     if(index[k][2] > 0) {
c@268:                         std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
c@268:                         return false;
c@268:                     }
c@241: 
cannam@477:                     if(index[k][2] < 0 && fabs(b[j][k]) > big)
cannam@477:                     {
cannam@477:                         irow = j;
cannam@477:                         icol = k;
cannam@477:                         big = fabs(b[j][k]);
cannam@477:                     }
cannam@477:                 } //   { k-loop }
cannam@477:             }
cannam@477:         }  // { j-loop }
cannam@477:         index [icol][2] = index [icol][2] + 1;
cannam@477:         index [i][0] = irow;
cannam@477:         index [i][1] = icol;
c@241: 
cannam@477:         // Interchange rows to put pivot on diagonal
cannam@477:         // GJ3
cannam@477:         if(irow != icol)
cannam@477:         {
cannam@477:             determ = -determ;
cannam@477:             for(int m = 0; m < ncol; ++m)
cannam@477:                 swap (b [irow][m], b[icol][m]);
cannam@477:             if (nv > 0)
cannam@477:                 for (int m = 0; m < nv; ++m)
cannam@477:                     swap (w[irow][m], w[icol][m]);
cannam@477:         } // end GJ3
c@241: 
cannam@477:         // divide pivot row by pivot column
cannam@477:         pivot = b[icol][icol];
cannam@477:         determ *= pivot;
cannam@477:         b[icol][icol] = 1.0;
c@241: 
cannam@477:         for(int m = 0; m < ncol; ++m)
cannam@477:             b[icol][m] /= pivot;
cannam@477:         if(nv > 0)
cannam@477:             for(int m = 0; m < nv; ++m)
cannam@477:                 w[icol][m] /= pivot;
c@241: 
cannam@477:         // Reduce nonpivot rows
cannam@477:         for(int n = 0; n < ncol; ++n)
cannam@477:         {
cannam@477:             if(n != icol)
cannam@477:             {
cannam@477:                 t = b[n][icol];
cannam@477:                 b[n][icol] = 0.0;
cannam@477:                 for(int m = 0; m < ncol; ++m)
cannam@477:                     b[n][m] -= b[icol][m] * t;
cannam@477:                 if(nv > 0)
cannam@477:                     for(int m = 0; m < nv; ++m)
cannam@477:                         w[n][m] -= w[icol][m] * t;
cannam@477:             }
cannam@477:         }
c@241:     } // { i-loop }
c@241:     return true;
c@241: }
c@241: 
c@241: #endif
c@241: