LaRCsim/ls_matrix.c

   1 /***************************************************************************
   2
   3         TITLE:          ls_matrix.c
   4
   5 ----------------------------------------------------------------------------
   6
   7         FUNCTION:       general real matrix routines; includes
   8                                 gaussj() for matrix inversion using
   9                                 Gauss-Jordan method with full pivoting.
  10
  11         The routines in this module have come more or less from ref [1].
  12         Note that, probably due to the heritage of ref [1] (which has a
  13         FORTRAN version that was probably written first), the use of 1 as
  14         the first element of an array (or vector) is used. This is accomplished
  15         in memory by allocating, but not using, the 0 elements in each dimension.
  16         While this wastes some memory, it allows the routines to be ported more
  17         easily from FORTRAN (I suspect) as well as adhering to conventional
  18         matrix notation.  As a result, however, traditional ANSI C convention
  19         (0-base indexing) is not followed; as the authors of ref [1] point out,
  20         there is some question of the portability of the resulting routines
  21         which sometimes access negative indexes. See ref [1] for more details.
  22
  23 ----------------------------------------------------------------------------
  24
  25         MODULE STATUS:  developmental
  26
  27 ----------------------------------------------------------------------------
  28
  29         GENEALOGY:      Created 950222 E. B. Jackson
  30
  31 ----------------------------------------------------------------------------
  32
  33         DESIGNED BY:    from Numerical Recipes in C, by Press, et. al.
  34
  35         CODED BY:       Bruce Jackson
  36
  37         MAINTAINED BY:
  38
  39 ----------------------------------------------------------------------------
  40
  41         MODIFICATION HISTORY:
  42
  43         DATE    PURPOSE                                         BY
  44
  45         CURRENT RCS HEADER:
  46
  47 $Header$
  48 $Log$
  49 Revision 1.1  1998/06/27 22:34:57  curt
  50 Initial revision.
  51
  52  * Revision 1.1  1995/02/27  19:55:44  bjax
  53  * Initial revision
  54  *
  55
  56 ----------------------------------------------------------------------------
  57
  58         REFERENCES:     [1] Press, William H., et. al, Numerical Recipes in
  59                             C, 2nd edition, Cambridge University Press, 1992
  60
  61 ----------------------------------------------------------------------------
  62
  63         CALLED BY:
  64
  65 ----------------------------------------------------------------------------
  66
  67         CALLS TO:
  68
  69 ----------------------------------------------------------------------------
  70
  71         INPUTS:
  72
  73 ----------------------------------------------------------------------------
  74
  75         OUTPUTS:
  76
  77 --------------------------------------------------------------------------*/
  78
  79 #ifdef HAVE_CONFIG_H
  80 #  include <config.h>
  81 #endif
  82
  83 #include <stdlib.h>
  84 #include <stdio.h>
  85 #include <math.h>
  86
  87 #ifdef HAVE_UNISTD_H
  88 #  include <unistd.h>
  89 #endif
  90
  91 #include "ls_matrix.h"
  92
  93
  94 #define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
  95
  96 static char rcsid[] = "$Id$";
  97
  98
  99 int *nr_ivector(long nl, long nh)
 100 {
 101     int *v;
 102
 103     v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
 104     return v-nl+NR_END;
 105 }
 106
 107
 108
 109 double **nr_matrix(long nrl, long nrh, long ncl, long nch)
 110 /* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
 111 {
 112     long i, nrow=nrh-nrl+1, ncol=nch-ncl+1;
 113     double **m;
 114
 115     /* allocate pointers to rows */
 116     m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
 117
 118     if (!m)
 119         {
 120             fprintf(stderr, "Memory failure in routine 'nr_matrix'.\n");
 121             exit(1);
 122         }
 123
 124     m += NR_END;
 125     m -= nrl;
 126
 127     /* allocate rows and set pointers to them */
 128     m[nrl] = (double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
 129     if (!m[nrl])
 130         {
 131             fprintf(stderr, "Memory failure in routine 'matrix'\n");
 132             exit(1);
 133         }
 134
 135     m[nrl] += NR_END;
 136     m[nrl] -= ncl;
 137
 138     for (i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
 139
 140     /* return pointer to array of pointers to rows */
 141     return m;
 142 }
 143
 144
 145 void nr_free_ivector(int *v, long nl /* , long nh */)
 146 {
 147     free( (char *) (v+nl-NR_END));
 148 }
 149
 150
 151 void nr_free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
 152 /* free a double matrix allocated by nr_matrix() */
 153 {
 154     free((char *) (m[nrl]+ncl-NR_END));
 155     free((char *) (m+nrl-NR_END));
 156 }
 157
 158
 159 int nr_gaussj(double **a, int n, double **b, int m)
 160
 161 /* Linear equation solution by Gauss-Jordan elimination. a[1..n][1..n] is */
 162 /* the input matrix. b[1..n][1..m] is input containing the m right-hand   */
 163 /* side vectors. On output, a is replaced by its matrix invers, and b is  */
 164 /* replaced by the corresponding set of solution vectors.                 */
 165
 166 /* Note: this routine modified by EBJ to make b optional, if m == 0 */
 167
 168 {
 169     int         *indxc, *indxr, *ipiv;
 170     int         i, icol, irow, j, k, l, ll;
 171     double       big, dum, pivinv, temp;
 172
 173     int         bexists = ((m != 0) || (b == 0));
 174
 175     indxc = nr_ivector(1,n);    /* The integer arrays ipiv, indxr, and  */
 176     indxr = nr_ivector(1,n);    /* indxc are used for pivot bookkeeping */
 177     ipiv  = nr_ivector(1,n);
 178
 179     for (j=1;j<=n;j++) ipiv[j] = 0;
 180
 181     for (i=1;i<=n;i++)          /* This is the main loop over columns   */
 182         {
 183             big = 0.0;
 184             for (j=1;j<=n;j++)  /* This is outer loop of pivot search   */
 185                 if (ipiv[j] != 1)
 186                     for (k=1;k<=n;k++)
 187                         {
 188                             if (ipiv[k] == 0)
 189                                 {
 190                                     if (fabs(a[j][k]) >= big)
 191                                         {
 192                                             big = fabs(a[j][k]);
 193                                             irow = j;
 194                                             icol = k;
 195                                         }
 196                                 }
 197                             else
 198                                 if (ipiv[k] > 1) return -1;
 199                         }
 200             ++(ipiv[icol]);
 201
 202 /*    We now have the pivot element, so we interchange rows, if needed,  */
 203 /* to put the pivot element on the diagonal.  The columns are not        */
 204 /* physically interchanged, only relabeled: indxc[i], the column of the  */
 205 /* ith pivot element, is the ith column that is reduced, while indxr[i]  */
 206 /* is the row in which that pivot element was orignally located. If      */
 207 /* indxr[i] != indxc[i] there is an implied column interchange.  With    */
 208 /* this form of bookkeeping, the solution b's will end up in the correct */
 209 /* order, and the inverse matrix will be scrambed by columns.            */
 210
 211             if (irow != icol)
 212                 {
 213 /*                  for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */
 214                         for (l=1;l<=n;l++)
 215                           {
 216                                 temp=a[irow][l];
 217                                 a[irow][l]=a[icol][l];
 218                                 a[icol][l]=temp;
 219                           }
 220                     if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
 221                 }
 222             indxr[i] = irow;    /* We are now ready to divide the pivot row */
 223             indxc[i] = icol;    /* by the pivot element, a[irow][icol]      */
 224             if (a[icol][icol] == 0.0) return -1;
 225             pivinv = 1.0/a[icol][icol];
 226             a[icol][icol] = 1.0;
 227             for (l=1;l<=n;l++) a[icol][l] *= pivinv;
 228             if (bexists) for (l=1;l<=m;l++) b[icol][l] *= pivinv;
 229             for (ll=1;ll<=n;ll++)       /* Next, we reduce the rows...  */
 230                 if (ll != icol )        /* .. except for the pivot one  */
 231                     {
 232                         dum = a[ll][icol];
 233                         a[ll][icol] = 0.0;
 234                         for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
 235            if (bexists) for (l=1;l<=m;l++) b[ll][i] -= b[icol][l]*dum;
 236                     }
 237         }
 238
 239 /* This is the end of the mail loop over columns of the reduction. It
 240        only remains to unscrambled the solution in view of the column
 241        interchanges. We do this by interchanging pairs of columns in
 242        the reverse order that the permutation was built up. */
 243
 244     for (l=n;l>=1;l--)
 245         {
 246             if (indxr[l] != indxc[l])
 247                 for (k=1;k<=n;k++)
 248                     SWAP(a[k][indxr[l]],a[k][indxc[l]])
 249         }
 250
 251 /* and we are done */
 252
 253     nr_free_ivector(ipiv,1 /*,n*/ );
 254     nr_free_ivector(indxr,1 /*,n*/ );
 255     nr_free_ivector(indxc,1 /*,n*/ );
 256
 257     return 0;   /* indicate success */
 258 }
 259
 260 void nr_copymat(double **orig, int n, double **copy)
 261 /* overwrites matrix 'copy' with copy of matrix 'orig' */
 262 {
 263         long i, j;
 264
 265         if ((orig==0)||(copy==0)||(n==0)) return;
 266
 267         for (i=1;i<=n;i++)
 268                 for (j=1;j<=n;j++)
 269                         copy[i][j] = orig[i][j];
 270 }
 271
 272 void nr_multmat(double **m1, int n, double **m2, double **prod)
 273 {
 274         long i, j, k;
 275
 276         if ((m1==0)||(m2==0)||(prod==0)||(n==0)) return;
 277
 278         for (i=1;i<=n;i++)
 279                 for (j=1;j<=n;j++)
 280                         {
 281                                 prod[i][j] = 0.0;
 282                                 for(k=1;k<=n;k++) prod[i][j] += m1[i][k]*m2[k][j];
 283                         }
 284 }
 285
 286
 287
 288 void nr_printmat(double **a, int n)
 289 {
 290     int i,j;
 291
 292     printf("\n");
 293     for(i=1;i<=n;i++)
 294       {
 295           for(j=1;j<=n;j++)
 296               printf("% 9.4f ", a[i][j]);
 297           printf("\n");
 298       }
 299     printf("\n");
 300
 301 }
 302
 303
 304 void testmat( void ) /* main() for test purposes */
 305 {
 306     double **mat1, **mat2, **mat3;
 307     double invmaxlong;
 308     int loop, i, j, n = 20;
 309     long maxlong = RAND_MAX;
 310     int maxloop = 2;
 311
 312     invmaxlong = 1.0/(double)maxlong;
 313     mat1 = nr_matrix(1, n, 1, n );
 314     mat2 = nr_matrix(1, n, 1, n );
 315     mat3 = nr_matrix(1, n, 1, n );
 316
 317 /*    for(i=1;i<=n;i++) mat1[i][i]= 5.0; */
 318
 319         for(loop=0;loop<maxloop;loop++)
 320         {
 321             if (loop != 0)
 322                 for(i=1;i<=n;i++)
 323                     for(j=1;j<=n;j++)
 324                         mat1[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
 325
 326                 printf("Original matrix:\n");
 327             nr_printmat( mat1, n );
 328
 329             nr_copymat( mat1, n, mat2 );
 330
 331             i = nr_gaussj( mat2, n, 0, 0 );
 332
 333             if (i) printf("Singular matrix.\n");
 334
 335                 printf("Inverted matrix:\n");
 336             nr_printmat( mat2, n );
 337
 338             nr_multmat( mat1, n, mat2, mat3 );
 339
 340             printf("Original multiplied by inverse:\n");
 341             nr_printmat( mat3, n );
 342
 343             if (loop < maxloop-1) /* sleep(1) */;
 344         }
 345
 346     nr_free_matrix( mat1, 1, n, 1, n );
 347     nr_free_matrix( mat2, 1, n, 1, n );
 348     nr_free_matrix( mat3, 1, n, 1, n );
 349 }