Ignore:
Timestamp:
05/13/11 22:54:42 (9 years ago)
Author:
aivar
Message:

Changed GCI_gauss_jordan to be GCI_solve and GCI_invert. The current default CGI_solve uses Gaussian Elimination. Added inversion method. Added Lower/Upper Decomposition code for both solve and invert, but it is commented out.

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/projects/slim-curve/src/main/c/EcfUtil.c

    r7606 r7706  
    1313#include <stdlib.h> 
    1414#include <stdarg.h> 
     15#include <string.h> 
    1516#include <math.h> 
    1617#ifdef _CVI_ 
     
    2425/******************************************************************** 
    2526 
    26                            GAUSS-JORDAN AND COVAR-SORTING ROUTINES 
     27                           EQUATION-SOLVING AND COVAR-SORTING ROUTINES 
    2728 
    2829*********************************************************************/ 
    2930 
    30  
    31 /* Linear equation solution by Gauss-Jordan elimination. 
    32    a[0..n-1][0..n-1] is the input matrix, 
    33    b[0..n] is input containing the single right-hand side vector. 
    34    On output, a is replaced by its matrix inverse and b is replaced by 
    35    the corresponding set of solution vectors. */ 
    36  
    3731#define SWAP(a,b) { temp=(a); (a)=(b); (b)=temp; } 
    3832 
    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 right-hand side vector, length n. 
     35   On output, b is replaced by the corresponding set of solution vectors 
     36   and A is trashed. 
    4137 */ 
    42 int GCI_gauss_jordan(float **a, int n, float *b) 
     38int GCI_solve_Gaussian(float **a, int n, float *b) 
    4339{ 
    4440    float max; 
    45     float tmp; 
    46  
     41    float temp; 
     42    float pivotInverse[n]; 
    4743    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    } */ 
    4853 
    4954    // base row of matrix 
     
    6772            for (i = k; i < n; ++i) 
    6873            { 
    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]); 
    7275            } 
    73             tmp = b[k]; 
    74             b[k] = b[m]; 
    75             b[m] = tmp; 
     76            SWAP(b[k], b[m]); 
    7677        } 
    7778 
     
    8283 
    8384        // triangulation of matrix with coefficients 
     85        pivotInverse[k] = 1.0 / a[k][k]; 
    8486        for (j = k + 1; j < n; ++j) // current row of matrix 
    8587        { 
    86             tmp = -a[j][k] / a[k][k]; 
     88            // want "temp = -a[j][k] / a[k][k]" 
     89            temp = -a[j][k] * pivotInverse[k]; 
    8790            for (i = k; i < n; ++i) 
    8891            { 
    89                 a[j][i] += tmp * a[k][i]; 
     92                a[j][i] += temp * a[k][i]; 
    9093            } 
    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]; 
    9499 
    95100    for (k = n - 1; k >= 0; --k) 
     
    99104            b[k] -= a[k][i] * b[i]; 
    100105        } 
    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 
    103117    return 0; 
    104118} 
    105119 
    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<(nDim-1); 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 */ 
     125int 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 */ 
     160void 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 in-place 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 */ 
     204int 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) 
    132215    { 
    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]) 
    142224    { 
    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=(nDim-1); 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? 
     279int 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[n-1] already 
     316    for (iRow = 1; iRow < n; ++iRow) { // iRow goes from 1 to n-1 
     317        nvbl = n - iRow - 1;           // nvbl goes from n-2 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 right-hand side vector, length n. 
     329   On output, b is replaced by the corresponding set of solution vectors 
     330   and A is trashed. 
     331 */ 
     332int 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 */ 
     346int 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 right-hand side vector, length n. 
     374   On output, b is replaced by the corresponding set of solution vectors 
     375   and A is trashed. 
     376 */ 
     377int 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 */ 
     387int GCI_invert(float **a, int n) 
     388{ 
     389    return GCI_invert_Gaussian(a, n); 
     390    //return GCI_invert_lu_decomp(a, n); 
    179391} 
    180392 
Note: See TracChangeset for help on using the changeset viewer.