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 struct NSUtility
  84 {
  85     static void swap(double &a, double &b) {double t = a; a = b; b = t;}
  86     // fills a vector with zeros.
  87     static void zeroise(vector<double> &array, int n) {
  88         array.clear();
  89         for(int j = 0; j < n; ++j) array.push_back(0);
  90     }
  91     // fills a vector with zeros.
  92     static void zeroise(vector<int> &array, int n) {
  93         array.clear();
  94         for(int j = 0; j < n; ++j) array.push_back(0);
  95     }
  96     // fills a (m by n) matrix with zeros.
  97     static void zeroise(vector<vector<double> > &matrix, int m, int n) {
  98         vector<double> zero;
  99         zeroise(zero, n);
 100         matrix.clear();
 101         for(int j = 0; j < m; ++j) matrix.push_back(zero);
 102     }
 103     // fills a (m by n) matrix with zeros.
 104     static void zeroise(vector<vector<int> > &matrix, int m, int n) {
 105         vector<int> zero;
 106         zeroise(zero, n);
 107         matrix.clear();
 108         for(int j = 0; j < m; ++j) matrix.push_back(zero);
 109     }
 110     static double sqr(const double &x) {return x * x;}
 111 };
 112
 113
 114 // main PolyFit routine
 115
 116 double TPolyFit::PolyFit2 (const vector<double> &x,
 117                            const vector<double> &y,
 118                            vector<double> &coefs)
 119 // nterms = coefs.size()
 120 // npoints = x.size()
 121 {
 122     int i, j;
 123     double xi, yi, yc, srs, sum_y, sum_y2;
 124     Matrix xmatr;        // Data matrix
 125     Matrix a;
 126     vector<double> g;      // Constant vector
 127     const int npoints(x.size());
 128     const int nterms(coefs.size());
 129     double correl_coef;
 130     NSUtility::zeroise(g, nterms);
 131     NSUtility::zeroise(a, nterms, nterms);
 132     NSUtility::zeroise(xmatr, npoints, nterms);
 133     if (nterms < 1) {
 134         std::cerr << "ERROR: PolyFit called with less than one term" << std::endl;
 135         return 0;
 136     }
 137     if(npoints < 2) {
 138         std::cerr << "ERROR: PolyFit called with less than two points" << std::endl;
 139         return 0;
 140     }
 141     if(npoints != (int)y.size()) {
 142         std::cerr << "ERROR: PolyFit called with x and y of unequal size" << std::endl;
 143         return 0;
 144     }
 145     for(i = 0; i < npoints; ++i)
 146     {
 147         //      { setup x matrix }
 148         xi = x[i];
 149         xmatr[i][0] = 1.0;         //     { first column }
 150         for(j = 1; j < nterms; ++j)
 151             xmatr[i][j] = xmatr [i][j - 1] * xi;
 152     }
 153     Square (xmatr, y, a, g, npoints, nterms);
 154     if(!GaussJordan (a, g, coefs))
 155         return -1;
 156     sum_y = 0.0;
 157     sum_y2 = 0.0;
 158     srs = 0.0;
 159     for(i = 0; i < npoints; ++i)
 160     {
 161         yi = y[i];
 162         yc = 0.0;
 163         for(j = 0; j < nterms; ++j)
 164             yc += coefs [j] * xmatr [i][j];
 165         srs += NSUtility::sqr (yc - yi);
 166         sum_y += yi;
 167         sum_y2 += yi * yi;
 168     }
 169
 170     // If all Y values are the same, avoid dividing by zero
 171     correl_coef = sum_y2 - NSUtility::sqr (sum_y) / npoints;
 172     // Either return 0 or the correct value of correlation coefficient
 173     if (correl_coef != 0)
 174         correl_coef = srs / correl_coef;
 175     if (correl_coef >= 1)
 176         correl_coef = 0.0;
 177     else
 178         correl_coef = sqrt (1.0 - correl_coef);
 179     return correl_coef;
 180 }
 181
 182
 183 //------------------------------------------------------------------------
 184
 185 // Matrix multiplication routine
 186 // A = transpose X times X
 187 // G = Y times X
 188
 189 // Form square coefficient matrix
 190
 191 void TPolyFit::Square (const Matrix &x,
 192                        const vector<double> &y,
 193                        Matrix &a,
 194                        vector<double> &g,
 195                        const int nrow,
 196                        const int ncol)
 197 {
 198     int i, k, l;
 199     for(k = 0; k < ncol; ++k)
 200     {
 201         for(l = 0; l < k + 1; ++l)
 202         {
 203             a [k][l] = 0.0;
 204             for(i = 0; i < nrow; ++i)
 205             {
 206                 a[k][l] += x[i][l] * x [i][k];
 207                 if(k != l)
 208                     a[l][k] = a[k][l];
 209             }
 210         }
 211         g[k] = 0.0;
 212         for(i = 0; i < nrow; ++i)
 213             g[k] += y[i] * x[i][k];
 214     }
 215 }
 216 //---------------------------------------------------------------------------------
 217
 218
 219 bool TPolyFit::GaussJordan (Matrix &b,
 220                             const vector<double> &y,
 221                             vector<double> &coef)
 222 //b square matrix of coefficients
 223 //y constant vector
 224 //coef solution vector
 225 //ncol order of matrix got from b.size()
 226
 227
 228 {
 229 /*
 230   { Gauss Jordan matrix inversion and solution }
 231
 232   { B (n, n) coefficient matrix becomes inverse }
 233   { Y (n) original constant vector }
 234   { W (n, m) constant vector(s) become solution vector }
 235   { DETERM is the determinant }
 236   { ERROR = 1 if singular }
 237   { INDEX (n, 3) }
 238   { NV is number of constant vectors }
 239 */
 240
 241     int ncol(b.size());
 242     int irow, icol;
 243     vector<vector<int> >index;
 244     Matrix w;
 245
 246     NSUtility::zeroise(w, ncol, ncol);
 247     NSUtility::zeroise(index, ncol, 3);
 248
 249     if(!GaussJordan2(b, y, w, index))
 250         return false;
 251
 252     // Interchange columns
 253     int m;
 254     for (int i = 0; i <  ncol; ++i)
 255     {
 256         m = ncol - i - 1;
 257         if(index [m][0] != index [m][1])
 258         {
 259             irow = index [m][0];
 260             icol = index [m][1];
 261             for(int k = 0; k < ncol; ++k)
 262                 swap (b[k][irow], b[k][icol]);
 263         }
 264     }
 265
 266     for(int k = 0; k < ncol; ++k)
 267     {
 268         if(index [k][2] != 0)
 269         {
 270             std::cerr << "ERROR: Error in PolyFit::GaussJordan: matrix is singular" << std::endl;
 271             return false;
 272         }
 273     }
 274
 275     for( int i = 0; i < ncol; ++i)
 276         coef[i] = w[i][0];
 277
 278
 279     return true;
 280 }   // end;     { procedure GaussJordan }
 281 //----------------------------------------------------------------------------------------------
 282
 283
 284 bool TPolyFit::GaussJordan2(Matrix &b,
 285                             const vector<double> &y,
 286                             Matrix &w,
 287                             vector<vector<int> > &index)
 288 {
 289     //GaussJordan2;         // first half of GaussJordan
 290     // actual start of gaussj
 291
 292     double big, t;
 293     double pivot;
 294     double determ;
 295     int irow = 0, icol = 0;
 296     int ncol(b.size());
 297     int nv = 1;                  // single constant vector
 298     for(int i = 0; i < ncol; ++i)
 299     {
 300         w[i][0] = y[i];      // copy constant vector
 301         index[i][2] = -1;
 302     }
 303     determ = 1.0;
 304     for(int i = 0; i < ncol; ++i)
 305     {
 306         // Search for largest element
 307         big = 0.0;
 308         for(int j = 0; j < ncol; ++j)
 309         {
 310             if(index[j][2] != 0)
 311             {
 312                 for(int k = 0; k < ncol; ++k)
 313                 {
 314                     if(index[k][2] > 0) {
 315                         std::cerr << "ERROR: Error in PolyFit::GaussJordan2: matrix is singular" << std::endl;
 316                         return false;
 317                     }
 318
 319                     if(index[k][2] < 0 && fabs(b[j][k]) > big)
 320                     {
 321                         irow = j;
 322                         icol = k;
 323                         big = fabs(b[j][k]);
 324                     }
 325                 } //   { k-loop }
 326             }
 327         }  // { j-loop }
 328         index [icol][2] = index [icol][2] + 1;
 329         index [i][0] = irow;
 330         index [i][1] = icol;
 331
 332         // Interchange rows to put pivot on diagonal
 333         // GJ3
 334         if(irow != icol)
 335         {
 336             determ = -determ;
 337             for(int m = 0; m < ncol; ++m)
 338                 swap (b [irow][m], b[icol][m]);
 339             if (nv > 0)
 340                 for (int m = 0; m < nv; ++m)
 341                     swap (w[irow][m], w[icol][m]);
 342         } // end GJ3
 343
 344         // divide pivot row by pivot column
 345         pivot = b[icol][icol];
 346         determ *= pivot;
 347         b[icol][icol] = 1.0;
 348
 349         for(int m = 0; m < ncol; ++m)
 350             b[icol][m] /= pivot;
 351         if(nv > 0)
 352             for(int m = 0; m < nv; ++m)
 353                 w[icol][m] /= pivot;
 354
 355         // Reduce nonpivot rows
 356         for(int n = 0; n < ncol; ++n)
 357         {
 358             if(n != icol)
 359             {
 360                 t = b[n][icol];
 361                 b[n][icol] = 0.0;
 362                 for(int m = 0; m < ncol; ++m)
 363                     b[n][m] -= b[icol][m] * t;
 364                 if(nv > 0)
 365                     for(int m = 0; m < nv; ++m)
 366                         w[n][m] -= w[icol][m] * t;
 367             }
 368         }
 369     } // { i-loop }
 370     return true;
 371 }
 372
 373 #endif
 374