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