src/FDM/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.2  2004/04/01 15:27:55  curt
  50 Clean up various compiler warnings that have crept into the code.  This
  51 by no means get's them all, but it's a start.
  52
  53 Revision 1.1.1.1  2002/09/10 01:14:02  curt
  54 Initial revision of FlightGear-0.9.0
  55
  56 Revision 1.1.1.1  1999/06/17 18:07:34  curt
  57 Start of 0.7.x branch
  58
  59 Revision 1.1.1.1  1999/04/05 21:32:45  curt
  60 Start of 0.6.x branch.
  61
  62 Revision 1.1  1998/06/27 22:34:57  curt
  63 Initial revision.
  64
  65  * Revision 1.1  1995/02/27  19:55:44  bjax
  66  * Initial revision
  67  *
  68
  69 ----------------------------------------------------------------------------
  70
  71         REFERENCES:     [1] Press, William H., et. al, Numerical Recipes in
  72                             C, 2nd edition, Cambridge University Press, 1992
  73
  74 ----------------------------------------------------------------------------
  75
  76         CALLED BY:
  77
  78 ----------------------------------------------------------------------------
  79
  80         CALLS TO:
  81
  82 ----------------------------------------------------------------------------
  83
  84         INPUTS:
  85
  86 ----------------------------------------------------------------------------
  87
  88         OUTPUTS:
  89
  90 --------------------------------------------------------------------------*/
  91
  92 #ifdef HAVE_CONFIG_H
  93 #  include <config.h>
  94 #endif
  95
  96 #include <stdlib.h>
  97 #include <stdio.h>
  98 #include <math.h>
  99
 100 #ifdef HAVE_UNISTD_H
 101 #  include <unistd.h>
 102 #endif
 103
 104 #include "ls_matrix.h"
 105
 106
 107 #define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
 108
 109 static char rcsid[] = "$Id$";
 110
 111
 112 int *nr_ivector(long nl, long nh)
 113 {
 114     int *v;
 115
 116     v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
 117     return v-nl+NR_END;
 118 }
 119
 120
 121
 122 double **nr_matrix(long nrl, long nrh, long ncl, long nch)
 123 /* allocate a double matrix with subscript range m[nrl..nrh][ncl..nch] */
 124 {
 125     long i, nrow=nrh-nrl+1, ncol=nch-ncl+1;
 126     double **m;
 127
 128     /* allocate pointers to rows */
 129     m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
 130
 131     if (!m)
 132         {
 133             fprintf(stderr, "Memory failure in routine 'nr_matrix'.\n");
 134             exit(1);
 135         }
 136
 137     m += NR_END;
 138     m -= nrl;
 139
 140     /* allocate rows and set pointers to them */
 141     m[nrl] = (double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
 142     if (!m[nrl])
 143         {
 144             fprintf(stderr, "Memory failure in routine 'matrix'\n");
 145             exit(1);
 146         }
 147
 148     m[nrl] += NR_END;
 149     m[nrl] -= ncl;
 150
 151     for (i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
 152
 153     /* return pointer to array of pointers to rows */
 154     return m;
 155 }
 156
 157
 158 void nr_free_ivector(int *v, long nl /* , long nh */)
 159 {
 160     free( (char *) (v+nl-NR_END));
 161 }
 162
 163
 164 void nr_free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
 165 /* free a double matrix allocated by nr_matrix() */
 166 {
 167     free((char *) (m[nrl]+ncl-NR_END));
 168     free((char *) (m+nrl-NR_END));
 169 }
 170
 171
 172 int nr_gaussj(double **a, int n, double **b, int m)
 173
 174 /* Linear equation solution by Gauss-Jordan elimination. a[1..n][1..n] is */
 175 /* the input matrix. b[1..n][1..m] is input containing the m right-hand   */
 176 /* side vectors. On output, a is replaced by its matrix invers, and b is  */
 177 /* replaced by the corresponding set of solution vectors.                 */
 178
 179 /* Note: this routine modified by EBJ to make b optional, if m == 0 */
 180
 181 {
 182     int         *indxc, *indxr, *ipiv;
 183     int         i, icol = 0, irow = 0, j, k, l, ll;
 184     double       big, dum, pivinv, temp;
 185
 186     int         bexists = ((m != 0) || (b == 0));
 187
 188     indxc = nr_ivector(1,n);    /* The integer arrays ipiv, indxr, and  */
 189     indxr = nr_ivector(1,n);    /* indxc are used for pivot bookkeeping */
 190     ipiv  = nr_ivector(1,n);
 191
 192     for (j=1;j<=n;j++) ipiv[j] = 0;
 193
 194     for (i=1;i<=n;i++)          /* This is the main loop over columns   */
 195         {
 196             big = 0.0;
 197             for (j=1;j<=n;j++)  /* This is outer loop of pivot search   */
 198                 if (ipiv[j] != 1)
 199                     for (k=1;k<=n;k++)
 200                         {
 201                             if (ipiv[k] == 0)
 202                                 {
 203                                     if (fabs(a[j][k]) >= big)
 204                                         {
 205                                             big = fabs(a[j][k]);
 206                                             irow = j;
 207                                             icol = k;
 208                                         }
 209                                 }
 210                             else
 211                                 if (ipiv[k] > 1) return -1;
 212                         }
 213             ++(ipiv[icol]);
 214
 215 /*    We now have the pivot element, so we interchange rows, if needed,  */
 216 /* to put the pivot element on the diagonal.  The columns are not        */
 217 /* physically interchanged, only relabeled: indxc[i], the column of the  */
 218 /* ith pivot element, is the ith column that is reduced, while indxr[i]  */
 219 /* is the row in which that pivot element was orignally located. If      */
 220 /* indxr[i] != indxc[i] there is an implied column interchange.  With    */
 221 /* this form of bookkeeping, the solution b's will end up in the correct */
 222 /* order, and the inverse matrix will be scrambed by columns.            */
 223
 224             if (irow != icol)
 225                 {
 226 /*                  for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */
 227                         for (l=1;l<=n;l++)
 228                           {
 229                                 temp=a[irow][l];
 230                                 a[irow][l]=a[icol][l];
 231                                 a[icol][l]=temp;
 232                           }
 233                     if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
 234                 }
 235             indxr[i] = irow;    /* We are now ready to divide the pivot row */
 236             indxc[i] = icol;    /* by the pivot element, a[irow][icol]      */
 237             if (a[icol][icol] == 0.0) return -1;
 238             pivinv = 1.0/a[icol][icol];
 239             a[icol][icol] = 1.0;
 240             for (l=1;l<=n;l++) a[icol][l] *= pivinv;
 241             if (bexists) for (l=1;l<=m;l++) b[icol][l] *= pivinv;
 242             for (ll=1;ll<=n;ll++)       /* Next, we reduce the rows...  */
 243                 if (ll != icol )        /* .. except for the pivot one  */
 244                     {
 245                         dum = a[ll][icol];
 246                         a[ll][icol] = 0.0;
 247                         for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
 248            if (bexists) for (l=1;l<=m;l++) b[ll][i] -= b[icol][l]*dum;
 249                     }
 250         }
 251
 252 /* This is the end of the mail loop over columns of the reduction. It
 253        only remains to unscrambled the solution in view of the column
 254        interchanges. We do this by interchanging pairs of columns in
 255        the reverse order that the permutation was built up. */
 256
 257     for (l=n;l>=1;l--)
 258         {
 259             if (indxr[l] != indxc[l])
 260                 for (k=1;k<=n;k++)
 261                     SWAP(a[k][indxr[l]],a[k][indxc[l]])
 262         }
 263
 264 /* and we are done */
 265
 266     nr_free_ivector(ipiv,1 /*,n*/ );
 267     nr_free_ivector(indxr,1 /*,n*/ );
 268     nr_free_ivector(indxc,1 /*,n*/ );
 269
 270     return 0;   /* indicate success */
 271 }
 272
 273 void nr_copymat(double **orig, int n, double **copy)
 274 /* overwrites matrix 'copy' with copy of matrix 'orig' */
 275 {
 276         long i, j;
 277
 278         if ((orig==0)||(copy==0)||(n==0)) return;
 279
 280         for (i=1;i<=n;i++)
 281                 for (j=1;j<=n;j++)
 282                         copy[i][j] = orig[i][j];
 283 }
 284
 285 void nr_multmat(double **m1, int n, double **m2, double **prod)
 286 {
 287         long i, j, k;
 288
 289         if ((m1==0)||(m2==0)||(prod==0)||(n==0)) return;
 290
 291         for (i=1;i<=n;i++)
 292                 for (j=1;j<=n;j++)
 293                         {
 294                                 prod[i][j] = 0.0;
 295                                 for(k=1;k<=n;k++) prod[i][j] += m1[i][k]*m2[k][j];
 296                         }
 297 }
 298
 299
 300
 301 void nr_printmat(double **a, int n)
 302 {
 303     int i,j;
 304
 305     printf("\n");
 306     for(i=1;i<=n;i++)
 307       {
 308           for(j=1;j<=n;j++)
 309               printf("% 9.4f ", a[i][j]);
 310           printf("\n");
 311       }
 312     printf("\n");
 313
 314 }
 315
 316
 317 void testmat( void ) /* main() for test purposes */
 318 {
 319     double **mat1, **mat2, **mat3;
 320     double invmaxlong;
 321     int loop, i, j, n = 20;
 322     long maxlong = RAND_MAX;
 323     int maxloop = 2;
 324
 325     invmaxlong = 1.0/(double)maxlong;
 326     mat1 = nr_matrix(1, n, 1, n );
 327     mat2 = nr_matrix(1, n, 1, n );
 328     mat3 = nr_matrix(1, n, 1, n );
 329
 330 /*    for(i=1;i<=n;i++) mat1[i][i]= 5.0; */
 331
 332         for(loop=0;loop<maxloop;loop++)
 333         {
 334             if (loop != 0)
 335                 for(i=1;i<=n;i++)
 336                     for(j=1;j<=n;j++)
 337                         mat1[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
 338
 339                 printf("Original matrix:\n");
 340             nr_printmat( mat1, n );
 341
 342             nr_copymat( mat1, n, mat2 );
 343
 344             i = nr_gaussj( mat2, n, 0, 0 );
 345
 346             if (i) printf("Singular matrix.\n");
 347
 348                 printf("Inverted matrix:\n");
 349             nr_printmat( mat2, n );
 350
 351             nr_multmat( mat1, n, mat2, mat3 );
 352
 353             printf("Original multiplied by inverse:\n");
 354             nr_printmat( mat3, n );
 355
 356             if (loop < maxloop-1) /* sleep(1) */;
 357         }
 358
 359     nr_free_matrix( mat1, 1, n, 1, n );
 360     nr_free_matrix( mat2, 1, n, 1, n );
 361     nr_free_matrix( mat3, 1, n, 1, n );
 362 }