comparison: src/polyfit.cpp
src/polyfit.cpp
- changeset 508
- 7f2ec2bc9d2e
equal
deleted
inserted
replaced
| 507:fa07b6c6238a | 508:7f2ec2bc9d2e |
|---|---|
| 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 |
