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     unsigned 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 unsigned int npoints(x.size());
 114     const unsigned 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 != 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 = 0;
 282     int icol = 0;
 283     int ncol(b.size());
 284     int nv = 1;                  // single constant vector
 285     for(int i = 0; i < ncol; ++i)
 286     {
 287         w[i][0] = y[i];      // copy constant vector
 288         index[i][2] = -1;
 289     }
 290     determ = 1.0;
 291     for(int i = 0; i < ncol; ++i)
 292     {
 293         // Search for largest element
 294         big = 0.0;
 295         for(int j = 0; j < ncol; ++j)
 296         {
 297             if(index[j][2] != 0)
 298             {
 299                 for(int k = 0; k < ncol; ++k)
 300                 {
 301                     if(index[k][2] > 0) {
 302                         std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
 303                         return false;
 304                     }
 305
 306                     if(index[k][2] < 0 && fabs(b[j][k]) > big)
 307                     {
 308                         irow = j;
 309                         icol = k;
 310                         big = fabs(b[j][k]);
 311                     }
 312                 } //   { k-loop }
 313             }
 314         }  // { j-loop }
 315         index [icol][2] = index [icol][2] + 1;
 316         index [i][0] = irow;
 317         index [i][1] = icol;
 318
 319         // Interchange rows to put pivot on diagonal
 320         // GJ3
 321         if(irow != icol)
 322         {
 323             determ = -determ;
 324             for(int m = 0; m < ncol; ++m)
 325                 swap (b [irow][m], b[icol][m]);
 326             if (nv > 0)
 327                 for (int m = 0; m < nv; ++m)
 328                     swap (w[irow][m], w[icol][m]);
 329         } // end GJ3
 330
 331         // divide pivot row by pivot column
 332         pivot = b[icol][icol];
 333         determ *= pivot;
 334         b[icol][icol] = 1.0;
 335
 336         for(int m = 0; m < ncol; ++m)
 337             b[icol][m] /= pivot;
 338         if(nv > 0)
 339             for(int m = 0; m < nv; ++m)
 340                 w[icol][m] /= pivot;
 341
 342         // Reduce nonpivot rows
 343         for(int n = 0; n < ncol; ++n)
 344         {
 345             if(n != icol)
 346             {
 347                 t = b[n][icol];
 348                 b[n][icol] = 0.0;
 349                 for(int m = 0; m < ncol; ++m)
 350                     b[n][m] -= b[icol][m] * t;
 351                 if(nv > 0)
 352                     for(int m = 0; m < nv; ++m)
 353                         w[n][m] -= w[icol][m] * t;
 354             }
 355         }
 356     } // { i-loop }
 357     return true;
 358 }
 359 //----------------------------------------------------------------------------------------------
 360
 361 //------------------------------------------------------------------------------------
 362
 363 // Utility functions
 364 //--------------------------------------------------------------------------
 365
 366 // fills a vector with zeros.
 367 void NSUtility::zeroise(vector<double> &array, int n)
 368 {
 369     array.clear();
 370     for(int j = 0; j < n; ++j)
 371         array.push_back(0);
 372 }
 373 //--------------------------------------------------------------------------
 374
 375 // fills a vector with zeros.
 376 void NSUtility::zeroise(vector<int> &array, int n)
 377 {
 378     array.clear();
 379     for(int j = 0; j < n; ++j)
 380         array.push_back(0);
 381 }
 382 //--------------------------------------------------------------------------
 383
 384 // fills a (m by n) matrix with zeros.
 385 void NSUtility::zeroise(vector<vector<double> > &matrix, int m, int n)
 386 {
 387     vector<double> zero;
 388     zeroise(zero, n);
 389     matrix.clear();
 390     for(int j = 0; j < m; ++j)
 391         matrix.push_back(zero);
 392 }
 393 //--------------------------------------------------------------------------
 394
 395 // fills a (m by n) matrix with zeros.
 396 void NSUtility::zeroise(vector<vector<int> > &matrix, int m, int n)
 397 {
 398     vector<int> zero;
 399     zeroise(zero, n);
 400     matrix.clear();
 401     for(int j = 0; j < m; ++j)
 402         matrix.push_back(zero);
 403 }
 404 //--------------------------------------------------------------------------
 405
 406
 407 #endif
 408