00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062 #include "mri_image.h"
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
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081 #undef SETUP_BLAS
00082 #undef DOTP
00083 #undef VSUB
00084
00085 #if defined(USE_ALTIVEC)
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)
00092
00093 # include <scsl_blas.h>
00094 # define SETUP_BLAS
00095
00096 #endif
00097
00098
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
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
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
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
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) ;
00189 #endif
00190 }
00191
00192
00193
00194
00195
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
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
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
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
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
00304
00305
00306
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;
00315
00316 float * far;
00317 char message [80];
00318
00319
00320
00321 if (filename == NULL) matrix_error ("Missing matrix file name");
00322
00323
00324
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
00340 far = MRI_FLOAT_PTR(flim);
00341
00342
00343
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
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
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
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 ) ;
00412 #endif
00413 }
00414 }
00415
00416
00417
00418
00419
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
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
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
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
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
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
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
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
00604
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];
00650 for (j = 0; j < n; j++)
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
00674
00675
00676
00677 int matrix_inverse_dsc (matrix a, matrix * ainv)
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 ;
00695 diag[i] = 1.0 / sqrt(diag[i]) ;
00696 }
00697
00698 for( i=0 ; i < n ; i++ )
00699 for( j=0 ; j < n ; j++ )
00700 atmp.elts[i][j] *= diag[i]*diag[j] ;
00701
00702 mir = matrix_inverse( atmp , ainv ) ;
00703
00704 for( i=0 ; i < n ; i++ )
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
00716
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
00782
00783
00784 void vector_initialize (vector * v)
00785 {
00786 v->dim = 0;
00787 v->elts = NULL;
00788 }
00789
00790
00791
00792
00793
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
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)
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
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
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
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 ) ;
00889 #endif
00890 }
00891
00892
00893
00894
00895
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
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
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
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
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
00991
00992
00993
00994
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 ){
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 {
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
01056
01057 #endif
01058
01059 }
01060
01061
01062
01063
01064
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 ){
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 {
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
01134
01135 #endif
01136
01137 return qsum ;
01138 }
01139
01140
01141
01142
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
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
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
01212
01213
01214
01215
01216
01217
01218
01219
01220
01221
01222
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 ;
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
01267
01268
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
01308
01309
01310
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 ) ;
01321 umat = (double *)calloc( sizeof(double),m*n ) ;
01322 vmat = (double *)calloc( sizeof(double),n*n ) ;
01323 sval = (double *)calloc( sizeof(double),n ) ;
01324 xfac = (double *)calloc( sizeof(double),n ) ;
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
01331
01332 for( ii=0 ; ii < m ; ii++ )
01333 for( jj=0 ; jj < n ; jj++ ) A(ii,jj) = X.elts[ii][jj] ;
01334
01335
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
01346
01347 svd_double( m , n , amat , sval , umat , vmat ) ;
01348
01349 free((void *)amat) ;
01350
01351
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 ){
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
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
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 }