/* * Copyright 2002,2004 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 //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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"); { int i,k; float ftemp1; LOG_printf(&trace, "\n \t %s ::",s); LOG_printf(&trace, "\t\t <- [%d x %d]::",x->row,x->col); for(k=1; k<= x->col; k++) { LOG_printf(&trace, "\n"); for(i=1; i<= x->row; ++i) { ftemp1 = x->mtx[i+(k-1)*x->row]; //k->row, i->col LOG_printf(&trace, " %9g ", ftemp1); } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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"); { LOG_printf(&trace, "\n %s\n",s); } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 // - printlog() -> MatrixCal.cpp //----------------------------------------------------------------------------- void addmat(Matrix *r, Matrix *x, Matrix *y) /* Addition of two matrices */ { /* Calculate r = x + y */ int i,ii ; if((x->col != y->col)||(x->row != y->row)) printlog("DIMENSION_MISMACH in addmat"); r->row = x->row; r->col = x->col; ii = x->row * x->col; for (i=1; i<= ii; i++) { r->mtx[i] = x->mtx[i]+ y->mtx[i]; } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 // - printlog() -> MatrixCal.cpp //----------------------------------------------------------------------------- void submat(Matrix *r, Matrix *x, Matrix *y) /* Substraction of two matrices */ { /* Calculate r = x - y */ int i,ii; if((x->col != y->col)||(x->row != y->row)) printlog("DIMENSION_MISMACH in submat"); r->row = x->row; r->col = x->col; ii = x->row * x->col; for( i=1;i<= ii; i++) { r->mtx[i] = x->mtx[i] - y->mtx[i]; } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 // - printlog() -> 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) printlog("DIMENSION_MISMACH in mulmat"); jj = y->col; r->col =jj; ii = x->row; r->row =ii; kk = x->col; for ( j=1; j<= jj; j++) { for( i=1;i<= ii; i++) { r->mtx[i+((j-1)*ii)] = 0; for(k=1;k <= kk; k++) { r->mtx[i+((j-1)*ii)] += x->mtx[i+((k-1)*ii)] * y->mtx[k+((j-1)*kk)] ; } } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 // - printlog() -> 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) printlog("DIMENSION_MISMACH in mulmat_t1"); jj = y->col; r->col =jj; ii = x->col; r->row =ii; kk = x->row; for ( j=1; j<= jj; j++) { for( i=1;i<= ii; i++) { r->mtx[i+((j-1)*ii)] = 0; for(k=1;k <= kk; k++) { r->mtx[i+((j-1)*ii)] += x->mtx[k+((i-1)*kk)] * y->mtx[k+((j-1)*kk)] ; } } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 // - printlog() -> 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) printlog("DIMENSION_MISMACH in mulmat_t2"); ii= x->row; r->row =ii; jj= y->row; r->col =jj; kk=x->col; for ( j=1; j<= jj; j++) { for( i=1;i<= ii; i++) { r->mtx[i+((j-1)*ii)] = 0; for(k=1;k <= kk; k++) { r->mtx[i+((j-1)*ii)] += x->mtx[i+((k-1)*ii)] * y->mtx[j+((k-1)*jj)] ; } } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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,ii,jj ; ii = x->row; r->row = ii; jj = x->col; r->col = jj; for ( i=1; i<= (ii*jj); i++) { r->mtx[i] = a * x->mtx[i]; } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 9 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) printlog("DIMENSION_ERROR in minv"); jj = x->row ; r->col = jj; r->row = jj; if(jj == 1) // if the matrix is scalar the inverse is calculated as: { det = x->mtx[1]; if(fabs(det) < POSITIV_ZERO) { printlog("det < POSITIV_ZERO in minv"); r->mtx[1] = HUGE_NUM; } else r->mtx[1] = 1/x->mtx[1]; } else if(jj==2)// for the two dimensional matrix the inverse is defined: { det = (x->mtx[1] * x->mtx[4]) - (x->mtx[2] * x->mtx[3]); if(fabs(det) < POSITIV_ZERO) { printlog("det < POSITIV_ZERO in minv"); r->mtx[1] = HUGE_NUM; r->mtx[2] = 0.0; r->mtx[3] = 0.0; r->mtx[4] = HUGE_NUM; } else { r->mtx[1] = x->mtx[4] / det; r->mtx[3] = -x->mtx[3] / det; r->mtx[2] = -x->mtx[2] / det; r->mtx[4] = x->mtx[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][j] = x->mtx[i][j]; for(i=1;i<=jj;i++)for(j=1;j<=jj;j++) cx[(i*MXED)+j] = x->mtx[i+((j-1)*jj)]; 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) { printlog("det < POSITIV_ZERO in minv"); for (j=1;j<=jj*jj;j++) r->mtx[j] = 0.0; //for (j=1;j<=jj;j++) r->mtx[j][j] = HUGE_NUM; for (j=1;j<=jj;j++) r->mtx[j+((j-1)*jj)] = 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]; for(i=1;i<=jj;i++) r->mtx[i+((j-1)*jj)]=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. } // <<--- inverse algorithm here (if dimension > 2) return 0; } // 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][j])) > big) big=temp; if ((temp=fabs(a[(i*MXED)+j])) > big) big=temp; if (big == 0.0) printlog("Singular matrix in routine ludcmp in minv"); //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]; // dum=a[imax][k]; a[(imax*MXED)+k]=a[(j*MXED)+k]; //a[imax][k]=a[j][k]; a[(j*MXED)+k]=dum; // a[j][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 (a[j][j] == 0.0) a[j][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]); //dum=1.0/(a[j][j]); for (i=j+1;i<=n;i++) a[(i*MXED)+j] *= dum; //a[i][j] *= dum; } } //Go back for the next column in the reduction. // free_vector(vv,1,n); return 0; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // Solves the set of n linear equations A X = B - for matrix inversion //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void lubksb(float *a, int n, int *indx, float b[]) //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]; //sum -= a[i][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]; //sum -= a[i][j]*b[j]; //b[i]=sum/a[i][i]; b[i]=sum/a[(i*MXED)+i]; //Store a component of the solution vector X. } // All done! } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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 ; jj= x->col; r->row = jj; ii= x->row; r->col = ii; for ( j=1; j<= jj; j++) { for( i=1;i<= ii; i++) { r->mtx[j+((i-1)*jj)] = x->mtx[i+((j-1)*ii)]; } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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,ii,jj ; jj = x->row; r->row = jj; ii = x->col; r->col = ii; for ( i=1; i<= ii*jj; i++) { r->mtx[i] = x->mtx[i] * x->mtx[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,ii,jj ; jj = x->row; r->row = jj; ii = x->col; r->col = ii; for ( i=1; i<= ii*jj; i++) { r->mtx[i] = sqrt(x->mtx[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,ii,jj ; jj= r->row; ii= r->col; for( i=1;i<= ii*jj; i++) { r->mtx[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,ii,jj ; jj= r->row; ii= r->col; if(jj!=ii) printlog("DIMENSION_EROOR in eyemat"); for( i=1;i<= ii*ii; i++) r->mtx[i] = (float)0.0; for (i=1; i<= ii; i++) r->mtx[i+((i-1)*ii)] = (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 ii,float x) /*Diagonal matrix [n x n]*/ { /* Calculate r by element*/ int i ; if(ii*ii > r->dim) printlog("Alocation error in diagn"); r->row = ii; r->col = ii; for ( i=1; i<= ii*ii; i++) { r->mtx[i] = (float)0.0; } for (i=1; i<= ii; i++) { r->mtx[i+((i-1)*ii)] = x; } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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,ii,jj ; jj= x->row; r->row = jj; ii= x->col; r->col = ii; for( i=1;i<= ii*jj; i++) { r->mtx[i] = x->mtx[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 = x->row; r->row = jj; ii1 = x->col; ii2 = y->col; r->col = ii1 + ii2; if(jj != y->row) printlog("DIMENSION_MISMACH in concat_row"); if((jj*(ii1+ii2)) > r->dim) printlog("Not eunagh MEMORY ALOCATED for concat_row"); for ( j=1; j<= jj; j++) { for( i=1;i<= ii1; i++) { r->mtx[j+((i-1)*jj)] = x->mtx[j+((i-1)*jj)]; } for( i=1;i<= ii2; i++) { //r->mtx[j][ii1+i] = y->mtx[j][i]; r->mtx[j+((ii1+i-1)*jj)] = y->mtx[j+((i-1)*jj)]; } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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,jjj ; jj1 = x->row; jj2 = y->row; ii = x->col; r->col = ii; jjj = jj1 + jj2; r->row = jjj; if(x->col != y->col) printlog("DIMENSION_MISMACH in concat_col"); if((ii*jjj) > r->dim) printlog("Not eunagh MEMORY ALOCATED for concat_col"); for( i=1;i<= ii; i++) { for ( j=1; j<= jj1; j++) { r->mtx[j+((i-1)*jjj)] = x->mtx[j+((i-1)*jj1)]; } for ( j=1; j<= jj2; j++) { //r->mtx[jj1+j][i] = y->mtx[j][i]; r->mtx[jj1+j+((i-1)*jjj)] = y->mtx[j+((i-1)*jj2)]; } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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,ii1 ; ii = x->col; r->col = ii; jj = re - (rs-1); r->row = jj; ii1 = x->row; if(ii1 < jj) printlog("DIMENSION_MISMACH in extract_row"); if((jj*ii) > r->dim) printlog("Not eunagh MEMORY ALOCATED for extract_row"); for( i=1;i<= ii; i++) { for ( j=1; j<= jj; j++) { //r->mtx[j][i] = x->mtx[j+(rs-1)][i]; r->mtx[j+((i-1)*jj)] = x->mtx[j+rs-1+((i-1)*ii1)]; } } } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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); r->col = ii; jj = x->row; r->row = jj; if(x->col < ii) printlog("DIMENSION_MISMACH in extract_col"); if((jj*ii) > r->dim) printlog("Not eunagh MEMORY ALOCATED for extract_col"); for( i=1;i<= ii; i++) { for ( j=1; j<= jj; j++) { //r->mtx[j][i] = x->mtx[j][i+(cs-1)]; r->mtx[j+((i-1)*jj)] = x->mtx[j+((i-1+cs-1)*jj)]; } } } /* The rand() function from the TI library of the DSP is avaliable: */ #if 0 //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- // // 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. } //----------------------------------------------------------------------------- #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 i,ii,jj; int j; int k; int r; float sum1; float sum2; ii=x->row; jj=x->col; if(jj != ii) printlog("DIMENSION_MISMACH in ptest"); r=1; for(j=1; j<=jj; j++) { for(i=1; i<=j; i++) { sum1=0; for(k=1; kmtx[i+((k-1)*ii)] * x->mtx[k+((j-1)*ii)]; } x->mtx[i+((j-1)*ii)] = x->mtx[i+((j-1)*ii)] - sum1; } if(x->mtx[j+((j-1)*ii)] < flim) { r=0; j=ii; } else { for(i=j+1; i<=jj ;i++) { sum2 = 0; for(k=1; kmtx[i+((k-1)*ii)] * x->mtx[k+((j-1)*ii)]; } x->mtx[i+((j-1)*ii)] = (x->mtx[i+((j-1)*ii)] - sum2)/x->mtx[j+((j-1)*ii)]; } } } /* 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; } ////----------------------------------------------------------------------------- ///----------------------------------------------------------------------------- // // 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; } //----------------------------------------------------------------------------- //-----------------------------------------------------------------------------