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