1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: general real matrix routines; includes
8 gaussj() for matrix inversion using
9 Gauss-Jordan method with full pivoting.
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.
23 ----------------------------------------------------------------------------
25 MODULE STATUS: developmental
27 ----------------------------------------------------------------------------
29 GENEALOGY: Created 950222 E. B. Jackson
31 ----------------------------------------------------------------------------
33 DESIGNED BY: from Numerical Recipes in C, by Press, et. al.
35 CODED BY: Bruce Jackson
39 ----------------------------------------------------------------------------
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.
53 Revision 1.1.1.1 2002/09/10 01:14:02 curt
54 Initial revision of FlightGear-0.9.0
56 Revision 1.1.1.1 1999/06/17 18:07:34 curt
59 Revision 1.1.1.1 1999/04/05 21:32:45 curt
60 Start of 0.6.x branch.
62 Revision 1.1 1998/06/27 22:34:57 curt
65 * Revision 1.1 1995/02/27 19:55:44 bjax
69 ----------------------------------------------------------------------------
71 REFERENCES: [1] Press, William H., et. al, Numerical Recipes in
72 C, 2nd edition, Cambridge University Press, 1992
74 ----------------------------------------------------------------------------
78 ----------------------------------------------------------------------------
82 ----------------------------------------------------------------------------
86 ----------------------------------------------------------------------------
90 --------------------------------------------------------------------------*/
104 #include "ls_matrix.h"
107 #define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}
109 // static char rcsid[] = "$Id$";
112 int *nr_ivector(long nl, long nh)
116 v=(int *)malloc((size_t) ((nh-nl+1+NR_END)*sizeof(int)));
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] */
125 long i, nrow=nrh-nrl+1, ncol=nch-ncl+1;
128 /* allocate pointers to rows */
129 m=(double **) malloc((size_t)((nrow+NR_END)*sizeof(double*)));
133 fprintf(stderr, "Memory failure in routine 'nr_matrix'.\n");
140 /* allocate rows and set pointers to them */
141 m[nrl] = (double *) malloc((size_t)((nrow*ncol+NR_END)*sizeof(double)));
144 fprintf(stderr, "Memory failure in routine 'matrix'\n");
151 for (i=nrl+1;i<=nrh;i++) m[i]=m[i-1]+ncol;
153 /* return pointer to array of pointers to rows */
158 void nr_free_ivector(int *v, long nl /* , long nh */)
160 free( (char *) (v+nl-NR_END));
164 void nr_free_matrix(double **m, long nrl, long nrh, long ncl, long nch)
165 /* free a double matrix allocated by nr_matrix() */
167 free((char *) (m[nrl]+ncl-NR_END));
168 free((char *) (m+nrl-NR_END));
172 int nr_gaussj(double **a, int n, double **b, int m)
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. */
179 /* Note: this routine modified by EBJ to make b optional, if m == 0 */
182 int *indxc, *indxr, *ipiv;
183 int i, icol = 0, irow = 0, j, k, l, ll;
184 double big, dum, pivinv, temp;
186 int bexists = ((m != 0) || (b == 0));
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);
192 for (j=1;j<=n;j++) ipiv[j] = 0;
194 for (i=1;i<=n;i++) /* This is the main loop over columns */
197 for (j=1;j<=n;j++) /* This is outer loop of pivot search */
203 if (fabs(a[j][k]) >= big)
211 if (ipiv[k] > 1) return -1;
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. */
226 /* for (l=1;1<=n;l++) SWAP(a[irow][l],a[icol][l]) */
230 a[irow][l]=a[icol][l];
233 if (bexists) for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
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];
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 */
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;
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. */
259 if (indxr[l] != indxc[l])
261 SWAP(a[k][indxr[l]],a[k][indxc[l]])
264 /* and we are done */
266 nr_free_ivector(ipiv,1 /*,n*/ );
267 nr_free_ivector(indxr,1 /*,n*/ );
268 nr_free_ivector(indxc,1 /*,n*/ );
270 return 0; /* indicate success */
273 void nr_copymat(double **orig, int n, double **copy)
274 /* overwrites matrix 'copy' with copy of matrix 'orig' */
278 if ((orig==0)||(copy==0)||(n==0)) return;
282 copy[i][j] = orig[i][j];
285 void nr_multmat(double **m1, int n, double **m2, double **prod)
289 if ((m1==0)||(m2==0)||(prod==0)||(n==0)) return;
295 for(k=1;k<=n;k++) prod[i][j] += m1[i][k]*m2[k][j];
301 void nr_printmat(double **a, int n)
309 printf("% 9.4f ", a[i][j]);
317 void testmat( void ) /* main() for test purposes */
319 double **mat1, **mat2, **mat3;
321 int loop, i, j, n = 20;
322 long maxlong = RAND_MAX;
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 );
330 /* for(i=1;i<=n;i++) mat1[i][i]= 5.0; */
332 for(loop=0;loop<maxloop;loop++)
337 mat1[i][j] = 2.0 - 4.0*invmaxlong*(double) rand();
339 printf("Original matrix:\n");
340 nr_printmat( mat1, n );
342 nr_copymat( mat1, n, mat2 );
344 i = nr_gaussj( mat2, n, 0, 0 );
346 if (i) printf("Singular matrix.\n");
348 printf("Inverted matrix:\n");
349 nr_printmat( mat2, n );
351 nr_multmat( mat1, n, mat2, mat3 );
353 printf("Original multiplied by inverse:\n");
354 nr_printmat( mat3, n );
356 if (loop < maxloop-1) /* sleep(1) */;
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 );