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