libs/qm-dsp/maths/Polyfit.h

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2 //---------------------------------------------------------------------------
   3
   4 #ifndef PolyfitHPP
   5 #define PolyfitHPP
   6 //---------------------------------------------------------------------------
   7 // Least-squares curve fitting class for arbitrary data types
   8 /*
   9
  10 { ******************************************
  11   ****   Scientific Subroutine Library  ****
  12   ****         for C++ Builder          ****
  13   ******************************************
  14
  15   The following programs were written by Allen Miller and appear in the
  16   book entitled "Pascal Programs For Scientists And Engineers" which is
  17   published by Sybex, 1981.
  18   They were originally typed and submitted to MTPUG in Oct. 1982
  19     Juergen Loewner
  20     Hoher Heckenweg 3
  21     D-4400 Muenster
  22   They have had minor corrections and adaptations for Turbo Pascal by
  23     Jeff Weiss
  24     1572 Peacock Ave.
  25     Sunnyvale, CA 94087.
  26
  27
  28 2000 Oct 28  Updated for Delphi 4, open array parameters.
  29              This allows the routine to be generalised so that it is no longer
  30              hard-coded to make a specific order of best fit or work with a
  31              specific number of points.
  32 2001 Jan 07  Update Web site address
  33
  34 Copyright � David J Taylor, Edinburgh and others listed above
  35 Web site:  www.satsignal.net
  36 E-mail:    davidtaylor@writeme.com
  37 }*/
  38
  39  ///////////////////////////////////////////////////////////////////////////////
  40  // Modified by CLandone for VC6 Aug 2004
  41  ///////////////////////////////////////////////////////////////////////////////
  42
  43 #include <iostream>
  44
  45 using std::vector;
  46
  47 class TPolyFit
  48 {
  49     typedef vector<vector<double> > Matrix;
  50 public:
  51
  52     static double PolyFit2 (const vector<double> &x,  // does the work
  53                             const vector<double> &y,
  54                             vector<double> &coef);
  55
  56
  57 private:
  58     TPolyFit &operator = (const TPolyFit &);   // disable assignment
  59     TPolyFit();                                // and instantiation
  60     TPolyFit(const TPolyFit&);                 // and copying
  61
  62
  63     static void Square (const Matrix &x,              // Matrix multiplication routine
  64                         const vector<double> &y,
  65                         Matrix &a,                    // A = transpose X times X
  66                         vector<double> &g,         // G = Y times X
  67                         const int nrow, const int ncol);
  68     // Forms square coefficient matrix
  69
  70     static bool GaussJordan (Matrix &b,                  // square matrix of coefficients
  71                              const vector<double> &y, // constant vector
  72                              vector<double> &coef);   // solution vector
  73     // returns false if matrix singular
  74
  75     static bool GaussJordan2(Matrix &b,
  76                              const vector<double> &y,
  77                              Matrix &w,
  78                              vector<vector<int> > &index);
  79 };
  80
  81 // some utility functions
  82
  83 namespace NSUtility
  84 {
  85     inline void swap(double &a, double &b) {double t = a; a = b; b = t;}
  86     void zeroise(vector<double> &array, int n);
  87     void zeroise(vector<int> &array, int n);
  88     void zeroise(vector<vector<double> > &matrix, int m, int n);
  89     void zeroise(vector<vector<int> > &matrix, int m, int n);
  90     inline double sqr(const double &x) {return x * x;}
  91 };
  92
  93 //---------------------------------------------------------------------------
  94 // Implementation
  95 //---------------------------------------------------------------------------
  96 using namespace NSUtility;
  97 //------------------------------------------------------------------------------------------
  98
  99
 100 // main PolyFit routine
 101
 102 double TPolyFit::PolyFit2 (const vector<double> &x,
 103                            const vector<double> &y,
 104                            vector<double> &coefs)
 105 // nterms = coefs.size()
 106 // npoints = x.size()
 107 {
 108     int i, j;
 109     double xi, yi, yc, srs, sum_y, sum_y2;
 110     Matrix xmatr;        // Data matrix
 111     Matrix a;
 112     vector<double> g;      // Constant vector
 113     const int npoints(x.size());
 114     const int nterms(coefs.size());
 115     double correl_coef;
 116     zeroise(g, nterms);
 117     zeroise(a, nterms, nterms);
 118     zeroise(xmatr, npoints, nterms);
 119     if (nterms < 1) {
 120         std::cerr << "ERROR: PolyFit called with less than one term" << std::endl;
 121         return 0;
 122     }
 123     if(npoints < 2) {
 124         std::cerr << "ERROR: PolyFit called with less than two points" << std::endl;
 125         return 0;
 126     }
 127     if(npoints != (int)y.size()) {
 128         std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl;
 129         return 0;
 130     }
 131     for(i = 0; i < npoints; ++i)
 132     {
 133         //      { setup x matrix }
 134         xi = x[i];
 135         xmatr[i][0] = 1.0;         //     { first column }
 136         for(j = 1; j < nterms; ++j)
 137             xmatr[i][j] = xmatr [i][j - 1] * xi;
 138     }
 139     Square (xmatr, y, a, g, npoints, nterms);
 140     if(!GaussJordan (a, g, coefs))
 141         return -1;
 142     sum_y = 0.0;
 143     sum_y2 = 0.0;
 144     srs = 0.0;
 145     for(i = 0; i < npoints; ++i)
 146     {
 147         yi = y[i];
 148         yc = 0.0;
 149         for(j = 0; j < nterms; ++j)
 150             yc += coefs [j] * xmatr [i][j];
 151         srs += sqr (yc - yi);
 152         sum_y += yi;
 153         sum_y2 += yi * yi;
 154     }
 155
 156     // If all Y values are the same, avoid dividing by zero
 157     correl_coef = sum_y2 - sqr (sum_y) / npoints;
 158     // Either return 0 or the correct value of correlation coefficient
 159     if (correl_coef != 0)
 160         correl_coef = srs / correl_coef;
 161     if (correl_coef >= 1)
 162         correl_coef = 0.0;
 163     else
 164         correl_coef = sqrt (1.0 - correl_coef);
 165     return correl_coef;
 166 }
 167
 168
 169 //------------------------------------------------------------------------
 170
 171 // Matrix multiplication routine
 172 // A = transpose X times X
 173 // G = Y times X
 174
 175 // Form square coefficient matrix
 176
 177 void TPolyFit::Square (const Matrix &x,
 178                        const vector<double> &y,
 179                        Matrix &a,
 180                        vector<double> &g,
 181                        const int nrow,
 182                        const int ncol)
 183 {
 184     int i, k, l;
 185     for(k = 0; k < ncol; ++k)
 186     {
 187         for(l = 0; l < k + 1; ++l)
 188         {
 189             a [k][l] = 0.0;
 190             for(i = 0; i < nrow; ++i)
 191             {
 192                 a[k][l] += x[i][l] * x [i][k];
 193                 if(k != l)
 194                     a[l][k] = a[k][l];
 195             }
 196         }
 197         g[k] = 0.0;
 198         for(i = 0; i < nrow; ++i)
 199             g[k] += y[i] * x[i][k];
 200     }
 201 }
 202 //---------------------------------------------------------------------------------
 203
 204
 205 bool TPolyFit::GaussJordan (Matrix &b,
 206                             const vector<double> &y,
 207                             vector<double> &coef)
 208 //b square matrix of coefficients
 209 //y constant vector
 210 //coef solution vector
 211 //ncol order of matrix got from b.size()
 212
 213
 214 {
 215 /*
 216   { Gauss Jordan matrix inversion and solution }
 217
 218   { B (n, n) coefficient matrix becomes inverse }
 219   { Y (n) original constant vector }
 220   { W (n, m) constant vector(s) become solution vector }
 221   { DETERM is the determinant }
 222   { ERROR = 1 if singular }
 223   { INDEX (n, 3) }
 224   { NV is number of constant vectors }
 225 */
 226
 227     int ncol(b.size());
 228     int irow, icol;
 229     vector<vector<int> >index;
 230     Matrix w;
 231
 232     zeroise(w, ncol, ncol);
 233     zeroise(index, ncol, 3);
 234
 235     if(!GaussJordan2(b, y, w, index))
 236         return false;
 237
 238     // Interchange columns
 239     int m;
 240     for (int i = 0; i <  ncol; ++i)
 241     {
 242         m = ncol - i - 1;
 243         if(index [m][0] != index [m][1])
 244         {
 245             irow = index [m][0];
 246             icol = index [m][1];
 247             for(int k = 0; k < ncol; ++k)
 248                 swap (b[k][irow], b[k][icol]);
 249         }
 250     }
 251
 252     for(int k = 0; k < ncol; ++k)
 253     {
 254         if(index [k][2] != 0)
 255         {
 256             std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl;
 257             return false;
 258         }
 259     }
 260
 261     for( int i = 0; i < ncol; ++i)
 262         coef[i] = w[i][0];
 263
 264
 265     return true;
 266 }   // end;     { procedure GaussJordan }
 267 //----------------------------------------------------------------------------------------------
 268
 269
 270 bool TPolyFit::GaussJordan2(Matrix &b,
 271                             const vector<double> &y,
 272                             Matrix &w,
 273                             vector<vector<int> > &index)
 274 {
 275     //GaussJordan2;         // first half of GaussJordan
 276     // actual start of gaussj
 277
 278     double big, t;
 279     double pivot;
 280     double determ;
 281     int irow, icol;
 282     int ncol(b.size());
 283     int nv = 1;                  // single constant vector
 284     for(int i = 0; i < ncol; ++i)
 285     {
 286         w[i][0] = y[i];      // copy constant vector
 287         index[i][2] = -1;
 288     }
 289     determ = 1.0;
 290     for(int i = 0; i < ncol; ++i)
 291     {
 292         // Search for largest element
 293         big = 0.0;
 294         for(int j = 0; j < ncol; ++j)
 295         {
 296             if(index[j][2] != 0)
 297             {
 298                 for(int k = 0; k < ncol; ++k)
 299                 {
 300                     if(index[k][2] > 0) {
 301                         std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
 302                         return false;
 303                     }
 304
 305                     if(index[k][2] < 0 && fabs(b[j][k]) > big)
 306                     {
 307                         irow = j;
 308                         icol = k;
 309                         big = fabs(b[j][k]);
 310                     }
 311                 } //   { k-loop }
 312             }
 313         }  // { j-loop }
 314         index [icol][2] = index [icol][2] + 1;
 315         index [i][0] = irow;
 316         index [i][1] = icol;
 317
 318         // Interchange rows to put pivot on diagonal
 319         // GJ3
 320         if(irow != icol)
 321         {
 322             determ = -determ;
 323             for(int m = 0; m < ncol; ++m)
 324                 swap (b [irow][m], b[icol][m]);
 325             if (nv > 0)
 326                 for (int m = 0; m < nv; ++m)
 327                     swap (w[irow][m], w[icol][m]);
 328         } // end GJ3
 329
 330         // divide pivot row by pivot column
 331         pivot = b[icol][icol];
 332         determ *= pivot;
 333         b[icol][icol] = 1.0;
 334
 335         for(int m = 0; m < ncol; ++m)
 336             b[icol][m] /= pivot;
 337         if(nv > 0)
 338             for(int m = 0; m < nv; ++m)
 339                 w[icol][m] /= pivot;
 340
 341         // Reduce nonpivot rows
 342         for(int n = 0; n < ncol; ++n)
 343         {
 344             if(n != icol)
 345             {
 346                 t = b[n][icol];
 347                 b[n][icol] = 0.0;
 348                 for(int m = 0; m < ncol; ++m)
 349                     b[n][m] -= b[icol][m] * t;
 350                 if(nv > 0)
 351                     for(int m = 0; m < nv; ++m)
 352                         w[n][m] -= w[icol][m] * t;
 353             }
 354         }
 355     } // { i-loop }
 356     return true;
 357 }
 358 //----------------------------------------------------------------------------------------------
 359
 360 //------------------------------------------------------------------------------------
 361
 362 // Utility functions
 363 //--------------------------------------------------------------------------
 364
 365 // fills a vector with zeros.
 366 void NSUtility::zeroise(vector<double> &array, int n)
 367 {
 368     array.clear();
 369     for(int j = 0; j < n; ++j)
 370         array.push_back(0);
 371 }
 372 //--------------------------------------------------------------------------
 373
 374 // fills a vector with zeros.
 375 void NSUtility::zeroise(vector<int> &array, int n)
 376 {
 377     array.clear();
 378     for(int j = 0; j < n; ++j)
 379         array.push_back(0);
 380 }
 381 //--------------------------------------------------------------------------
 382
 383 // fills a (m by n) matrix with zeros.
 384 void NSUtility::zeroise(vector<vector<double> > &matrix, int m, int n)
 385 {
 386     vector<double> zero;
 387     zeroise(zero, n);
 388     matrix.clear();
 389     for(int j = 0; j < m; ++j)
 390         matrix.push_back(zero);
 391 }
 392 //--------------------------------------------------------------------------
 393
 394 // fills a (m by n) matrix with zeros.
 395 void NSUtility::zeroise(vector<vector<int> > &matrix, int m, int n)
 396 {
 397     vector<int> zero;
 398     zeroise(zero, n);
 399     matrix.clear();
 400     for(int j = 0; j < m; ++j)
 401         matrix.push_back(zero);
 402 }
 403 //--------------------------------------------------------------------------
 404
 405
 406 #endif
 407