Changeset 7706 for trunk/projects/slimcurve/src/main/c/EcfUtil.c
 Timestamp:
 05/13/11 22:54:42 (9 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/projects/slimcurve/src/main/c/EcfUtil.c
r7606 r7706 13 13 #include <stdlib.h> 14 14 #include <stdarg.h> 15 #include <string.h> 15 16 #include <math.h> 16 17 #ifdef _CVI_ … … 24 25 /******************************************************************** 25 26 26 GAUSSJORDANAND COVARSORTING ROUTINES27 EQUATIONSOLVING AND COVARSORTING ROUTINES 27 28 28 29 *********************************************************************/ 29 30 30 31 /* Linear equation solution by GaussJordan elimination.32 a[0..n1][0..n1] is the input matrix,33 b[0..n] is input containing the single righthand side vector.34 On output, a is replaced by its matrix inverse and b is replaced by35 the corresponding set of solution vectors. */36 37 31 #define SWAP(a,b) { temp=(a); (a)=(b); (b)=temp; } 38 32 39 /* 40 * Based on LinearEquationSolving from linearEquations.c by Henry Guennadi Levkin. 33 /* Linear equation solution of Ax = b by Gaussian elimination. 34 A is the n x n input matrix, b is the righthand side vector, length n. 35 On output, b is replaced by the corresponding set of solution vectors 36 and A is trashed. 41 37 */ 42 int GCI_ gauss_jordan(float **a, int n, float *b)38 int GCI_solve_Gaussian(float **a, int n, float *b) 43 39 { 44 40 float max; 45 float t mp;46 41 float temp; 42 float pivotInverse[n]; 47 43 int i, j, k, m; 44 45 /*int q, w; 46 printf("\n"); 47 for (q = 0; q < n; ++q) { 48 for (w = 0; w < n; ++w) { 49 printf("%f ", a[q][w]); 50 } 51 printf(" %f\n", b[q]); 52 } */ 48 53 49 54 // base row of matrix … … 67 72 for (i = k; i < n; ++i) 68 73 { 69 tmp = a[k][i]; 70 a[k][i] = a[m][i]; 71 a[m][i] = tmp; 74 SWAP(a[k][i], a[m][i]); 72 75 } 73 tmp = b[k]; 74 b[k] = b[m]; 75 b[m] = tmp; 76 SWAP(b[k], b[m]); 76 77 } 77 78 … … 82 83 83 84 // triangulation of matrix with coefficients 85 pivotInverse[k] = 1.0 / a[k][k]; 84 86 for (j = k + 1; j < n; ++j) // current row of matrix 85 87 { 86 tmp = a[j][k] / a[k][k]; 88 // want "temp = a[j][k] / a[k][k]" 89 temp = a[j][k] * pivotInverse[k]; 87 90 for (i = k; i < n; ++i) 88 91 { 89 a[j][i] += t mp * a[k][i];92 a[j][i] += temp * a[k][i]; 90 93 } 91 b[j] += tmp * b[k]; // free member recalculation 92 } 93 } 94 b[j] += temp * b[k]; // free member recalculation 95 } 96 } 97 // precalculate last pivot inverse 98 pivotInverse[n  1] = 1.0 / a[n  1][n  1]; 94 99 95 100 for (k = n  1; k >= 0; k) … … 99 104 b[k] = a[k][i] * b[i]; 100 105 } 101 b[k] /= a[k][k]; 102 } 106 b[k] *= pivotInverse[k]; 107 } 108 109 /*printf("====>\n"); 110 for (q = 0; q < n; ++q) { 111 for (w = 0; w < n; ++w) { 112 printf("%f ", a[q][w]); 113 } 114 printf(" %f\n", b[q]); 115 }*/ 116 103 117 return 0; 104 118 } 105 119 106 //============================================================================== 107 // return 1 if system not solving 108 // nDim  system dimension 109 // pfMatr  matrix with coefficients 110 // pfVect  vector with free members 111 // pfSolution  vector with system solution 112 // pfMatr becames trianglular after function call 113 // pfVect changes after function call 114 // 115 // Developer: Henry Guennadi Levkin 116 // 117 //============================================================================== 118 int LinearEquationsSolving(int nDim, double* pfMatr, double* pfVect, double* pfSolution) 119 { 120 double fMaxElem; 121 double fAcc; 122 123 int i , j, k, m; 124 125 126 for(k=0; k<(nDim1); k++) // base row of matrix 127 { 128 // search of line with max element 129 fMaxElem = fabs( pfMatr[k*nDim + k] ); 130 m = k; 131 for(i=k+1; i<nDim; i++) 120 /* Matrix inversion by Gaussian elimination. 121 A is the n x n input matrix. 122 On output, A is replaced by its matrix inverse. 123 Returns 0 upon success, 2 if matrix is singular. 124 */ 125 int GCI_invert_Gaussian(float **a, int n) 126 { 127 int returnValue = 0; 128 float identity[n][n]; 129 float **work = GCI_ecf_matrix(n, n); 130 int i, j, k; 131 132 for (j = 0; j < n; ++j) { 133 // find inverse by columns 134 for (i = 0; i < n; ++i) { 135 identity[j][i] = 0.0; 136 // need a fresh copy of matrix a 137 for (k = 0; k < n; ++k) { 138 work[k][i] = a[k][i]; 139 } 140 } 141 identity[j][j] = 1.0; 142 returnValue = GCI_solve_Gaussian(work, n, identity[j]); 143 if (returnValue < 0) { 144 return returnValue; 145 } 146 } 147 GCI_ecf_free_matrix(work); 148 149 // copy over results 150 for (j = 0; j < n; ++j) { 151 for (i = 0; i < n; ++i) { 152 a[j][i] = identity[j][i]; 153 } 154 } 155 return returnValue; 156 } 157 158 /* Pivots matrix on given column. Also keeps track of order. 159 */ 160 void pivot(float **a, int n, int *order, int col) 161 { 162 int pivotRow; 163 float maxValue; 164 float rowValue; 165 int i; 166 167 // find row with maximum element value in col, below diagonal 168 pivotRow = col; 169 maxValue = fabs(a[col][col]); 170 for (i = col + 1; i < n; ++i) { 171 rowValue = fabs(a[i][col]); 172 if (rowValue > maxValue) { 173 pivotRow = i; 174 maxValue = rowValue; 175 } 176 } 177 178 // swap rows 179 if (pivotRow != col) { 180 // swap elements in a matrix 181 for (i = 0; i < n; ++i) { 182 float temp; 183 SWAP(a[col][i], a[pivotRow][i]); 184 } 185 186 // swap elements in order vector 187 { 188 int temp; 189 SWAP(order[col], order[pivotRow]); 190 } 191 } 192 } 193 194 /* 195 Performs an inplace Crout lower/upper decomposition of n x n matrix A. 196 Values on or below diagonals are lowers, values about the 197 diagonal are uppers, with an implicit 1.0 value for the 198 diagonals. 199 Returns 0 upon success, 2 if matrix is singular. 200 201 Based on _Applied Numerical Analysis_, Fourth Edition, Gerald & Wheatley 202 Sections 2.5 & 2.14. 203 */ 204 int lu_decomp(float **a, int n, int *order) 205 { 206 int i; 207 float inverse; 208 int jCol; 209 int iRow; 210 int kCol; 211 float sum; 212 213 // initialize ordering vector 214 for (i = 0; i < n; ++i) 132 215 { 133 if(fMaxElem < fabs(pfMatr[i*nDim + k]) ) 134 { 135 fMaxElem = pfMatr[i*nDim + k]; 136 m = i; 137 } 138 } 139 140 // permutation of base line (index k) and max element line(index m) 141 if(m != k) 216 order[i] = i; 217 } 218 219 // pivot first column 220 pivot(a, n, order, 0); 221 222 // check for singularity 223 if (0.0 == a[0][0]) 142 224 { 143 for(i=k; i<nDim; i++) 144 { 145 fAcc = pfMatr[k*nDim + i]; 146 pfMatr[k*nDim + i] = pfMatr[m*nDim + i]; 147 pfMatr[m*nDim + i] = fAcc; 148 } 149 fAcc = pfVect[k]; 150 pfVect[k] = pfVect[m]; 151 pfVect[m] = fAcc; 152 } 153 154 if( pfMatr[k*nDim + k] == 0.) return 1; // needs improvement !!! 155 156 // triangulation of matrix with coefficients 157 for(j=(k+1); j<nDim; j++) // current row of matrix 158 { 159 fAcc =  pfMatr[j*nDim + k] / pfMatr[k*nDim + k]; 160 for(i=k; i<nDim; i++) 161 { 162 pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i]; 163 } 164 pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation 165 } 166 } 167 168 for(k=(nDim1); k>=0; k) 169 { 170 pfSolution[k] = pfVect[k]; 171 for(i=(k+1); i<nDim; i++) 172 { 173 pfSolution[k] = (pfMatr[k*nDim + i]*pfSolution[i]); 174 } 175 pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k]; 176 } 177 178 return 0; 225 return 2; 226 } 227 228 // compute first row of upper 229 inverse = 1.0 / a[0][0]; 230 for (i = 1; i < n; ++i) { 231 a[0][i] *= inverse; 232 } 233 234 // continue computing columns of lowers then rows of uppers 235 for (jCol = 1; jCol < n  1; ++jCol) { 236 // compute column of lowers 237 for (iRow = jCol; iRow < n; ++iRow) { 238 sum = 0.0; 239 for (kCol = 0; kCol < jCol; ++kCol) { 240 sum += a[iRow][kCol] * a[kCol][jCol]; 241 } 242 a[iRow][jCol] = sum; 243 } 244 245 // find pivot for row 246 pivot(a, n, order, jCol); 247 if (0.0 == a[jCol][jCol]) 248 { 249 return 2; 250 } 251 252 // build row of uppers 253 inverse = 1.0 / a[jCol][jCol]; 254 for (kCol = jCol + 1; kCol < n; ++kCol) { 255 sum = 0.0; 256 for (iRow = 0; iRow < jCol; ++iRow) { 257 sum += a[jCol][iRow] * a[iRow][kCol]; 258 } 259 a[jCol][kCol] = sum; 260 a[jCol][kCol] *= inverse; 261 } 262 } 263 264 // get remaining lower 265 sum = 0.0; 266 for (kCol = 0; kCol < n  1; ++kCol) { 267 sum += a[n  1][kCol] * a[kCol][n  1]; 268 } 269 a[n  1][n  1] = sum; 270 return 0; 271 } 272 273 /* 274 Given a LU decomposition of an n x n matrix A, and an order vector 275 specifying any reordering done during pivoting, solves the equation 276 Ax = b. Returns result in b. 277 */ 278 //TODO check for lu[i][i] != 0? 279 int solve_lu(float **lu, int n, float *b, int *order) 280 { 281 int startIndex; 282 int index; 283 float temp; 284 int iRow; 285 int jCol; 286 int nvbl; 287 float sum; 288 289 // rearrange the elements of the b vector in place. 290 startIndex = order[0]; 291 index = startIndex; 292 temp = b[index]; 293 while (1) { 294 int nextIndex = order[index]; 295 if (nextIndex == startIndex) { 296 b[index] = temp; 297 break; 298 } 299 b[index] = b[nextIndex]; 300 index = nextIndex; 301 } 302 303 // compute the b' vector 304 b[0] /= lu[0][0]; 305 for (iRow = 1; iRow < n; ++iRow) { 306 sum = 0.0; 307 int jCol; 308 for (jCol = 0; jCol < iRow; ++jCol) { 309 sum += lu[iRow][jCol] * b[jCol]; 310 } 311 b[iRow] = sum; 312 b[iRow] /= lu[iRow][iRow]; 313 } 314 315 // get the solution, we have b[n1] already 316 for (iRow = 1; iRow < n; ++iRow) { // iRow goes from 1 to n1 317 nvbl = n  iRow  1; // nvbl goes from n2 to 0 318 sum = 0.0f; 319 for (jCol = nvbl + 1; jCol < n; ++jCol) { 320 sum += lu[nvbl][jCol] * b[jCol]; 321 } 322 b[nvbl] = sum; 323 } 324 return 0; 325 } 326 327 /* Linear equation solution of Ax = b by lower/upper decomposition. 328 A is the n x n input max, b is the righthand side vector, length n. 329 On output, b is replaced by the corresponding set of solution vectors 330 and A is trashed. 331 */ 332 int GCI_solve_lu_decomp(float **a, int n, float *b) 333 { 334 int order[n]; 335 int return_value = lu_decomp(a, n, order); 336 if (return_value >= 0) { 337 return_value = solve_lu(a, n, b, order); 338 } 339 return return_value; 340 } 341 342 /* Matrix inversion by lower/upper decomposition. 343 A is the n x n input matrix. 344 On output, a is replaced by its matrix inverse.. 345 */ 346 int GCI_invert_lu_decomp(float **a, int n) 347 { 348 int returnValue; 349 int order[n]; 350 float identity[n][n]; 351 int i, j; 352 353 returnValue = lu_decomp(a, n, order); 354 if (returnValue >= 0) { 355 for (j = 0; j < n; ++j) { 356 // find inverse by columns 357 for (i = 0; i < n; ++i) { 358 identity[j][i] = 0.0; 359 } 360 identity[j][j] = 1.0; 361 solve_lu(a, n, identity[j], order); 362 } 363 for (j = 0; j < n; ++j) { 364 for (i = 0; i < n; ++i) { 365 a[j][i] = identity[j][i]; 366 } 367 } 368 } 369 return returnValue; 370 } 371 372 /* Linear equation solution of Ax = b.. 373 A is the n x n input max, b is the righthand side vector, length n. 374 On output, b is replaced by the corresponding set of solution vectors 375 and A is trashed. 376 */ 377 int GCI_solve(float **a, int n, float *b) 378 { 379 return GCI_solve_Gaussian(a, n, b); 380 //return GCI_solve_lu_decomp(a, n, b); 381 } 382 383 /* Matrix inversion. 384 A is the n x n input matrix. 385 On output, a is replaced by its matrix inverse.. 386 */ 387 int GCI_invert(float **a, int n) 388 { 389 return GCI_invert_Gaussian(a, n); 390 //return GCI_invert_lu_decomp(a, n); 179 391 } 180 392
Note: See TracChangeset
for help on using the changeset viewer.