/* * Copyright 2002 by Soos, Antal * All rights reserved. Property of Soos, Antal. * Restricted rights to use, duplicate or disclose this code are * granted through contract. */ /***************************************************************************/ /* */ /* MatrixCal . cpp */ /* */ /* Basic C++ standard matrix calculations. */ /* */ /* */ /***************************************************************************/ #include "MatrixCal.hpp" // All the necessery definitions //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Dynamic float matrix alocation, initializad to zero // // Dimensions: rows(1,2...row},columns{1,2...col} // // Usage: Matrix x; // Matrix structure definition // matrix_alloc(&m, row, col); // row,col >= 1 // Dependences: - structure Matrix -> MatrixCal.hpp // - matrixa() -> MatrixCal.cpp //----------------------------------------------------------------------------- void matrix_alloc(Matrix *x, int row, int col) /*Allocate float zero matrix*/ { // *x is the adress of the matrix structure int ia,ib; x->mtx = matrixa(1,row,1,col); x->row = row, x->col = col; // during calculation - dimension test x->al_row = row, x->al_col = col; // for the delete process only // Initialization for 0 for (ia=1;ia<=row;ia++) for(ib=1;ib<=col;ib++) x->mtx[ia][ib] = (float)0.0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Dynamic float unity [dim x dim] matrix alocation // // Dimensions: rows(1,2...dim},columns{1,2...dim} // // Usage: Matrix x; // Matrix structure definition // eye_matrix_alloc(&m, dim); // dim >= 1 // Dependences: - structure Matrix -> MatrixCal.hpp // - matrixa() -> MatrixCal.cpp //----------------------------------------------------------------------------- void eye_matrix_alloc(Matrix *x, int dim) /*Allocate float unity Matrix*/ { // *x is the adress of the matrix structure int ia,ib; x->mtx = matrixa(1,dim,1,dim); x->row = dim, x->col = dim; x->al_row = dim, x->al_col = dim; // for the delete/test process // Initialization for 0 for(ia=1;ia<=dim;ia++) for(ib=1;ib<=dim;ib++) x->mtx[ia][ib] = (float)0.0; for (ia=1;ia<=dim;ia++) x->mtx[ia][ia] = (float) 1.0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Dynamic float matrix alocation // Alocate a float matrix with subscript range m[nrl,..nrh][ncl,..nch] // Dimensions: rows(nrl,2...nrh},columns{ncl,2...nch} // // Usage: float **x; // 2 dimension pointer definition // x = matrixa(nrl, nrh, ncl, nch); // where: nrl, ncl >= 0; nrh >= nrl; nch >= ncl // Dependences: - malloc() -> // - printef() -> //----------------------------------------------------------------------------- float **matrixa(int nrl, int nrh, int ncl, int nch) /*Alocate a float matrix*/ { int i, nrow=nrh-nrl+1, ncol=nch-ncl+1; float **m; //alocate pointers to rows: m=(float **) malloc((unsigned)((nrow+NR_END)*sizeof(float))); if(!m) printf("\n\tAllocation failure 1 in matrixa()\n"); m += NR_END; m -= nrl; //allocate rows and set pointers to them: m[nrl]=(float *) malloc((unsigned)((nrow*ncol+NR_END)*sizeof(float))); if(!m[nrl]) printf("\n\tAllocation failure 2 in matrixa()\n"); m[nrl] += NR_END; m[nrl] -= ncl; for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol; //return pointer to array of pointers to rows: return m; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Remouve the dynamically alocated float Matrix structure // This makes the memory space available again // // Usage: Matrix x; // Matrix structure definition // matrix_delete(&x); // Dependences: - structure Matrix -> MatrixCal.hpp // - free_matrixa() -> MatrixCal.cpp //----------------------------------------------------------------------------- void matrix_delete(Matrix *x) /*Remouve the alocated matrix*/ { free_matrixa(x->mtx,1,x->al_row,1,x->al_col); x->row = 0, x->col = 0; x->al_row = 0, x->al_col = 0; // hopfully it is dealocated by the system } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Dynamic float matrix dealocation // This makes the memory space available again, used in matrix_delete() // Free a float matrix allocated by matrix(): // Usage: float **x; // the ptiviously alocated matrix with matrixa() // x = matrixa(nrl, nrh, ncl, nch); // where: nrl, nrh, ncl, nch are the used dimensions // Dependences: - free() -> //----------------------------------------------------------------------------- void free_matrixa(float **m, int nrl, int nrh, int ncl, int nch) { free((FREE_ARG) (m[nrl]+ncl-NR_END)); free((FREE_ARG) (m+nrl-NR_END)); } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Print function for the float Matrix // During the development for tests if DEBUG_PRINT_ON == 1 // // Usage: Matrix x; // Matrix structure definition // print_matrix(&x,"mesage"); // Dependences: - printf() -> // - getch() -> //----------------------------------------------------------------------------- void print_matrix(Matrix *x,char s[80]) // usage : printmat(&x,"x"); { #if DEBUG_PRINT_ON /* Matrix print of it is debug */ #define MATLABF 1 #if MATLABF int i,k; printf("\n\n %s = [ ",s); x->al_row, x->row, x->al_col, x->col; for(k=1; k<= x->row; k++) { printf("\n"); for(i=1; i<= x->col; ++i)printf(" %9g ", x->mtx[k][i]); } printf("]\n"); #else int i,k; //int ch; printf("\n\n %s - MATRIX\n",s); printf("rowMax = %d >= %d, colMax = %d >= %d\n", x->al_row, x->row, x->al_col, x->col); for(k=1; k<= x->row; k++) { printf("\n|"); for(i=1; i<= x->col; ++i)printf(" %9g ", x->mtx[k][i]); printf(" |"); } printf("\n"); //putchar('\a'); //while(ch = getchar() != 0) ; // ch = 0; //cout << ch; //while(ch == 0) { scanf(" %i ", &ch); } //scanf(" %i ", &ch); // pause(); //scanf("%i",ch); //cout << ch; //getch(); // Not working on the DSP TMS320C6711 // getchar(); // Not working on the DSP TMS320C6711 //printf("/n Test /n "); #endif #endif // DEBUG_PRINT_ON } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Print function for mesages // During the development for tests if DEBUG_PRINT_ON == 1 // // Usage: printlog("mesage"); // Dependences: - printf() -> //----------------------------------------------------------------------------- void printlog(char s[80]) // usage : printlog("mesage"); { #if DEBUG_PRINT_ON /* Matrix print only for PC */ printf("\n %s\n",s); #endif // print debug } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Error handling function for catastrofic/panic situation // The error number is selected during the development for backtrace // >>> IT IS A TRAP <<<, exit only by the restart // Usage: sg_error(num); //where the num the error index // Dependences: - printf() -> //----------------------------------------------------------------------------- void sg_error(int x) /* Error handling function */ { while(1){ /* PANIC !!! */ printf("\n Error = %d\n",x); } /* Idle until reset */ } //----------------------------------------------------------------------------- void sg_warning(int x) /* Warning handling function */ { printf("\n Warning = %d\n",x); // single warning mesage! } //----------------------------------------------------------------------------- //***************************************************************************** //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Addition of two matricess: r = x + y // // Dimensions: r,x,y must hawe same dimension // // Usage: Matrix x; // Matrix structure definition // addmat(&r, &x, &y); // Dependences: - structure Matrix -> MatrixCal.hpp // - sg_error() -> MatrixCal.cpp //----------------------------------------------------------------------------- void addmat(Matrix *r, Matrix *x, Matrix *y) /* Addition of two matrices */ { /* Calculate r = x + y */ int i,j,ii,jj ; if((x->col != y->col)||(x->row != y->row)) sg_error(DIMENSION_MISMACH); r->row =ii= x->row; r->col =jj= x->col; for (j=1; j<= jj; j++) for(i=1;i<= ii; i++) r->mtx[i][j] = x->mtx[i][j]+ y->mtx[i][j]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Substraction of two matricess: r = x - y // // Dimensions: r,x,y must hawe same dimension // // Usage: Matrix x; // Matrix structure definition // submat(&r, &x, &y); // Dependences: - structure Matrix -> MatrixCal.hpp // - sg_error() -> MatrixCal.cpp //----------------------------------------------------------------------------- void submat(Matrix *r, Matrix *x, Matrix *y) /* Substraction of two matrices */ { /* Calculate r = x - y */ int i,j,ii,jj ; if((x->col != y->col)||(x->row != y->row)) sg_error(DIMENSION_MISMACH); r->row =jj= x->row; r->col = ii= x->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[i][j] = x->mtx[i][j] - y->mtx[i][j]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Multiplixation of two matricess: r = x * y // // Dimensions: r[x_row,y_col] = x[x_row,x_col] * y[y_row,y_col] // where: x_col = y_row // Usage: Matrix x; // Matrix structure definition // mulmat(&r, &x, &y); // Dependences: - structure Matrix -> MatrixCal.hpp // - sg_error() -> MatrixCal.cpp //----------------------------------------------------------------------------- void mulmat(Matrix *r, Matrix *x, Matrix *y) /*Multiplixation of two matrices*/ { /* Calculate r = x * y */ int i,j,k ,ii,jj,kk; if(x->col != y->row) sg_error(1010); r->row =ii= x->row; r->col =jj= y->col; kk=x->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) { r->mtx[i][j] = 0; for(k=1;k <= kk; k++) { r->mtx[i][j] += x->mtx[i][k] * y->mtx[k][j] ;} } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Multiplixation of two matricess: r = x' * y (whith transpose first) // // Dimensions: r[x_row,y_col] = x[x_col,x_row] * y[y_row,y_col] // where: x_row = y_row // Usage: Matrix x; // Matrix structure definition // mulmat_t1(&r, &xT, &y); // Dependences: - structure Matrix -> MatrixCal.hpp // - sg_error() -> MatrixCal.cpp //----------------------------------------------------------------------------- void mulmat_t1(Matrix *r, Matrix *x, Matrix *y) /*Multiplixation two matrices*/ { /* Calculate r = x' * y */ int i,j,k ,ii,jj,kk; if(x->row != y->row) nrerror("mulmat_t1 "); r->row =ii= x->col; r->col =jj= y->col; kk=x->row; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) { r->mtx[i][j] = 0; for(k=1;k <= kk; k++) { r->mtx[i][j] += x->mtx[k][i] * y->mtx[k][j] ;} } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Multiplixation of two matricess: r = x * y' (whith transpose first) // // Dimensions: r[x_row,y_row] = x[x_row,x_col] * y[y_col,y_row] // where: x_col = y_col // Usage: Matrix x; // Matrix structure definition // mulmat_t2(&r, &xT, &y); // Dependences: - structure Matrix -> MatrixCal.hpp // - sg_error() -> MatrixCal.cpp //----------------------------------------------------------------------------- void mulmat_t2(Matrix *r, Matrix *x, Matrix *y) /*Multiplixation two matrices*/ { /* Calculate r = x * y' */ int i,j,k ,ii,jj,kk; if(x->col != y->col) nrerror("mulmat_t2 "); // sg_error(11); r->row =ii= x->row; r->col =jj= y->row; kk=x->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) { r->mtx[i][j] = 0; for(k=1;k <= kk; k++) { r->mtx[i][j] += x->mtx[i][k] * y->mtx[j][k] ;} } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Multiplication of a matrix whith constanr: r = variable * x // // Dimensions: r will become the x dimension // // Usage: Matrix x; // Matrix structure definition // cmulmat(&r, variable, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void cmulmat(Matrix *r, float a, Matrix *x) /* Multiplixation with constant */ { /* Calculate r = a * x */ int i,j ; r->row = x->row; r->col = x->col; for ( j=1; j<= r->col; j++) for( i=1;i<= r->row; i++) r->mtx[i][j] = a * x->mtx[i][j]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Matrix inverse calculation r = x^(-1) // if returns =/= 0 --> eror // Dimensions: r will become the x dimension [n,n] // Input: x, and y (y is a temporary matrix) // Output r the inverse of the x matrix // Usage: Matrix x; // Matrix structure definition // cmulmat(&r, &x, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- #define MXED 10 int minv(Matrix *r, Matrix *x) /* Matrix inverse calculation */ { /* Calculate r = inv(x) */ int jj,i,j ,itemp ; float det; // int *indx; // float *col; //float **cx; // cx stores the x matrix for calculation // allocate the temporary matrix for calculation int indx[MXED]; //= ivector(1,jj); float col[MXED]; //= vectora(1,jj); float cx[MXED*MXED]; //= matrixa(1,jj,1,jj); if(x->row != x->col) sg_error(160); jj = r->row = x->row; r->col = jj; if(jj == 1) // if the matrix is scalar the inverse is calculated as: { det = x->mtx[1][1]; if(fabs(det) < POSITIV_ZERO) { sg_warning(161); r->mtx[1][1] = HUGE_NUM; } else r->mtx[1][1] = 1/x->mtx[1][1]; } else if(jj==2)// for the two dimensional matrix the inverse is defined: { det = (x->mtx[1][1] * x->mtx[2][2]) - (x->mtx[1][2] * x->mtx[2][1]); if(fabs(det) < POSITIV_ZERO) { sg_warning(162); for (j=1;j<=jj;j++) r->mtx[j][j] = HUGE_NUM; } else { r->mtx[1][1] = x->mtx[2][2] / det; r->mtx[1][2] = -x->mtx[1][2] / det; r->mtx[2][1] = -x->mtx[2][1] / det; r->mtx[2][2] = x->mtx[1][1] / det; } } else // inverse algorithm here (if dimension > 2) -->>> { //Inverse of a Matrix dimension > 2 //Using the LU decomposition and backsubstitution routines, it is completely //straightforward to find the inverse of a matrix column by column. //indx = ivector(1,jj); //col = vectora(1,jj); //cx = matrixa(1,jj,1,jj); // allocate the temporary matrix for calculation for(i=1;i<=jj;i++)for(j=1;j<=jj;j++) cx[(i*MXED)+j] = x->mtx[i][j]; itemp = ludcmp(cx,jj,indx,&det); //Decompose the matrix just once. // determinant test: //for(j=1;j<=jj;j++) det *= cx[j][j]; // calculate the determinant if(itemp) { sg_warning(162); for (j=1;j<=jj;j++) r->mtx[j][j] = HUGE_NUM; } else { for(j=1;j<=jj;j++) { //Find inverse by columns. for(i=1;i<=jj;i++) col[i]=0.0; col[j]=1.0; lubksb(cx,jj,indx,col); for(i=1;i<=jj;i++) r->mtx[i][j]=col[i]; } } //The matrix y will now contain the inverse of the original matrix a, which will have //been destroyed. Alternatively, there is nothing wrong with using a Gauss-Jordan //routine like gaussj (x2.1) to invert a matrix in place, again destroying the original. //Both methods have practically the same operations count. //NRinC: Numerical Recipes in C - The Art of Scientific Computing * Second Edition //free_matrixa(cx,1,jj,1,jj); //free_vector(col,1,jj); //free_ivector(indx,1,jj); } // <<--- inverse algorithm here (if dimension > 2) } // End of matrix inversion algorithm //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // the LU decomposition - for matrix inversion //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int ludcmp(float *a, int n, int *indx, float *d) //Given a matrix a[1..n][1..n], this routine replaces it by the LU decomposition of a rowwise //permutation of itself. // a and n are input. a is output, arranged as in equation (2.3.14) NRinC; //indx[1..n] is an output vector that records the row permutation effected by the partial //pivoting; d is output as +/- 1 depending on whether the number of row interchanges was even //or odd, respectively. //This routine is used in combination with lubksb to solve linear equations or invert a matrix. { int i,imax,j,k; float big,dum,sum,temp; // float *vv; // vv stores the implicit scaling of each row. float vv[MXED]; // vv=vectora(1,n); *d=1.0; //No row interchanges yet. for (i=1;i<=n;i++) { //Loop over rows to get the implicit scaling information. big=0.0; for (j=1;j<=n;j++) if ((temp=fabs(a[(i*MXED)+j])) > big) big=temp; if (big == 0.0) return 1 ;//nrerror("Singular matrix in routine ludcmp"); //No nonzero largest element. vv[i]=1.0/big; //Save the scaling. } for (j=1;j<=n;j++) { //This is the loop over columns of Crout's method. for (i=1;i= big) { //Is the gure of merit for the pivot better than the best so far? big=dum; imax=i; } } if (j != imax) { //Do we need to interchange rows? for (k=1;k<=n;k++) { //Yes, do so... dum=a[(imax*MXED)+k]; a[(imax*MXED)+k]=a[(j*MXED)+k]; a[(j*MXED)+k]=dum; } *d = -(*d); //and change the parity of d. vv[imax]=vv[j]; //Also interchange the scale factor. } indx[j]=imax; if (a[(j*MXED)+j] == 0.0) a[(j*MXED)+j]=TINY_NUM; //If the pivot element is zero the matrix is singular (at least to the precision of the //algorithm). For some applications on singular matrices, it is desirable to substitute //TINY for zero. if (j != n) { //Now, finally, divide by the pivot element. dum=1.0/(a[(j*MXED)+j]); for (i=j+1;i<=n;i++) a[(i*MXED)+j] *= dum; } } //Go back for the next column in the reduction. // free_vector(vv,1,n); return 0; } // NRinC: Numerical Recipes in C - The Art of Scientific Computing * Second Edition //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // Solves the set of n linear equations A X = B - for matrix inversion //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void lubksb(float *a, int n, int *indx, float b[]) //Solves the set of n linear equations A X = B. //Here a[1..n][1..n] is input, not as the matrix A but rather as its LU decomposition, //determined by the routine ludcmp. //indx[1..n] is input as the permutation vector returned by ludcmp. //b[1..n] is input as the right-hand side vector B, //and returns with the solution vector X. a, n, and indx are not modified by this routine //and can be left in place for successive calls with different right-hand sides b. //This routine takes into account the possibility that b will begin with many zero elements, //so it is efficient for use in matrix inversion. { int i,ii=0,ip,j; float sum; for (i=1;i<=n;i++) { //When ii is set to a positive value, it will become the ip=indx[i]; //index of the first nonvanishing element of b. Wenow sum=b[ip]; // do the forward substitution, equation (2.3.6) NRinC. The b[ip]=b[i]; //only new wrinkle is to unscramble the permutation if (ii) //as we go. for (j=ii;j<=i-1;j++) sum -= a[(i*MXED)+j]*b[j]; else if (sum) ii=i; // A nonzero element was encountered, so from now on we b[i]=sum; //will have to do the sums in the loop above. } for (i=n;i>=1;i--) { //Now we do the backsubstitution, equation (2.3.7)NRinC. sum=b[i]; for (j=i+1;j<=n;j++) sum -= a[(i*MXED)+j]*b[j]; b[i]=sum/a[(i*MXED)+i]; //Store a component of the solution vector X. } // All done! } // NRinC: Numerical Recipes in C - The Art of Scientific Computing * Second Edition //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Transpose of a matrix : r = x' // // Dimensions: r will become the transpose of x dimension // // Usage: Matrix x; // Matrix structure definition // transpose(&r, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void transpose(Matrix *r, Matrix *x ) /* Transpose of matrice */ { /* Calculate r = x^T */ int i,j,ii,jj ; r->row = jj= x->col; r->col = ii= x->row; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[j][i] = x->mtx[i][j]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Square of a matrix : r = x'*I*x // // Dimensions: r will become the x dimension // // Usage: Matrix x; // Matrix structure definition // squaremat(&r, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void squaremat(Matrix *r, Matrix *x) /* Power of 2 of a matrix */ { /* Calculate r = x^T * I * x = x^2 by element*/ int i,j,ii,jj ; r->row = jj= x->row; r->col = ii= x->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[j][i] = x->mtx[j][i] * x->mtx[j][i]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Square Root of a matrix : r = x^(0.5) by the elements // // Dimensions: r will become the x dimension // // Usage: Matrix x; // Matrix structure definition // sqrtmat(&r, &y); // Dependences: - structure Matrix -> MatrixCal.hpp // //----------------------------------------------------------------------------- void sqrtmat(Matrix *r, Matrix *x) /* sqrt of a matrix */ { /* Calculate r = x^(1/2) by element*/ int i,j,ii,jj ; r->row = jj= x->row; r->col = ii= x->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[j][i] = sqrt(x->mtx[j][i]); } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Zero the matrix : r = 0 // // Dimensions: r will become the r dimension // // Usage: Matrix r; // Matrix structure definition // zeromat(&r); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void zeromat(Matrix *r) /* zeros the matrix */ { /* Calculate r by element*/ int i,j,ii,jj ; jj= r->row; ii= r->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[j][i] = (float)0.0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Unity square matrix : r = 1 if i==j, else 0 // // Dimensions: r will become the r dimension // // Usage: Matrix r; // Matrix structure definition // eyemat(&r); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void eyemat(Matrix *r) /* unity matrix */ { /* Calculate r by element*/ int i,j,ii,jj ; jj= r->row; ii= r->col; if(jj!=ii) sg_error(44); for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[j][i] = (float)0.0; for (j=1; j<= jj; j++) r->mtx[j][j] = (float)1.0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Diagonal square matrix : r{i,j) = x if i==j, else 0 [n x n] // // Dimensions: r will become the n dimension matrix // // Usage: Matrix r; // Matrix structure definition // diagn(&r,n,x); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void diagn(Matrix *r,int n,float x) /*Diagonal matrix [n x n]*/ { /* Calculate r by element*/ int i,j,ii,jj ; r->row = n; r->col = n; for ( j=1; j<= n; j++) for( i=1;i<= n; i++) r->mtx[j][i] = (float)0.0; for (j=1; j<= n; j++) r->mtx[j][j] = x; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Maximum matrix element selection: r = max{r,x} elements // // Dimensions: rof r and x are same // // Usage: Matrix x; // Matrix structure definition // maxmatel(&r, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void maxmatel(Matrix *r, Matrix *x) /* Maximum matrix element selection */ { /* Calculate r by element*/ int i,j,ii,jj ; jj= r->row; ii= r->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) if( r->mtx[j][i] < x->mtx[j][i]) r->mtx[j][i] = x->mtx[j][i]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Minimum matrix element selection: r = min{r,x} elements // // Dimensions: of r and x are same // // Usage: Matrix x; // Matrix structure definition // minmatel(&r, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void minmatel(Matrix *r, Matrix *x) /* Minimum matrix element selection */ { /* Calculate r by element*/ int i,j,ii,jj ; jj= r->row; ii= r->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) if( r->mtx[j][i] > x->mtx[j][i]) r->mtx[j][i] = x->mtx[j][i]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Cop[y matrix x to r: r = x // // Dimensions: of x are becom the dimension of r // // Usage: Matrix x; // Matrix structure definition // copymat(&r, &y); // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void copymat(Matrix *r, Matrix *x) /* Copy matrix x to r:: r=x; */ { /* Calculate r by element*/ int i,j,ii,jj ; jj= r->row = x->row; ii= r->col = x->col; for ( j=1; j<= jj; j++) for( i=1;i<= ii; i++) r->mtx[j][i] = x->mtx[j][i]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Concatenation matrix x and y: r = [x y] // // Dimensions: of x:[n,q], y:[n,p] are becom the dimension of r:[n,q+p] // // Usage: Matrix x; // Matrix structure definition // concat_row(&r, &x, &y); /* r = [x y] Concatenation*/ // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void concat_row(Matrix *r,Matrix *x,Matrix *y)/*Concatenation of row matrices*/ { /* Calculate r by element*/ int i,j,ii1,jj, ii2; jj = r->row = x->row; ii1 = x->col; ii2 = y->col; r->col = ii1 + ii2; if(x->row != y->row) sg_error(14); for ( j=1; j<= jj; j++) { for( i=1;i<= ii1; i++) r->mtx[j][i] = x->mtx[j][i]; for( i=1;i<= ii2; i++) r->mtx[j][ii1+i] = y->mtx[j][i]; } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Concatenation matrix x and y: r = [x ; y] by colum // // Dimensions: of x:[q,n], y:[p,n] are becom the dimension of r:[q+p,n] // // Usage: Matrix x; // Matrix structure definition // concat_col(&r, &x, &y); /* r = [x; y] Concatenation*/ // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void concat_col(Matrix *r,Matrix *x,Matrix *y)/*Concatenation of col matrices*/ { /* Calculate r by element*/ int i,j,ii,jj1,jj2 ; jj1 = x->row; jj2 = y->row; ii = r->col = x->col; r->row = jj1 + jj2; if(x->col != y->col) sg_error(15); for( i=1;i<= ii; i++) { for ( j=1; j<= jj1; j++) r->mtx[j][i] = x->mtx[j][i]; for ( j=1; j<= jj2; j++) r->mtx[jj1+j][i] = y->mtx[j][i]; } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Extract the matrix x rows: r = x[x(rs,:),,,x(re,:)] by rows // // Dimensions: of x:[q,n], the dimension of r:[re-rs,n] // where rs >= 1 and re =< q // // Usage: Matrix x; // Matrix structure definition // extract_row(&r,rs,re, &y); /* r = x[re-rs,n] extracts */ // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void extract_row(Matrix *r,int rs,int re,Matrix *x) /*extract rs to re rows*/ { int i,j,ii,jj ; ii = r->col = x->col; jj = r->row = re - (rs-1); if(x->row < jj) sg_error(16); for( i=1;i<= ii; i++) for ( j=1; j<= jj; j++) r->mtx[j][i] = x->mtx[j+(rs-1)][i]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Extract the matrix x coloms: r = x[x(:,cs),,,x(:,re)] by rows // // Dimensions: of x:[n,q], the dimension of r:[n,ce-cs] // where cs >= 1 and ce =< q // // Usage: Matrix x; // Matrix structure definition // extract_col(&r,rs,re, &y); /* r = x[n,ce-cs] extracts */ // Dependences: - structure Matrix -> MatrixCal.hpp //----------------------------------------------------------------------------- void extract_col(Matrix *r,int cs,int ce,Matrix *x) /*extract cs to ce coloms*/ { int i,j,ii,jj ; ii = r->col = ce - (cs-1); jj = r->row = x->row; if(x->col < ii) sg_error(16); for( i=1;i<= ii; i++) for ( j=1; j<= jj; j++) r->mtx[j][i] = x->mtx[j][i+(cs-1)]; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Frobenius norm: Norm F of a matrix : r = ||x||_F // if: x :: R^(m x n) then ||x||_2 =< ||x||_F =< sqrt(n)*||x||_2 // Dimensions: [row x col] // // Usage: Matrix x; // Matrix structure definition // r = frobenius_norm( &x ); // Dependences: - structure Matrix -> MatrixCal.hpp // - sqrt() -> //----------------------------------------------------------------------------- float frobenius_norm(Matrix *x) /* NormF of a matrice or a vector */ { int i,j,ii,jj ; float r; r=0; ii = x->row; jj = x->col; for(j=1; j<=jj; j++) for(i=1;i<= ii; i++) r += x->mtx[i][j]*x->mtx[i][j]; r = sqrt(r); return r; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Covariance calculation for the vector X{[1]*[N]} // if: x :: R^(m x n) then ||x||_2 =< ||x||_F =< sqrt(n)*||x||_2 // Dimensions: [row x col] // // Usage: Matrix x; // Matrix structure definition // r = frobenius_norm( &x ); // Dependences: - structure Matrix -> MatrixCal.hpp // - sqrt() -> //----------------------------------------------------------------------------- float covar(Matrix *x) /* Covariance for the vector X{[1]*[N]} */ { int i,n ; float nf; /* the float lengt of x */ float s; /* sum of arry */ float xav; /* The average of x vector */ float r; n = x->col; nf = (float)n; /* Calculation the average of the vector x */ s=(float)0.0; for( i=1;i<= n; i++) s += x->mtx[1][i]; xav = s/nf; /* Calculate the kovariance of the vector x */ s=(float)0.0; for( i=1;i<= n; i++) { r = x->mtx[1][i] - xav ; s += r*r; } r = s/(nf-1); return r; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Random number generation random_1 // // // // Usage: ling int idnum // rand = random_1(&idum); // Dependences: - //----------------------------------------------------------------------------- #define IA 16807 #define IM 2147483647 #define AM 4.656612875245796924105750827168e-10 // (1.0/IM) #define IQ 127773 #define IR 2836 #define NTAB 32 #define NDIV 67108864.9375 // (1+(IM-1)/NTAB) #define EPS 1.2e-7 #define RNMX 0.99999988 // (1.0-EPS) float random_1(long int *idum) /* random number generation =< (+/- 1) */ { int j; long int k; static long int iy=0; static long int iv[NTAB]; float temp; if (*idum <= 0 || !iy) { //Initialize. if (-(*idum) < 1) *idum = *idum+1; //Be sure to prevent idum = 0. else *idum = -(*idum); for (j=NTAB+7;j>=0;j--) { //Load the shu.e table (after 8 warm-ups) k=(*idum)/IQ; *idum=IA*(*idum-k*IQ)-IR*k; if (*idum < 0) *idum += IM; if (j < NTAB) iv[j] = *idum; } iy=iv[0]; } k=(*idum)/IQ; //Start here when not initializing. *idum=IA*(*idum-k*IQ)-IR*k; //Compute idum=(IA*idum) % IM without if (*idum < 0) *idum += IM; // overflows by Schrage's method. temp = iy/NDIV; // Will be in the range 0..NTAB-1. //j=iy/NDIV; // Will be in the range 0..NTAB-1. j = (int)temp; iy=iv[j]; //Output previously stored value and re ll the iv[j] = *idum; // shu.e table. if ((temp=AM*iy) > RNMX) return (float) RNMX; //Because users don't expect else return ((float)(2.0*temp)-1.0); //::(-1 < r < 1) endpoint values. } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- #if 0 //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Computes all eigenvalues and eigenvectors of a real symmetric matrix // for the matrices up to [10x10] // // // Usage: jacobi((&rd, &rv, &x); // x input symetrical matix // rd eigenvalues in vector rd[1..n][1] // rv eigenvectors in matrix v[1..n][1..n] // // Dependences: - //----------------------------------------------------------------------------- #define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);a[k][l]=h+s*(g-h*tau); void jacobi(Matrix *rd,Matrix *rv,Matrix *x) //void jacobi(float **a, int n, float d[], float **v, int *nrot) /*Computes all eigenvalues and eigenvectors of a real symmetric matrix a[1..n][1..n]. On output, elements of a above the diagonal are not destroyed. d[1..n] returns the eigenvalues of a. v[1..n][1..n] is a matrix whose columns contain, on output, the normalized eigenvectors of a. nrot returns the number of Jacobi rotations that were required.*/ { int j,iq,ip,i; float tresh,theta,tau,t,sm,s,h,g,c,*b,*z; float **a, **v; float *d; int n, nrotm, *nrot; nrot = &nrotm; n=x->col; b=vector(1,n); //alocate b vector [1 x n] z=vector(1,n); //alocate z vector [1 x n] a=matrix(1,n,1,n); // Input simetric matrix v=matrix(1,n,1,n); // output eigenvalue vectors (matrix) d=vector(1,n); // output eigenvalue vector for(j=1; j<=n; j++) for(i=1;i<= n; i++) a[i][j]= x->mtx[i][j]; // Original code ->> for (ip=1;ip<=n;ip++) { //Initialize to the identity matrix. for (iq=1;iq<=n;iq++) v[ip][iq]=0.0; v[ip][ip]=1.0; } for (ip=1;ip<=n;ip++) { //Initialize b and d to the diagonal b[ip]=d[ip]=a[ip][ip]; //of a. z[ip]=0.0; //This vector will accumulate terms } //of the form tapq as in equation (11.1.14) *nrot=0; for (i=1;i<=50;i++) { sm=0.0; for (ip=1;ip<=n-1;ip++) { //Sum odiagonal elements. for (iq=ip+1;iq<=n;iq++) sm += fabs(a[ip][iq]); } if (sm == 0.0) { //The normal return, which relies free_vector(z,1,n); //on quadratic convergence to free_vector(b,1,n); // machine underflow. // reyurn proccess: for(j=1; j<=n; j++) for(i=1;i<= n; i++) rv->mtx[i][j] = v[i][j]; rv->col = n, rv->row = n; for(i=1;i<= n; i++) rd->mtx[i][1] = d[i]; rd->col = 1, rd->row = n; return; } if (i < 4) tresh=0.2*sm/(n*n); // ...on the rst three sweeps. else tresh=0.0; //...thereafter. for (ip=1;ip<=n-1;ip++) { for (iq=ip+1;iq<=n;iq++) { g=100.0*fabs(a[ip][iq]); // After four sweeps, skip the rotation if the off-diagonal element is small. if (i > 4 && (float)(fabs(d[ip])+g) == (float)fabs(d[ip]) && (float)(fabs(d[iq])+g) == (float)fabs(d[iq])) a[ip][iq]=0.0; else if (fabs(a[ip][iq]) > tresh) { h=d[iq]-d[ip]; if ((float)(fabs(h)+g) == (float)fabs(h)) t=(a[ip][iq])/h; //t = 1=(2) else { theta=0.5*h/(a[ip][iq]); //Equation (11.1.10). t=1.0/(fabs(theta)+sqrt(1.0+theta*theta)); if (theta < 0.0) t = -t; } c=1.0/sqrt(1+t*t); s=t*c; tau=s/(1.0+c); h=t*a[ip][iq]; z[ip] -= h; z[iq] += h; d[ip] -= h; d[iq] += h; a[ip][iq]=0.0; for (j=1;j<=ip-1;j++) { //Case of rotations 1  j < p. ROTATE(a,j,ip,j,iq) } for (j=ip+1;j<=iq-1;j++) { //Case of rotations p < j < q. ROTATE(a,ip,j,j,iq) } for (j=iq+1;j<=n;j++) { //Case of rotations q < j n. ROTATE(a,ip,j,iq,j) } for (j=1;j<=n;j++) { ROTATE(v,j,ip,j,iq) } ++(*nrot); } } } for (ip=1;ip<=n;ip++) { b[ip] += z[ip]; d[ip]=b[ip]; //Update d with the sum of tapq, z[ip]=0.0; //and reinitialize z. } } nrerror("Too many iterations in routine jacobi"); } #endif // ---------------------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Positivity test for a square matrix // if the matrix x is positiv: ptest(x)=1 // else if the matrix is negative or zero: ptest(x)=0 // T H E I N P U T M A T R I X I S D E S T R O J E D !!!!!!! // Usage: Matrix r; // Matrix structure definition // int k = ptest(&x); // Dependences: - structure Matrix -> MatrixCal.cpp //----------------------------------------------------------------------------- int ptest(Matrix *x) /* matrix positivity test */ { // const float flim = 1e-33; const float flim = -1e-3; int m; int i; int j; int k; int r; float sum1; float sum2; m=x->col; r=1; for(j=1; j<=m; j++){ for(i=1; i<=j; i++){ sum1=0; for(k=1; kmtx[i][k] * x->mtx[k][j]; x->mtx[i][j] = x->mtx[i][j] - sum1; } if(x->mtx[j][j] < flim) { r=0, j=m;} else { for(i=j+1; i<=m; i++){ sum2 = 0; for(k=1; kmtx[i][k] * x->mtx[k][j]; x->mtx[i][j] = (x->mtx[i][j] - sum2)/x->mtx[j][j]; } } } /* function [r] = ptest(A) %return r=1 if 'a' is a P-matrix (r=0 othervise) EPS = 1e-26; n=length(A); r=1; for j = 1:n for i = 1:j sum1=0; for k=1:(i-1) sum1=sum1 + (A(i,k)*A(k,j)); end A(i,j)=A(i,j) - sum1; end if A(j,j) <= EPS r = 0; j = n; else for i = (j+1):n sum2=0; for k=1:(j-1) sum2=sum2 + (A(i,k)*A(k,j)); end A(i,j) = (A(i,j) - sum2) / A(j,j); end end end */ return r; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // Dynamic float vector alocation, initializad to zero // // Dimensions: rows(1,2...row) // // Usage: float *v; // vector pointer definition // v = vectora( 1, row); // row >= 1 // Dependences: - vectora() -> MatrixCal.cpp //----------------------------------------------------------------------------- float *vectora(int nl, int nh) { /* allocate a float vector with subscript range v[nl,...nh] */ float *v; v=(float *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float))); if (!v) printf("allocation failure in vector()"); for(int i=nl; i MatrixCal.cpp ////----------------------------------------------------------------------------- //void free_vector(float *v, long nl, long nh) //{ /* free a float vector allocated with vectora() */ // // free((FREE_ARG) (v+nl-NR_END)); //} ////----------------------------------------------------------------------------- ///----------------------------------------------------------------------------- // // signum function from matlab // // // Usage: float s,x; // s=sign(x); // Dependences: -> MatrixCal.cpp //----------------------------------------------------------------------------- float sign(float x) /* return the signum of x:(-1,0,1) */ { /* free a float vector allocated with vectora() */ if( x > POSITIV_ZERO ) return 1.0; else if ( x < NEGATIV_ZERO ) return -1.0; else return 0.0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // And here is nrutil.c:: #define NR_END 1 #define FREE_ARG char* void nrerror(char error_text[]) /* Numerical Recipes standard error handler */ { fprintf(stderr,"Numerical Recipes run-time error...\n"); fprintf(stderr,"%s\n",error_text); fprintf(stderr,"...now exiting to system...\n"); exit(1); } float *vector(long nl, long nh) /* allocate a float vector with subscript range v[nl..nh] */ { float *v; v=(float *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(float))); if (!v) nrerror("allocation failure in vector()"); return v-nl+NR_END; } float **matrix(long nrl, long nrh, long ncl, long nch) /* allocate a float matrix with subscript range m[nrl..nrh][ncl..nch] */ { long i, nrow=nrh-nrl+1,ncol=nch-ncl+1; float **m; /* allocate pointers to rows */ m=(float **) malloc((size_t)((nrow+NR_END)*sizeof(float*))); if (!m) nrerror("allocation failure 1 in matrix()"); m += NR_END; m -= nrl; /* allocate rows and set pointers to them */ m[nrl]=(float *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(float))); if (!m[nrl]) nrerror("allocation failure 2 in matrix()"); m[nrl] += NR_END; m[nrl] -= ncl; for(i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol; /* return pointer to array of pointers to rows */ return m; } void free_matrix(float **m, long nrl, long nrh, long ncl, long nch) /* free a float matrix allocated by matrix() */ { free((FREE_ARG) (m[nrl]+ncl-NR_END)); free((FREE_ARG) (m+nrl-NR_END)); } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- //-----------------------------------------------------------------------------