Tue, 05 Mar 2024 10:25:15 +0100
Clear mash measurments when duplicating a product.
508 | 1 | /** |
2 | * @brief Simple polynomial fitting functions. | |
3 | * @file polyfit.cpp | |
4 | * @autor Henry M. Forson, Melbourne, Florida USA | |
5 | * | |
6 | *------------------------------------------------------------------------------------ | |
7 | * MIT License | |
8 | * | |
9 | * Copyright (c) 2020 Henry M. Forson | |
10 | * | |
11 | * Permission is hereby granted, free of charge, to any person obtaining a copy | |
12 | * of this software and associated documentation files (the "Software"), to deal | |
13 | * in the Software without restriction, including without limitation the rights | |
14 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | |
15 | * copies of the Software, and to permit persons to whom the Software is | |
16 | * furnished to do so, subject to the following conditions: | |
17 | * | |
18 | * The above copyright notice and this permission notice shall be included in all | |
19 | * copies or substantial portions of the Software. | |
20 | * | |
21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | |
22 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | |
23 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | |
24 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | |
25 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | |
26 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE | |
27 | * SOFTWARE. | |
28 | *------------------------------------------------------------------------------------ | |
29 | */ | |
30 | ||
31 | #include "polyfit.h" | |
32 | ||
33 | #include <QDebug> | |
34 | #include <math.h> | |
35 | ||
36 | ||
37 | ||
38 | // Structure of a matrix. | |
39 | typedef struct matrix_s | |
40 | { | |
41 | int rows; | |
42 | int cols; | |
43 | double *pContents; | |
44 | } matrix_t; | |
45 | ||
46 | // MACRO to access a value with a matrix. | |
47 | #define MATRIX_VALUE_PTR( pA, row, col ) (&(((pA)->pContents)[ (row * (pA)->cols) + col])) | |
48 | ||
49 | ||
50 | //------------------------------------------------ | |
51 | // Private Function Prototypes | |
52 | //------------------------------------------------ | |
53 | ||
54 | static matrix_t * createMatrix( int rows, int cols ); | |
55 | static void destroyMatrix( matrix_t *pMat ); | |
56 | static matrix_t * createTranspose( matrix_t *pMat ); | |
57 | static matrix_t * createProduct( matrix_t *pLeft, matrix_t *pRight ); | |
58 | ||
59 | ||
60 | //========================================================= | |
61 | // Global function definitions | |
62 | //========================================================= | |
63 | ||
64 | ||
65 | //-------------------------------------------------------- | |
66 | // polyfit() | |
67 | // Computes polynomial coefficients that best fit a set | |
68 | // of input points. | |
69 | // | |
70 | // The degree of the fitted polynomial is one less than | |
71 | // the count of elements in the polynomial vector. | |
72 | // | |
73 | // Uses matrix algebra of the form: | |
74 | // A * x = b | |
75 | // To solve the MLS equation: | |
76 | // (AT) * A * x = (AT) * b | |
77 | // where (AT) is the transpose of A. | |
78 | // | |
79 | // If the n+1 input points are {(x0, y0), (x1, y1), ... (xn, yn)}, | |
80 | // then the i'th row of A is: {(xi)^0, (xi)^1, ... (xn)^n}, | |
81 | // and the i'th row of b is: {yi}. | |
82 | // | |
83 | // Returns 0 if success, | |
84 | // -1 if passed a NULL pointer, | |
85 | // -2 if (pointCount < coefficientCount), | |
86 | // -3 if unable to allocate memory, | |
87 | // -4 if unable to solve equations. | |
88 | //-------------------------------------------------------- | |
89 | //int polyfit( int pointCount, point_t pointArray[], int coeffCount, double coeffArray[] ) | |
90 | int Polyfit::polyfit( int pointCount, double *xValues, double *yValues, int coefficientCount, double *coefficientResults ) | |
91 | { | |
92 | int rVal = 0; | |
93 | int degree = coefficientCount - 1; | |
94 | ||
95 | // Check that the input pointers aren't null. | |
96 | if( (NULL == xValues) || (NULL == yValues) || (NULL == coefficientResults) ) | |
97 | { | |
98 | return -1; | |
99 | } | |
100 | // Check that pointCount >= coefficientCount. | |
101 | if(pointCount < coefficientCount) | |
102 | { | |
103 | return -2; | |
104 | } | |
105 | ||
106 | // Make the A matrix: | |
107 | matrix_t *pMatA = createMatrix( pointCount, coefficientCount ); | |
108 | if( NULL == pMatA) | |
109 | { | |
110 | return -3; | |
111 | } | |
112 | ||
113 | for( int r = 0; r < pointCount; r++) | |
114 | { | |
115 | for( int c = 0; c < coefficientCount; c++) | |
116 | { | |
117 | *(MATRIX_VALUE_PTR(pMatA, r, c)) = powl((xValues[r]), (double) (degree -c)); | |
118 | } | |
119 | } | |
120 | ||
121 | // Make the b matrix | |
122 | matrix_t *pMatB = createMatrix( pointCount, 1); | |
123 | if( NULL == pMatB ) | |
124 | { | |
125 | return -3; | |
126 | } | |
127 | ||
128 | for( int r = 0; r < pointCount; r++) | |
129 | { | |
130 | *(MATRIX_VALUE_PTR(pMatB, r, 0)) = yValues[r]; | |
131 | } | |
132 | ||
133 | // Make the transpose of matrix A | |
134 | matrix_t * pMatAT = createTranspose( pMatA ); | |
135 | if( NULL == pMatAT ) | |
136 | { | |
137 | return -3; | |
138 | } | |
139 | ||
140 | // Make the product of matrices AT and A: | |
141 | matrix_t *pMatATA = createProduct( pMatAT, pMatA ); | |
142 | if( NULL == pMatATA ) | |
143 | { | |
144 | return -3; | |
145 | } | |
146 | ||
147 | // Make the product of matrices AT and b: | |
148 | matrix_t *pMatATB = createProduct( pMatAT, pMatB ); | |
149 | if( NULL == pMatATB ) | |
150 | { | |
151 | return -3; | |
152 | } | |
153 | ||
154 | // Now we need to solve the system of linear equations, | |
155 | // (AT)Ax = (AT)b for "x", the coefficients of the polynomial. | |
156 | ||
157 | for( int c = 0; c < pMatATA->cols; c++ ) | |
158 | { | |
159 | int pr = c; // pr is the pivot row. | |
160 | double prVal = *MATRIX_VALUE_PTR(pMatATA, pr, c); | |
161 | // If it's zero, we can't solve the equations. | |
162 | if( 0.0 == prVal ) | |
163 | { | |
164 | // printf( "Unable to solve equations, pr = %d, c = %d.\n", pr, c ); | |
165 | rVal = -4; | |
166 | break; | |
167 | } | |
168 | for( int r = 0; r < pMatATA->rows; r++) | |
169 | { | |
170 | if( r != pr ) | |
171 | { | |
172 | double targetRowVal = *MATRIX_VALUE_PTR(pMatATA, r, c); | |
173 | double factor = targetRowVal / prVal; | |
174 | for( int c2 = 0; c2 < pMatATA->cols; c2++ ) | |
175 | { | |
176 | *MATRIX_VALUE_PTR(pMatATA, r, c2) -= *MATRIX_VALUE_PTR(pMatATA, pr, c2) * factor; | |
177 | // printf( "c = %d, pr = %d, r = %d, c2=%d, targetRowVal = %f, prVal = %f, factor = %f.\n", | |
178 | // c, pr, r, c2, targetRowVal, prVal, factor ); | |
179 | ||
180 | // showMatrix( pMatATA ); | |
181 | ||
182 | } | |
183 | *MATRIX_VALUE_PTR(pMatATB, r, 0) -= *MATRIX_VALUE_PTR(pMatATB, pr, 0) * factor; | |
184 | } | |
185 | } | |
186 | } | |
187 | for( int c = 0; c < pMatATA->cols; c++ ) | |
188 | { | |
189 | int pr = c; | |
190 | // now, pr is the pivot row. | |
191 | double prVal = *MATRIX_VALUE_PTR(pMatATA, pr, c); | |
192 | *MATRIX_VALUE_PTR(pMatATA, pr, c) /= prVal; | |
193 | *MATRIX_VALUE_PTR(pMatATB, pr, 0) /= prVal; | |
194 | } | |
195 | ||
196 | for( int i = 0; i < coefficientCount; i++) | |
197 | { | |
198 | coefficientResults[i] = *MATRIX_VALUE_PTR(pMatATB, i, 0); | |
199 | } | |
200 | ||
201 | ||
202 | destroyMatrix( pMatATB ); | |
203 | destroyMatrix( pMatATA ); | |
204 | destroyMatrix( pMatAT ); | |
205 | ||
206 | destroyMatrix( pMatA ); | |
207 | destroyMatrix( pMatB ); | |
208 | return rVal; | |
209 | } | |
210 | ||
211 | ||
212 | //========================================================= | |
213 | // Private function definitions | |
214 | //========================================================= | |
215 | ||
216 | ||
217 | //-------------------------------------------------------- | |
218 | // createTranspose() | |
219 | // Returns the transpose of a matrix, or NULL. | |
220 | // | |
221 | // The caller must free both the allocated matrix | |
222 | // and its contents array. | |
223 | //-------------------------------------------------------- | |
224 | static matrix_t * createTranspose( matrix_t *pMat ) | |
225 | { | |
226 | matrix_t *rVal = (matrix_t *) calloc(1, sizeof(matrix_t)); | |
227 | rVal->pContents = (double *) calloc( pMat->rows * pMat->cols, sizeof( double )); | |
228 | rVal->cols = pMat->rows; | |
229 | rVal->rows = pMat->cols; | |
230 | for( int r = 0; r < rVal->rows; r++ ) | |
231 | { | |
232 | for( int c = 0; c < rVal->cols; c++ ) | |
233 | { | |
234 | *MATRIX_VALUE_PTR(rVal, r, c) = *MATRIX_VALUE_PTR(pMat, c, r); | |
235 | } | |
236 | } | |
237 | return rVal; | |
238 | } | |
239 | ||
240 | //-------------------------------------------------------- | |
241 | // createProduct() | |
242 | // Returns the product of two matrices, or NULL. | |
243 | // | |
244 | // The caller must free both the allocated product matrix | |
245 | // and its contents array. | |
246 | //-------------------------------------------------------- | |
247 | static matrix_t * createProduct( matrix_t *pLeft, matrix_t *pRight ) | |
248 | { | |
249 | matrix_t *rVal = NULL; | |
250 | if( (NULL == pLeft) || (NULL == pRight) || (pLeft->cols != pRight->rows) ) | |
251 | { | |
252 | printf( "Illegal parameter passed to createProduct().\n"); | |
253 | } | |
254 | else | |
255 | { | |
256 | // Allocate the product matrix. | |
257 | rVal = (matrix_t *) calloc(1, sizeof(matrix_t)); | |
258 | rVal->rows = pLeft->rows; | |
259 | rVal->cols = pRight->cols; | |
260 | rVal->pContents = (double *) calloc( rVal->rows * rVal->cols, sizeof( double )); | |
261 | ||
262 | // Initialize the product matrix contents: | |
263 | // product[i,j] = sum{k = 0 .. (pLeft->cols - 1)} (pLeft[i,k] * pRight[ k, j]) | |
264 | for( int i = 0; i < rVal->rows; i++) | |
265 | { | |
266 | for( int j = 0; j < rVal->cols; j++ ) | |
267 | { | |
268 | for( int k = 0; k < pLeft->cols; k++) | |
269 | { | |
270 | *MATRIX_VALUE_PTR(rVal, i, j) += | |
271 | (*MATRIX_VALUE_PTR(pLeft, i, k)) * (*MATRIX_VALUE_PTR(pRight, k, j)); | |
272 | } | |
273 | } | |
274 | } | |
275 | } | |
276 | ||
277 | return rVal; | |
278 | } | |
279 | ||
280 | //-------------------------------------------------------- | |
281 | // destroyMatrix() | |
282 | // Frees both the allocated matrix and its contents array. | |
283 | //-------------------------------------------------------- | |
284 | static void destroyMatrix( matrix_t *pMat ) | |
285 | { | |
286 | if(NULL != pMat) | |
287 | { | |
288 | if(NULL != pMat->pContents) | |
289 | { | |
290 | free(pMat->pContents); | |
291 | } | |
292 | free( pMat ); | |
293 | } | |
294 | } | |
295 | ||
296 | //-------------------------------------------------------- | |
297 | // createMatrix() | |
298 | // Allocates the matrix and clears its contents array. | |
299 | //-------------------------------------------------------- | |
300 | static matrix_t *createMatrix( int rows, int cols ) | |
301 | { | |
302 | matrix_t *rVal = (matrix_t *) calloc(1, sizeof(matrix_t)); | |
303 | if(NULL != rVal) | |
304 | { | |
305 | rVal->rows = rows; | |
306 | rVal->cols = cols; | |
307 | rVal->pContents = (double *) calloc( rows * cols, sizeof( double )); | |
308 | if(NULL == rVal->pContents) | |
309 | { | |
310 | free( rVal ); | |
311 | rVal = NULL; | |
312 | } | |
313 | } | |
314 | ||
315 | return rVal; | |
316 | } | |
317 | ||
318 |