00001 /*****************************************************************************
00002    Major portions of this software are copyrighted by the Medical College
00003    of Wisconsin, 1994-2000, and are released under the Gnu General Public
00004    License, Version 2.  See the file README.Copyright for details.
00005 ******************************************************************************/
00006 
00007 /*!
00008   This file contains matrix and vector arithmetic routines.
00009 
00010   File:     matrix.c
00011   Author:   B. Douglas Ward
00012   Date:     23 April 1997
00013 
00014   Mod:      Changed print format for functions matrix_print and vector_print.
00015   Date:     05 August 1997
00016 
00017   Mod:      Changed initialization in function vector_create.
00018   Date:     04 November 1997
00019 
00020   Mod:      Added routines matrix_file_write and matrix_file_read.
00021   Date:     02 July 1999
00022         
00023   Mod:      Added routine for calculating square root of matrix.
00024   Date:     30 September 1999
00025 
00026   Mod:      Added routines matrix_sprint and vector_sprint.
00027   Date:     04 October 1999
00028 
00029   Mod:      Modified matrix_file_read to use mri_read_ascii routine.
00030   Date:     12 January 2000
00031 
00032   Mod:      Changed return type of vector_dot from float to double.
00033   Date:     13 April 2000
00034 
00035   Mod:      Added functions column_to_vector and matrix_extract_rows.
00036   Date:     21 April 2000
00037 
00038   Mod:      Added test for missing matrix file name.
00039   Date:     08 May 2000
00040 
00041   Mod:      Added "register" declarations and a few other things to speed
00042             up calculations (including vector_create_noinit) -- RWCox.
00043   Date:     28 Dec 2001
00044 
00045   Mod:      Allow matrices and vectors with zero rows and columns. 
00046   Date:     26 February 2002
00047 
00048   Mod:      Corrected errors in vector_multiply and vector_multiply_subtract 
00049             routines that would produce segmentation fault for certain input 
00050             matrices.
00051   Date:     18 March 2003
00052 
00053   Mod:      Added UNROLL_VECMUL stuff from matrix.c to this file as well.
00054             Added 'ipr' to matrix_print().
00055             Added USE_ALTIVEC stuff for Macs.
00056   Date:     03 Aug 2004
00057 
00058   Mod:      Added USE_SCSLBLAS stuff for SGI Altix.
00059   Date:     01 Mar 2005
00060 */
00061 
00062 #include "mri_image.h"  /* moved here on 16 May 2005, for OS X Tiger */
00063 extern MRI_IMAGE *mri_read_1D(char *) ;
00064 
00065 #include <math.h>
00066 #include <stdlib.h>
00067 #include <stdio.h>
00068 #include "matrix_f.h"
00069 
00070 /*---------------------------------------------------------------------*/
00071 /** Vectorization macros:
00072    - DOTP(n,x,y,z) computes the n-long dot product of vectors
00073        x and y and puts the result into the place pointed to by z.
00074    - VSUB(n,x,y,z) computes vector x-y into vector z.
00075    - These are intended to be the fast method for doing these things. **/
00076 /*---------------------------------------------------------------------*/
00077 
00078 /* Solaris BLAS isn't used here because it is slower than
00079    inline code for single precision, but faster for double. */
00080 
00081 #undef SETUP_BLAS  /* define this to use BLAS-1 functions */
00082 #undef DOTP
00083 #undef VSUB
00084 
00085 #if defined(USE_ALTIVEC)                             /** Apple **/
00086 
00087 # include <Accelerate/Accelerate.h>
00088 # define DOTP(n,x,y,z) dotpr( x,1 , y,1 , z , n )
00089 # define VSUB(n,x,y,z) vsub( x,1 , y,1 , z,1 , n )
00090 
00091 #elif defined(USE_SCSLBLAS)                          /** SGI Altix **/
00092 
00093 # include <scsl_blas.h>
00094 # define SETUP_BLAS
00095 
00096 #endif  /* vectorization special cases */
00097 
00098 /* single precision BLAS-1 functions */
00099 
00100 #ifdef SETUP_BLAS
00101 # define DOTP(n,x,y,z) *(z)=sdot(n,x,1,y,1)
00102 # define VSUB(n,x,y,z) (memcpy(z,x,sizeof(float)*n),saxpy(n,-1.0f,y,1,z,1))
00103 #endif
00104 
00105 #include <string.h>
00106 
00107 /*---------------------------------------------------------------------------*/
00108 /*!
00109   Routine to print and error message and stop.
00110 */
00111 
00112 void matrix_error (char * message)
00113 {
00114   printf ("Matrix error: %s \n", message);
00115   exit (1);
00116 }
00117 
00118 
00119 /*---------------------------------------------------------------------------*/
00120 /*!
00121   Initialize matrix data structure.
00122 */
00123 
00124 void matrix_initialize (matrix * m)
00125 {
00126   m->rows = 0;
00127   m->cols = 0;
00128   m->elts = NULL;
00129 #ifndef DONT_USE_MATRIX_MAT
00130   m->mat  = NULL;
00131 #endif
00132 }
00133 
00134 
00135 /*---------------------------------------------------------------------------*/
00136 /*!
00137   Destroy matrix data structure by deallocating memory.
00138 */
00139 
00140 void matrix_destroy (matrix * m)
00141 {
00142   if (m->elts != NULL){
00143 #ifdef DONT_USE_MATRIX_MAT
00144     int i ;
00145     for( i=0 ; i < m->rows ; i++ )
00146       if( m->elts[i] != NULL ) free(m->elts[i]) ;
00147 #endif
00148     free(m->elts) ;
00149   }
00150 #ifndef DONT_USE_MATRIX_MAT
00151   if( m->mat  != NULL) free (m->mat) ;
00152 #endif
00153   matrix_initialize (m);
00154 }
00155 
00156 /*---------------------------------------------------------------------------*/
00157 /*!
00158   Create matrix data structure by allocating memory and initializing values.
00159 */
00160 
00161 void matrix_create (int rows, int cols, matrix * m)
00162 {
00163   register int i, j;
00164 
00165   matrix_destroy (m);
00166 
00167   if ((rows < 0) || (cols < 0))
00168     matrix_error ("Illegal dimensions for new matrix");
00169 
00170   m->rows = rows;
00171   m->cols = cols;
00172   if ((rows < 1) || (cols < 1))  return;
00173 
00174   m->elts = (float  **) malloc (sizeof(float  *) * rows);
00175   if (m->elts == NULL)
00176     matrix_error ("Memory allocation error");
00177 
00178 #ifdef DONT_USE_MATRIX_MAT
00179   for (i = 0;  i < rows;  i++){
00180     m->elts[i] = (float *) calloc (sizeof(float) , cols);
00181     if (m->elts[i] == NULL) matrix_error ("Memory allocation error");
00182   }
00183 #else
00184   m->mat  = (float *) calloc( sizeof(float) , rows*cols ) ;
00185   if( m->mat == NULL )
00186     matrix_error ("Memory allocation error");
00187   for (i = 0;  i < rows;  i++)
00188      m->elts[i] = m->mat + (i*cols) ;   /* 04 Mar 2005: offsets into mat */
00189 #endif
00190 }
00191 
00192 
00193 /*---------------------------------------------------------------------------*/
00194 /*!
00195   Print contents of matrix m.
00196 */
00197 
00198 void matrix_print (matrix m)
00199 {
00200   int i, j;
00201   int rows, cols;
00202   float val ;
00203   int ipr ;
00204 
00205   rows = m.rows;
00206   cols = m.cols;
00207 
00208   for( i=0 ; i < rows ; i++ ){
00209     for( j=0 ; j < cols ; j++ ){
00210       val = (int)m.elts[i][j] ;
00211       if( val != m.elts[i][j] || fabs(val) > 9.0f ) goto zork ;
00212     }
00213   }
00214 zork:
00215   ipr = (i==rows && j==cols) ;
00216 
00217   for (i = 0;  i < rows;  i++)
00218     {
00219       for (j = 0;  j < cols;  j++)
00220         if( ipr ) printf (" %2d"   , (int)m.elts[i][j]);
00221         else      printf (" %10.4g", m.elts[i][j]);
00222       printf (" \n");
00223     }
00224   printf (" \n"); fflush(stdout);
00225 }
00226 
00227 
00228 /*---------------------------------------------------------------------------*/
00229 /*!
00230   Print label and contents of matrix m.
00231 */
00232 
00233 void matrix_sprint (char * s, matrix m)
00234 {
00235   printf ("%s \n", s);
00236 
00237   matrix_print (m);
00238 }
00239 
00240 
00241 /*---------------------------------------------------------------------------*/
00242 /*!
00243   Print contents of matrix m to specified file.
00244 */
00245 
00246 void matrix_file_write (char * filename, matrix m)
00247 {
00248   int i, j;
00249   int rows, cols;
00250   FILE * outfile = NULL;
00251 
00252 
00253   /*----- First, check for empty file name -----*/
00254   if (filename == NULL)  matrix_error ("Missing matrix file name");
00255 
00256 
00257   outfile = fopen (filename, "w");
00258 
00259   rows = m.rows;
00260   cols = m.cols;
00261 
00262   for (i = 0;  i < rows;  i++)
00263     {
00264       for (j = 0;  j < cols;  j++)
00265         fprintf (outfile, "  %g", m.elts[i][j]);
00266       fprintf (outfile, " \n");
00267     }
00268   fprintf (outfile, " \n");
00269 
00270   fclose (outfile);
00271 }
00272 
00273 /*---------------------------------------------------------------------------*/
00274 /*!
00275   Manual entry of matrix data.
00276 */
00277 
00278 void matrix_enter (matrix * m)
00279 {
00280   int rows, cols;
00281   int i, j;
00282   float fval;
00283 
00284   printf ("Enter number of rows: "); fflush(stdout);
00285   scanf ("%d", &rows);
00286   printf ("Enter number of cols: "); fflush(stdout);
00287   scanf ("%d", &cols);
00288 
00289   matrix_create (rows, cols, m);
00290 
00291   for (i = 0;  i < rows;  i++)
00292     for (j = 0;  j < cols;  j++)
00293       {
00294         printf ("elts[%d][%d] = ", i, j); fflush(stdout);
00295         scanf ("%f", &fval);
00296         m->elts[i][j] = fval;
00297       }
00298 }
00299 
00300 
00301 /*---------------------------------------------------------------------------*/
00302 /*!
00303   Read contents of matrix m from specified file.
00304   If unable to read matrix from file, or matrix has wrong dimensions:
00305      If error_exit flag is set, then print error message and exit.
00306      Otherwise, return null matrix.
00307 */
00308 
00309 void matrix_file_read (char *filename, int rows, int cols,  matrix *m,
00310                        int error_exit)
00311 {
00312   int i, j;
00313 
00314   MRI_IMAGE *im, *flim;  /* pointers to image structures
00315                             -- used to read ASCII file */
00316   float * far;             /* pointer to MRI_IMAGE floating point data */
00317   char message [80];       /* error message */
00318 
00319 
00320   /*----- First, check for empty file name -----*/
00321   if (filename == NULL)  matrix_error ("Missing matrix file name");
00322 
00323 
00324   /*----- Read the matrix file -----*/
00325   flim = mri_read_1D (filename); 
00326   if (flim == NULL)
00327     if (error_exit)
00328       {
00329         sprintf (message,  "Unable to read matrix from file: %s",  filename);
00330         matrix_error (message);
00331       }
00332     else
00333       {
00334         matrix_destroy (m);
00335         return;
00336       }
00337 
00338   
00339   /*----- Set pointer to data  -----*/
00340   far = MRI_FLOAT_PTR(flim);
00341 
00342 
00343   /*----- Test for correct dimensions -----*/
00344   if ( (rows != flim->nx) || (cols != flim->ny) )
00345     if (error_exit)
00346       {
00347         sprintf (message, 
00348                  "In matrix file: %s   Expected: %d x %d   Actual: %d x %d",
00349                  filename, rows, cols, flim->nx, flim->ny);
00350         matrix_error (message);
00351       }
00352     else
00353       {
00354         matrix_destroy (m);
00355         return;
00356       }
00357   
00358 
00359   matrix_create (rows, cols, m);
00360 
00361 
00362   /*----- Copy data from image structure to matrix structure -----*/
00363   for (i = 0;  i < rows;  i++)
00364     for (j = 0;  j < cols;  j++)
00365       m->elts[i][j] = far[i + j*rows];
00366 
00367 
00368   mri_free (flim);  flim = NULL;
00369 
00370 }
00371 
00372 
00373 /*---------------------------------------------------------------------------*/
00374 /*!
00375   Convert simple array to matrix structure.
00376 */
00377 
00378 void array_to_matrix (int rows, int cols, float ** f, matrix * m)
00379 {
00380   register int i, j;
00381 
00382   matrix_create (rows, cols, m);
00383 
00384   for (i = 0;  i < rows;  i++)
00385     for (j = 0;  j < cols;  j++)
00386       m->elts[i][j] = f[i][j];
00387 }
00388 
00389 
00390 /*---------------------------------------------------------------------------*/
00391 /*!
00392   Make a copy of the first matrix, return copy as the second matrix.
00393 */
00394 
00395 void matrix_equate (matrix a, matrix * b)
00396 {
00397   register int i, j;
00398   register int rows, cols;
00399 
00400   rows = a.rows;
00401   cols = a.cols;
00402 
00403   matrix_create (rows, cols, b);
00404 
00405   for (i = 0;  i < rows;  i++){
00406 #if 0
00407     for (j = 0;  j < cols;  j++)
00408       b->elts[i][j] = a.elts[i][j];
00409 #else
00410     if( cols > 0 )
00411       memcpy( b->elts[i] , a.elts[i] , sizeof(float )*cols ) ;  /* RWCox */
00412 #endif
00413   }
00414 }
00415 
00416 
00417 /*---------------------------------------------------------------------------*/
00418 /*!
00419   Extract p columns (specified by list) from matrix a.  Result is matrix b.
00420 */
00421 
00422 void matrix_extract (matrix a, int p, int * list, matrix * b)
00423 {
00424   register int i, j;
00425   register int rows, cols;
00426 
00427   rows = a.rows;
00428   cols = p;
00429 
00430   matrix_create (rows, cols, b);
00431 
00432   for (i = 0;  i < rows;  i++)
00433     for (j = 0;  j < cols;  j++)
00434       b->elts[i][j] = a.elts[i][list[j]];
00435 }
00436 
00437 
00438 /*---------------------------------------------------------------------------*/
00439 /*!
00440   Extract p rows (specified by list) from matrix a.  Result is matrix b.
00441 */
00442 
00443 void matrix_extract_rows (matrix a, int p, int * list, matrix * b)
00444 {
00445   register int i, j;
00446   register int rows, cols;
00447 
00448   rows = p;
00449   cols = a.cols;
00450 
00451   matrix_create (rows, cols, b);
00452 
00453   for (i = 0;  i < rows;  i++)
00454     for (j = 0;  j < cols;  j++)
00455       b->elts[i][j] = a.elts[list[i]][j];
00456 }
00457 
00458 
00459 /*---------------------------------------------------------------------------*/
00460 /*!
00461   Create n x n identity matrix.
00462 */
00463 
00464 void matrix_identity (int n, matrix * m)
00465 {
00466   register int i, j;
00467 
00468   if (n < 0)
00469     matrix_error ("Illegal dimensions for identity matrix");
00470 
00471   matrix_create (n, n, m);
00472 
00473   for (i = 0;  i < n;  i++)
00474     for (j = 0;  j < n;  j++)
00475       if (i == j)
00476         m->elts[i][j] = 1.0;
00477       else
00478         m->elts[i][j] = 0.0;
00479 }
00480 
00481 
00482 /*---------------------------------------------------------------------------*/
00483 /*!
00484   Add matrix a to matrix b.  Result is matrix c.
00485 */
00486 
00487 void matrix_add (matrix a, matrix b, matrix * c)
00488 {
00489   register int rows, cols;
00490   register int i, j;
00491 
00492   if ((a.rows != b.rows) || (a.cols != b.cols))
00493     matrix_error ("Incompatible dimensions for matrix addition");
00494 
00495   rows = a.rows;
00496   cols = a.cols;
00497 
00498   matrix_create (rows, cols, c);
00499 
00500   for (i = 0;  i < rows;  i++)
00501     for (j = 0;  j < cols;  j++)
00502       c->elts[i][j] = a.elts[i][j] + b.elts[i][j];
00503 }
00504 
00505 
00506 /*---------------------------------------------------------------------------*/
00507 /*!
00508   Subtract matrix b from matrix a.  Result is matrix c.
00509 */
00510 
00511 void matrix_subtract (matrix a, matrix b, matrix * c)
00512 {
00513   register int rows, cols;
00514   register int i, j;
00515 
00516   if ((a.rows != b.rows) || (a.cols != b.cols))
00517     matrix_error ("Incompatible dimensions for matrix subtraction");
00518 
00519   rows = a.rows;
00520   cols = a.cols;
00521 
00522   matrix_create (rows, cols, c);
00523 
00524   for (i = 0;  i < rows;  i++)
00525     for (j = 0;  j < cols;  j++)
00526       c->elts[i][j] = a.elts[i][j] - b.elts[i][j];
00527 }
00528 
00529 
00530 /*---------------------------------------------------------------------------*/
00531 /*!
00532   Multiply matrix a by matrix b.  Result is matrix c.
00533 */
00534 
00535 void matrix_multiply (matrix a, matrix b, matrix * c)
00536 {
00537   int rows, cols;
00538   register int i, j, k;
00539   register float  sum ;
00540 
00541   if (a.cols != b.rows)
00542     matrix_error ("Incompatible dimensions for matrix multiplication");
00543 
00544   rows = a.rows;
00545   cols = b.cols;
00546 
00547   matrix_create (rows, cols, c);
00548 
00549   for (i = 0;  i < rows;  i++)
00550     for (j = 0;  j < cols;  j++)
00551       {
00552         sum = 0.0 ;
00553         for (k = 0;  k < a.cols;  k++)
00554           sum += a.elts[i][k] * b.elts[k][j];
00555         c->elts[i][j] = sum;
00556       }
00557 }
00558 
00559 
00560 /*---------------------------------------------------------------------------*/
00561 /*!
00562   Multiply matrix a by scalar constant k.  Result is matrix c.
00563 */
00564 
00565 void matrix_scale (float  k, matrix a, matrix * c)
00566 {
00567   register int rows, cols;
00568   register int i, j;
00569 
00570   rows = a.rows;
00571   cols = a.cols;
00572 
00573   matrix_create (rows, cols, c);
00574 
00575   for (i = 0;  i < rows;  i++)
00576     for (j = 0;  j < cols;  j++)
00577       c->elts[i][j] = k * a.elts[i][j];
00578 }
00579 
00580 
00581 /*---------------------------------------------------------------------------*/
00582 /*!
00583   Take transpose of matrix a.  Result is matrix t.
00584 */
00585 
00586 void matrix_transpose (matrix a, matrix * t)
00587 {
00588   register int rows, cols;
00589   register int i, j;
00590 
00591   rows = a.cols;
00592   cols = a.rows;
00593 
00594   matrix_create (rows, cols, t);
00595   for (i = 0;  i < rows;  i++)
00596     for (j = 0;  j < cols;  j++)
00597       t->elts[i][j] = a.elts[j][i];
00598 }
00599 
00600  
00601 /*---------------------------------------------------------------------------*/
00602 /*!
00603   Use Gaussian elimination to calculate inverse of matrix a.  Result is 
00604   matrix ainv.
00605 */
00606 
00607 int matrix_inverse (matrix a, matrix * ainv)
00608 {
00609   const float  epsilon = 1.0e-10;
00610   matrix tmp;
00611   register int i, j, ii, n;
00612   register float  fval;
00613   register float  fmax;
00614   register float  * p;
00615 
00616   matrix_initialize (&tmp);
00617 
00618 
00619   if (a.rows != a.cols) 
00620     matrix_error ("Illegal dimensions for matrix inversion");
00621 
00622 
00623   n = a.rows;
00624   matrix_identity (n, ainv);
00625   matrix_equate (a, &tmp);
00626 
00627   for (i = 0;  i < n;  i++)
00628     {
00629       fmax = fabs(tmp.elts[i][i]);
00630       for (j = i+1;  j < n;  j++)
00631         if (fabs(tmp.elts[j][i]) > fmax)
00632           {
00633             fmax = fabs(tmp.elts[j][i]);
00634             p = tmp.elts[i];
00635             tmp.elts[i] = tmp.elts[j];
00636             tmp.elts[j] = p;
00637             p = ainv->elts[i];
00638             ainv->elts[i] = ainv->elts[j];
00639             ainv->elts[j] = p;
00640           }
00641 
00642       if (fmax < epsilon)
00643         {
00644           matrix_destroy (&tmp);
00645           return (0);
00646         }
00647         
00648 
00649       fval = 1.0 / tmp.elts[i][i];   /* RWCox: change division by this to */
00650       for (j = 0;  j < n;  j++)      /*        multiplication by 1.0/this */
00651         {
00652           tmp.elts[i][j]   *= fval;
00653           ainv->elts[i][j] *= fval;
00654         }
00655       for (ii = 0;  ii < n;  ii++)
00656         if (ii != i)
00657           {
00658             fval = tmp.elts[ii][i];
00659             for (j = 0;  j < n;  j++)
00660               {
00661                 tmp.elts[ii][j] -= fval*tmp.elts[i][j];
00662                 ainv->elts[ii][j] -= fval*ainv->elts[i][j];
00663               }
00664           }
00665         
00666     }
00667   matrix_destroy (&tmp);
00668   return (1);
00669 }
00670 
00671 /*---------------------------------------------------------------------------*/
00672 /*!
00673   Use Gaussian elimination to calculate inverse of matrix a, with diagonal
00674   scaling applied for stability.  Result is matrix ainv.
00675 */
00676 
00677 int matrix_inverse_dsc (matrix a, matrix * ainv)  /* 15 Jul 2004 - RWCox */
00678 {
00679   matrix atmp;
00680   register int i, j, n;
00681   register double *diag ;
00682   int mir ;
00683 
00684   if (a.rows != a.cols)
00685     matrix_error ("Illegal dimensions for matrix inversion");
00686 
00687   matrix_initialize (&atmp);
00688 
00689   n = a.rows;
00690   matrix_equate (a, &atmp);
00691   diag = (double *)malloc( sizeof(double)*n ) ;
00692   for( i=0 ; i < n ; i++ ){
00693     diag[i] = fabs(atmp.elts[i][i]) ;
00694     if( diag[i] == 0.0 ) diag[i] = 1.0 ;  /* shouldn't happen? */
00695     diag[i] = 1.0 / sqrt(diag[i]) ;
00696   }
00697 
00698   for( i=0 ; i < n ; i++ )                /* scale a */
00699    for( j=0 ; j < n ; j++ )
00700     atmp.elts[i][j] *= diag[i]*diag[j] ;
00701 
00702   mir = matrix_inverse( atmp , ainv ) ;   /* invert */
00703 
00704   for( i=0 ; i < n ; i++ )                /* scale inverse */
00705    for( j=0 ; j < n ; j++ )
00706     ainv->elts[i][j] *= diag[i]*diag[j] ;
00707 
00708   matrix_destroy (&atmp); free((void *)diag) ;
00709   return (mir);
00710 }
00711 
00712  
00713 /*---------------------------------------------------------------------------*/
00714 /*!
00715   Calculate square root of symmetric positive definite matrix a.  
00716   Result is matrix s.
00717 */
00718 
00719 int matrix_sqrt (matrix a, matrix * s)
00720 {
00721   const int MAX_ITER = 100;
00722   int n;
00723   int ok;
00724   int iter;
00725   register float sse, psse;
00726   register int i, j;
00727   matrix x, xinv, axinv, xtemp, error;
00728 
00729   matrix_initialize (&x);
00730   matrix_initialize (&xinv);
00731   matrix_initialize (&axinv);
00732   matrix_initialize (&xtemp);
00733   matrix_initialize (&error);
00734 
00735 
00736   if (a.rows != a.cols) 
00737     matrix_error ("Illegal dimensions for matrix square root");
00738 
00739 
00740   n = a.rows;
00741   matrix_identity (n, &x);
00742 
00743 
00744   psse = 1.0e+30;
00745   for (iter = 0;  iter < MAX_ITER;  iter++)
00746     {
00747       ok = matrix_inverse (x, &xinv);
00748       if (! ok)  return (0);
00749       matrix_multiply (a, xinv, &axinv);
00750       matrix_add (x, axinv, &xtemp);
00751       matrix_scale (0.5, xtemp, &x);
00752 
00753       matrix_multiply (x, x, &xtemp);
00754       matrix_subtract (a, xtemp, &error);
00755       sse = 0.0;
00756       for (i = 0;  i < n;  i++)
00757         for (j = 0;  j < n;  j++)
00758           sse += error.elts[i][j] * error.elts[i][j] ;
00759 
00760       if (sse >= psse) break;
00761 
00762       psse = sse;
00763     }
00764 
00765   if (iter == MAX_ITER)  return (0);
00766 
00767   matrix_equate (x, s);
00768 
00769   matrix_destroy (&x);
00770   matrix_destroy (&xinv);
00771   matrix_destroy (&axinv);
00772   matrix_destroy (&xtemp);
00773 
00774   return (1);
00775 
00776 }
00777 
00778 
00779 /*---------------------------------------------------------------------------*/
00780 /*!
00781   Initialize vector data structure.
00782 */
00783 
00784 void vector_initialize (vector * v)
00785 {
00786   v->dim = 0;
00787   v->elts = NULL;
00788 }
00789 
00790 
00791 /*---------------------------------------------------------------------------*/
00792 /*!
00793   Destroy vector data structure by deallocating memory.
00794 */
00795 
00796 void vector_destroy (vector * v)
00797 {
00798   if (v->elts != NULL)  free (v->elts);
00799   vector_initialize (v);
00800 }
00801 
00802 
00803 /*---------------------------------------------------------------------------*/
00804 /*!
00805   Create vector v by allocating memory and initializing values.
00806 */
00807 
00808 void vector_create (int dim, vector * v)
00809 {
00810   register int i;
00811 
00812   vector_destroy (v);
00813   
00814   if (dim < 0)  matrix_error ("Illegal dimensions for new vector");
00815 
00816   v->dim = dim;
00817   if (dim < 1)  return;
00818 
00819   v->elts = (float  *) calloc (sizeof(float) , dim);
00820   if (v->elts == NULL)
00821     matrix_error ("Memory allocation error");
00822 }
00823 
00824 /*---------------------------------------------------------------------------*/
00825 static void vector_create_noinit(int dim, vector * v)  /* 28 Dec 2001: RWCox */
00826 {
00827   register int i;
00828 
00829   vector_destroy (v);
00830  
00831   if (dim < 0)  matrix_error ("Illegal dimensions for new vector");
00832 
00833   v->dim = dim;
00834   if (dim < 1)  return;
00835 
00836   v->elts = (float  *) malloc (sizeof(float ) * dim);
00837   if (v->elts == NULL)
00838     matrix_error ("Memory allocation error");
00839 }
00840 
00841 /*---------------------------------------------------------------------------*/
00842 /*!
00843   Print contents of vector v.
00844 */
00845 
00846 void vector_print (vector v)
00847 {
00848   int i;
00849 
00850   for (i = 0;  i < v.dim;  i++)
00851     printf ("  %10.4g \n", v.elts[i]);
00852   printf (" \n"); fflush(stdout);
00853     
00854 }
00855 
00856 
00857 /*---------------------------------------------------------------------------*/
00858 /*!
00859   Print label and contents of vector v.
00860 */
00861 
00862 void vector_sprint (char * s, vector v)
00863 {
00864   printf ("%s \n", s);
00865 
00866   vector_print (v);
00867 }
00868 
00869 
00870 /*---------------------------------------------------------------------------*/
00871 /*!
00872   Copy vector a.  Result is vector b.
00873 */
00874 
00875 void vector_equate (vector a, vector * b)
00876 {
00877   register int i, dim;
00878 
00879   dim = a.dim;
00880 
00881   vector_create_noinit (dim, b);
00882 
00883 #if 0
00884   for (i = 0;  i < dim;  i++)
00885     b->elts[i] = a.elts[i];
00886 #else
00887   if( dim > 0 )
00888     memcpy( b->elts , a.elts , sizeof(float )*dim ) ;  /* RWCox */
00889 #endif
00890 }
00891 
00892 
00893 /*---------------------------------------------------------------------------*/
00894 /*!
00895   Convert simple array f into vector v.
00896 */
00897 
00898 void array_to_vector (int dim, float * f, vector * v)
00899 {
00900   register int i;
00901 
00902   vector_create_noinit (dim, v);
00903 
00904   for (i = 0;  i < dim;  i++)
00905     v->elts[i] = f[i];
00906 }
00907 
00908 
00909 
00910 /*---------------------------------------------------------------------------*/
00911 /*!
00912   Convert column c of matrix m into vector v.
00913 */
00914 
00915 void column_to_vector (matrix m, int c, vector * v)
00916 {
00917   register int i;
00918   register int dim;
00919 
00920   dim = m.rows;
00921   vector_create_noinit (dim, v);
00922 
00923   for (i = 0;  i < dim;  i++)
00924     v->elts[i] = m.elts[i][c];
00925 }
00926 
00927 
00928 
00929 /*---------------------------------------------------------------------------*/
00930 /*!
00931   Convert vector v into array f.
00932 */
00933 
00934 void vector_to_array (vector v, float * f)
00935 {
00936   register int i;
00937  
00938   for (i = 0;  i < v.dim;  i++)
00939     f[i] = v.elts[i];
00940 }
00941 
00942 
00943 /*---------------------------------------------------------------------------*/
00944 /*!
00945   Add vector a to vector b.  Result is vector c.
00946 */
00947 
00948 void vector_add (vector a, vector b, vector * c)
00949 {
00950   register int i, dim;
00951 
00952   if (a.dim != b.dim)
00953     matrix_error ("Incompatible dimensions for vector addition");
00954 
00955   dim = a.dim;
00956 
00957   vector_create_noinit (dim, c);
00958 
00959   for (i = 0;  i < dim;  i++)
00960     c->elts[i] = a.elts[i] + b.elts[i];
00961 }
00962 
00963 
00964 /*---------------------------------------------------------------------------*/
00965 /*!
00966   Subtract vector b from vector a.  Result is vector c.
00967 */
00968 
00969 void vector_subtract (vector a, vector b, vector * c)
00970 {
00971   register int i, dim;
00972   register float  *aa,*bb,*cc ;
00973 
00974   if (a.dim != b.dim)
00975     matrix_error ("Incompatible dimensions for vector subtraction");
00976 
00977   dim = a.dim;
00978 
00979   vector_create_noinit (dim, c);
00980 
00981   aa = a.elts ; bb = b.elts ; cc = c->elts ;
00982   for (i = 0;  i < dim;  i++)
00983 #if 0
00984     c->elts[i] = a.elts[i] - b.elts[i];
00985 #else
00986     cc[i] = aa[i] - bb[i] ;
00987 #endif
00988 }
00989 
00990 #define UNROLL_VECMUL  /* RWCox */
00991 
00992 /*---------------------------------------------------------------------------*/
00993 /*!
00994   Right multiply matrix a by vector b.  Result is vector c.
00995 */
00996 void vector_multiply (matrix a, vector b, vector * c)
00997 {
00998   register int rows, cols;
00999   register int i, j;
01000   register float  sum ;
01001   register float  *bb ;
01002 #ifdef DOTP
01003   register float **aa , *cc ;
01004 #else
01005   register float *aa ;
01006 #endif
01007 
01008   if (a.cols != b.dim)
01009     matrix_error ("Incompatible dimensions for vector multiplication");
01010 
01011   rows = a.rows;
01012   cols = a.cols;
01013 
01014   vector_create_noinit (rows, c);
01015 
01016   if( cols <= 0 ){
01017     for( i=0 ; i < rows ; i++ ) c->elts[i] = 0.0f ;
01018     return ;
01019   }
01020 
01021   bb = b.elts ;
01022 
01023 #ifdef DOTP
01024   aa = a.elts ; cc = c->elts ;
01025   i = rows%2 ;
01026   if( i == 1 ) DOTP(cols,aa[0],bb,cc) ;
01027   for( ; i < rows ; i+=2 ){
01028     DOTP(cols,aa[i]  ,bb,cc+i    ) ;
01029     DOTP(cols,aa[i+1],bb,cc+(i+1)) ;
01030   }
01031 #else
01032 
01033 #ifdef UNROLL_VECMUL
01034   if( cols%2 == 0 ){              /* even number of cols */
01035     for (i = 0;  i < rows;  i++){
01036         sum = 0.0f ; aa = a.elts[i] ;
01037         for (j = 0;  j < cols;  j+=2 )
01038           sum += aa[j]*bb[j] + aa[j+1]*bb[j+1];
01039         c->elts[i] = sum ;
01040     }
01041   } else {                        /* odd number of cols */
01042     for (i = 0;  i < rows;  i++){
01043         aa = a.elts[i] ; sum = aa[0]*bb[0] ;
01044         for (j = 1;  j < cols;  j+=2 )
01045           sum += aa[j]*bb[j] + aa[j+1]*bb[j+1];
01046         c->elts[i] = sum ;
01047     }
01048   }
01049 #else
01050     for (i = 0;  i < rows;  i++){
01051         sum = 0.0f ; aa = a.elts[i] ;
01052         for (j = 0;  j < cols;  j++ ) sum += aa[j]*bb[j] ;
01053         c->elts[i] = sum ;
01054     }
01055 #endif /* UNROLL_VECMUL */
01056 
01057 #endif /* DOTP */
01058 
01059 }
01060 
01061 /*---------------------------------------------------------------------------*/
01062 /*!
01063   Compute d = c-a*b: a is a matrix; b,c,d are vectors -- RWCox
01064   26 Feb 2002: return value is sum of squares of d vector
01065 */
01066 
01067 float  vector_multiply_subtract (matrix a, vector b, vector c, vector * d)
01068 {
01069   register int rows, cols;
01070   register int i, j;
01071   register float  *bb ;
01072 #ifdef DOTP
01073   float qsum,sum , **aa , *dd,*cc,*ee ;
01074 #else
01075   register float qsum,sum, *aa ;
01076 #endif
01077 
01078   if (a.cols != b.dim || a.rows != c.dim )
01079     matrix_error ("Incompatible dimensions for vector multiplication-subtraction");
01080 
01081   rows = a.rows;
01082   cols = a.cols;
01083 
01084   vector_create_noinit (rows, d);
01085 
01086   if( cols <= 0 ){
01087     qsum = 0.0f ;
01088     for( i=0 ; i < rows ; i++ ){
01089       d->elts[i] = c.elts[i] ;
01090       qsum += d->elts[i] * d->elts[i] ;
01091     }
01092     return qsum ;
01093   }
01094 
01095   qsum = 0.0f ; bb = b.elts ;
01096 
01097 #ifdef DOTP
01098   aa = a.elts ; dd = d->elts ; cc = c.elts ;
01099   ee = (float *)malloc(sizeof(float)*rows) ;
01100   i  = rows%2 ;
01101   if( i == 1 ) DOTP(cols,aa[0],bb,ee) ;
01102   for( ; i < rows ; i+=2 ){
01103     DOTP(cols,aa[i]  ,bb,ee+i    ) ;
01104     DOTP(cols,aa[i+1],bb,ee+(i+1)) ;
01105   }
01106   VSUB(rows,cc,ee,dd) ;
01107   DOTP(rows,dd,dd,&qsum) ;
01108   free((void *)ee) ;
01109 #else
01110 
01111 #ifdef UNROLL_VECMUL
01112   if( cols%2 == 0 ){                   /* even number */
01113     for (i = 0;  i < rows;  i++){
01114       aa = a.elts[i] ; sum = c.elts[i] ;
01115       for (j = 0;  j < cols;  j+=2)
01116         sum -= aa[j]*bb[j] + aa[j+1]*bb[j+1];
01117       d->elts[i] = sum ; qsum += sum*sum ;
01118     }
01119   } else {                            /* odd number */
01120     for (i = 0;  i < rows;  i++){
01121       aa = a.elts[i] ; sum = c.elts[i] - aa[0]*bb[0] ;
01122       for (j = 1;  j < cols;  j+=2)
01123         sum -= aa[j]*bb[j] + aa[j+1]*bb[j+1];
01124       d->elts[i] = sum ; qsum += sum*sum ;
01125     }
01126   }
01127 #else
01128   for (i = 0;  i < rows;  i++){
01129     aa = a.elts[i] ; sum = c.elts[i] ;
01130     for (j = 0;  j < cols;  j++) sum -= aa[j] * bb[j] ;
01131     d->elts[i] = sum ; qsum += sum*sum ;
01132   }
01133 #endif /* UNROLL_VECMUL */
01134 
01135 #endif /* DOTP */
01136 
01137   return qsum ;  /* 26 Feb 2003 */
01138 }
01139 
01140 /*---------------------------------------------------------------------------*/
01141 /*!
01142   Calculate dot product of vector a with vector b.
01143 */
01144 
01145 float  vector_dot (vector a, vector b)
01146 {
01147   register int i, dim;
01148   register float  sum;
01149   register float  *aa , *bb ;
01150 
01151   if (a.dim != b.dim)
01152     matrix_error ("Incompatible dimensions for vector dot product");
01153 
01154   dim = a.dim;
01155 
01156   sum = 0.0f;
01157   aa = a.elts ; bb = b.elts ;
01158   for (i = 0;  i < dim;  i++)
01159 #if 0
01160     sum += a.elts[i] * b.elts[i];
01161 #else
01162     sum += aa[i] * bb[i] ;
01163 #endif
01164 
01165   return (sum);
01166 }
01167 
01168 /*--------------------------------------------------------------------------*/
01169 /*!
01170   Calculate dot product of vector a with itself -- 28 Dec 2001, RWCox.
01171 */
01172 
01173 float  vector_dotself( vector a )
01174 {
01175   register int i, dim;
01176   register float  sum;
01177   register float  *aa ;
01178 
01179   dim = a.dim;
01180   sum = 0.0f;
01181   aa = a.elts ;
01182   for (i = 0;  i < dim;  i++)
01183 #if 0
01184     sum += a.elts[i] * a.elts[i];
01185 #else
01186     sum += aa[i] * aa[i] ;
01187 #endif
01188 
01189   return (sum);
01190 }
01191 
01192 /*---------------------------------------------------------------------------*/
01193 /*!
01194   Compute the L_infinity norm of a matrix: the max absolute row sum.
01195 */
01196 
01197 float matrix_norm( matrix a )
01198 {
01199    int i,j , rows=a.rows, cols=a.cols ;
01200    float sum , smax=0.0f ;
01201 
01202    for (i = 0;  i < rows;  i++){
01203      sum = 0.0f ;
01204      for (j = 0;  j < cols;  j++) sum += fabs(a.elts[i][j]) ;
01205      if( sum > smax ) smax = sum ;
01206    }
01207    return smax ;
01208 }
01209 
01210 /*---------------------------------------------------------------------------*/
01211 /*! Search a matrix for nearly identical column pairs, where "nearly identical"
01212     means they are correlated closer than 1-eps.
01213 
01214     Return is a pointer to an int array of the form
01215       [ i1 j1 i2 j2 ... -1 -1 ]
01216     where columns (i1,j1) are nearly the same, (i2,j2) also, etc.
01217     In addition:
01218      - A pair (i,-1) indicates that column #i is all zeros.
01219      - The array is terminated with the pair (-1,-1).
01220      - If there are no bad column pairs or all-zero columns, NULL is returned.
01221      - Pairs of all-zero columns are NOT reported.
01222      - The array should be free()-ed when you are done with it.
01223 -----------------------------------------------------------------------------*/
01224 
01225 int * matrix_check_columns( matrix a , double eps )
01226 {
01227    int i,j,k , rows=a.rows , cols=a.cols ;
01228    int *iar=NULL , nar=0 ;
01229    double sumi,sumj,sumd ;
01230 
01231    if( eps <= 0.0f ) eps = 1.e-5 ;
01232 
01233    for( i=0 ; i < cols ; i++ ){
01234      sumi = 0.0 ;
01235      for( k=0 ; k < rows ; k++ ) sumi += a.elts[k][i] * a.elts[k][i] ;
01236      if( sumi <= 0.0 ){
01237        iar = (int *)realloc( (void *)iar , sizeof(int)*2*(nar+1) ) ;
01238        iar[2*nar] = i ; iar[2*nar+1] = -1 ; nar++ ;
01239        continue ;                           /* skip to next column i */
01240      }
01241      for( j=i+1 ; j < cols ; j++ ){
01242        sumj = sumd = 0.0 ;
01243        for( k=0 ; k < rows ; k++ ){
01244          sumj += a.elts[k][j] * a.elts[k][j] ;
01245          sumd += a.elts[k][j] * a.elts[k][i] ;
01246        }
01247        if( sumj > 0.0 ){
01248          sumd = fabs(sumd) / sqrt(sumi*sumj) ;
01249          if( sumd >= 1.0-eps ){
01250            iar = (int *)realloc( (void *)iar , sizeof(int)*2*(nar+1) ) ;
01251            iar[2*nar] = i ; iar[2*nar+1] = j ; nar++ ;
01252          }
01253        }
01254      }
01255    }
01256 
01257    if( iar != NULL ){
01258      iar = (int *)realloc( (void *)iar , sizeof(int)*2*(nar+1) ) ;
01259      iar[2*nar] = iar[2*nar+1] = -1 ;
01260    }
01261 
01262    return iar ;
01263 }
01264 
01265 /*---------------------------------------------------------------------------*/
01266 /*! Return the eigenvalues of matrix X-transpose X.
01267     The output points to a vector of doubles, of length X.cols.  This
01268     should be free()-ed when you are done with it.
01269 -----------------------------------------------------------------------------*/
01270 
01271 double * matrix_singvals( matrix X )
01272 {
01273    int i,j,k , M=X.rows , N=X.cols ;
01274    double *a , *e , sum ;
01275 
01276    a = (double *) malloc( sizeof(double)*N*N ) ;
01277    e = (double *) malloc( sizeof(double)*N   ) ;
01278 
01279    for( i=0 ; i < N ; i++ ){
01280      for( j=0 ; j <= i ; j++ ){
01281        sum = 0.0 ;
01282        for( k=0 ; k < M ; k++ ) sum += X.elts[k][i] * X.elts[k][j] ;
01283        a[j+N*i] = sum ;
01284        if( j < i ) a[i+N*j] = sum ;
01285      }
01286    }
01287 
01288    for( i=0 ; i < N ; i++ ){
01289      if( a[i+N*i] > 0.0 ) e[i] = 1.0 / sqrt(a[i+N*i]) ;
01290      else                 e[i] = 1.0 ;
01291    }
01292 
01293    for( i=0 ; i < N ; i++ ){
01294      for( j=0 ; j < N ; j++ ) a[j+N*i] *= sqrt(e[i]*e[j]) ;
01295    }
01296 
01297    symeigval_double( N , a , e ) ;
01298    free( (void *)a ) ;
01299    return e ;
01300 }
01301 
01302 /*---------------------------------------------------------------------------*/
01303 
01304 extern void svd_double( int, int, double *, double *, double *, double * ) ;
01305 
01306 /*---------------------------------------------------------------------------*/
01307 /*! Given MxN matrix X, return the NxN matrix
01308 
01309        [  T  ]-1                       [  T  ]-1 T
01310        [ X X ]     and the NxM matrix  [ X X ]  X
01311 -----------------------------------------------------------------------------*/
01312 
01313 void matrix_psinv( matrix X , matrix *XtXinv , matrix *XtXinvXt )
01314 {
01315    int m = X.rows , n = X.cols , ii,jj,kk ;
01316    double *amat , *umat , *vmat , *sval , *xfac , smax,del,sum ;
01317 
01318    if( m < 1 || n < 1 || m < n || (XtXinv == NULL && XtXinvXt == NULL) ) return;
01319 
01320    amat = (double *)calloc( sizeof(double),m*n ) ;  /* input matrix */
01321    umat = (double *)calloc( sizeof(double),m*n ) ;  /* left singular vectors */
01322    vmat = (double *)calloc( sizeof(double),n*n ) ;  /* right singular vectors */
01323    sval = (double *)calloc( sizeof(double),n   ) ;  /* singular values */
01324    xfac = (double *)calloc( sizeof(double),n   ) ;  /* column norms of [a] */
01325 
01326 #define A(i,j) amat[(i)+(j)*m]
01327 #define U(i,j) umat[(i)+(j)*m]
01328 #define V(i,j) vmat[(i)+(j)*n]
01329 
01330    /* copy input matrix into amat */
01331 
01332    for( ii=0 ; ii < m ; ii++ )
01333      for( jj=0 ; jj < n ; jj++ ) A(ii,jj) = X.elts[ii][jj] ;
01334 
01335    /* scale each column to have norm 1 */
01336 
01337    for( jj=0 ; jj < n ; jj++ ){
01338      sum = 0.0 ;
01339      for( ii=0 ; ii < m ; ii++ ) sum += A(ii,jj)*A(ii,jj) ;
01340      if( sum > 0.0 ) sum = 1.0/sqrt(sum) ;
01341      xfac[jj] = sum ;
01342      for( ii=0 ; ii < m ; ii++ ) A(ii,jj) *= sum ;
01343    }
01344 
01345    /* compute SVD of scaled matrix */
01346 
01347    svd_double( m , n , amat , sval , umat , vmat ) ; 
01348 
01349    free((void *)amat) ;  /* done with this */
01350 
01351    /* find largest singular value */
01352 
01353    smax = sval[0] ;
01354    for( ii=1 ; ii < n ; ii++ )
01355      if( sval[ii] > smax ) smax = sval[ii] ;
01356 
01357    if( smax <= 0.0 ){                        /* this is bad */
01358      free((void *)xfac); free((void *)sval);
01359      free((void *)vmat); free((void *)umat); return;
01360    }
01361 
01362    for( ii=0 ; ii < n ; ii++ )
01363      if( sval[ii] < 0.0 ) sval[ii] = 0.0 ;
01364 
01365 #ifdef FLOATIZE
01366 #define PSINV_EPS 1.e-8
01367 #else
01368 #define PSINV_EPS 1.e-16
01369 #endif
01370 
01371    /* "reciprocals" of singular values:  1/s is actually s/(s^2+del) */
01372 
01373    del  = PSINV_EPS * smax*smax ;
01374    for( ii=0 ; ii < n ; ii++ )
01375      sval[ii] = sval[ii] / ( sval[ii]*sval[ii] + del ) ;
01376 
01377    /* create pseudo-inverse */
01378 
01379    if( XtXinvXt != NULL ){
01380      matrix_create( n , m , XtXinvXt ) ;
01381      for( ii=0 ; ii < n ; ii++ ){
01382        for( jj=0 ; jj < m ; jj++ ){
01383          sum = 0.0 ;
01384          for( kk=0 ; kk < n ; kk++ )
01385            sum += sval[kk] * V(ii,kk) * U(jj,kk) ;
01386          XtXinvXt->elts[ii][jj] = sum * xfac[ii] ;
01387        }
01388      }
01389    }
01390 
01391    if( XtXinv != NULL ){
01392      matrix_create( n , n , XtXinv ) ;
01393      for( ii=0 ; ii < n ; ii++ ) sval[ii] = sval[ii] * sval[ii] ;
01394      matrix_create( n , n , XtXinv ) ;
01395      for( ii=0 ; ii < n ; ii++ ){
01396        for( jj=0 ; jj < n ; jj++ ){
01397          sum = 0.0 ;
01398          for( kk=0 ; kk < n ; kk++ )
01399            sum += sval[kk] * V(ii,kk) * V(jj,kk) ;
01400          XtXinv->elts[ii][jj] = sum * xfac[ii] * xfac[jj] ;
01401        }
01402      }
01403    }
01404 
01405    free((void *)xfac); free((void *)sval);
01406    free((void *)vmat); free((void *)umat); return;
01407 }