home *** CD-ROM | disk | FTP | other *** search
/ System Booster / System Booster.iso / Archives / GNU / gnuplot.lha / gnuplot / src / matrix.c < prev    next >
Encoding:
C/C++ Source or Header  |  1993-08-01  |  7.8 KB  |  355 lines
  1. #ifndef lint
  2. static char *RCSid = "$Id: matrix.c 1.1 1993/08/01 04:21:37 cg Exp cg $";
  3. #endif
  4.  
  5. /*
  6.  *    Matrix algebra, part of
  7.  *
  8.  *    Nonlinear least squares fit according to the
  9.  *    Marquardt-Levenberg-algorithm
  10.  *
  11.  *    added as Patch to Gnuplot (v3.2 and higher)
  12.  *    by Carsten Grammes
  13.  *    Experimental Physics, University of Saarbruecken, Germany
  14.  *
  15.  *    Internet address: cagr@rz.uni-sb.de
  16.  *
  17.  *    Copyright of this module:   Carsten Grammes, 1993
  18.  *
  19.  *    Permission to use, copy, and distribute this software and its
  20.  *    documentation for any purpose with or without fee is hereby granted,
  21.  *    provided that the above copyright notice appear in all copies and
  22.  *    that both that copyright notice and this permission notice appear
  23.  *    in supporting documentation.
  24.  *
  25.  *    This software is provided "as is" without express or implied warranty.
  26.  */
  27.  
  28.  
  29. #define MATRIX_MAIN
  30.  
  31. #include <math.h>
  32. #include <stdlib.h>
  33. #include <string.h>
  34.  
  35. #include "type.h"
  36. #include "fit.h"
  37. #include "matrix.h"
  38.  
  39.  
  40. /*****************************************************************/
  41.  
  42. #define Swap(a,b)   {double temp=(a); (a)=(b); (b)=temp;}
  43. #define WINZIG          1e-30
  44.  
  45.  
  46. /*****************************************************************
  47.     internal prototypes
  48. *****************************************************************/
  49. static void lu_decomp (double **a, int n, int *indx, double *d);
  50. static void lu_backsubst (double **a, int n, int *indx, double b[]);
  51.  
  52.  
  53. /*****************************************************************
  54.     first straightforward vector and matrix allocation functions
  55. *****************************************************************/
  56. double *vec (int n)
  57. {
  58.     /* allocates a double vector with n elements */
  59.     double *dp;
  60.     if( n < 1 )
  61.     return (double *) NULL;
  62.     if ( (dp = (double *) malloc ( n * sizeof(double) )) == NULL )
  63.     error_ex ("Memory running low during fit");
  64.     else
  65.     return dp;
  66. }
  67.  
  68.  
  69. int *ivec (int n)
  70. {
  71.     /* allocates a int vector with n elements */
  72.     int *ip;
  73.     if( n < 1 )
  74.     return (int *) NULL;
  75.     if ( (ip = (int *) malloc ( n * sizeof(int) )) == NULL )
  76.     error_ex ("Memory running low during fit");
  77.     else
  78.     return ip;
  79. }
  80.  
  81. double **matr (int rows, int cols)
  82. {
  83.     /* allocates a double matrix */
  84.  
  85.     register int i;
  86.     register double **m;
  87.  
  88.     if ( rows < 1  ||  cols < 1 )
  89.         return NULL;
  90.     /* allocate pointers to rows */
  91.     if ( (m = (double **) malloc ( rows * sizeof(double *) )) == NULL )
  92.     error_ex ("Memory running low during fit");
  93.     /* allocate rows and set pointers to them */
  94.     for ( i=0 ; i<rows ; i++ )
  95.     if ( (m[i] = (double *) malloc (cols * sizeof(double))) == NULL )
  96.         error_ex ("Memory running low during fit");
  97.     return m;
  98. }
  99.  
  100.  
  101. void free_matr (double **m, int rows)
  102. {
  103.     register int i;
  104.     for ( i=0 ; i<rows ; i++ )
  105.     free ( m[i] );
  106.     free (m);
  107. }
  108.  
  109.  
  110. void redim_vec (double **v, int n)
  111. {
  112.     if ( n < 1 ) {
  113.     *v = NULL;
  114.     return;
  115.     }
  116.     if ( (*v = (double *) realloc (*v, n * sizeof(double) )) == NULL )
  117.     error_ex ("Memory running low during fit");
  118. }
  119.  
  120. void redim_ivec (int **v, int n)
  121. {
  122.     if ( n < 1 ) {
  123.     *v = NULL;
  124.     return;
  125.     }
  126.     if ( (*v = (int *) realloc (*v, n * sizeof(int) )) == NULL )
  127.     error_ex ("Memory running low during fit");
  128. }
  129.  
  130.  
  131.  
  132. /*****************************************************************
  133.     Linear equation solution by Gauss-Jordan elimination
  134. *****************************************************************/
  135. void solve (double **a, int n, double **b, int m)
  136. {
  137.     int     *c_ix, *r_ix, *pivot_ix, *ipj, *ipk,
  138.         i, ic, ir, j, k, l, s;
  139.  
  140.     double  large, dummy, tmp, recpiv,
  141.         **ar, **rp,
  142.         *ac, *cp, *aic, *bic;
  143.  
  144.     c_ix    = ivec (n);
  145.     r_ix    = ivec (n);
  146.     pivot_ix    = ivec (n);
  147.     memset (pivot_ix, 0, n*sizeof(int));
  148.  
  149.     for ( i=0 ; i<n ; i++ ) {
  150.     large = 0.0;
  151.     ipj = pivot_ix;
  152.     ar = a;
  153.     for ( j=0  ;  j<n  ;  j++, ipj++, ar++ )
  154.         if (*ipj != 1) {
  155.         ipk = pivot_ix;
  156.         ac = *ar;
  157.         for ( k=0  ;  k<n  ;  k++, ipk++, ac++ )
  158.             if ( *ipk ) {
  159.             if ( *ipk > 1 )
  160.                             error_ex ("Singular matrix");
  161.             }
  162.             else {
  163.             if ( (tmp = fabs(*ac)) >= large ) {
  164.                 large = tmp;
  165.                 ir = j;
  166.                 ic = k;
  167.             }
  168.             }
  169.         }
  170.     ++(pivot_ix[ic]);
  171.  
  172.     if ( ic != ir ) {
  173.         ac = b[ir];
  174.         cp = b[ic];
  175.         for ( l=0  ;  l<m  ;  l++, ac++, cp++ )
  176.         Swap(*ac, *cp)
  177.         ac = a[ir];
  178.             cp = a[ic];
  179.             for ( l=0  ;  l<n  ;  l++, ac++, cp++ )
  180.                 Swap(*ac, *cp)
  181.         }
  182.  
  183.     c_ix[i] = ic;
  184.         r_ix[i] = ir;
  185.     if ( *(cp = &a[ic][ic]) == 0.0 )
  186.         error_ex ("Singular matrix");
  187.     recpiv = 1/(*cp);
  188.     *cp = 1;
  189.  
  190.     cp = b[ic];
  191.         for ( l=0 ; l<m ; l++ )
  192.             *cp++ *= recpiv;
  193.         cp = a[ic];
  194.     for ( l=0 ; l<n ; l++ )
  195.         *cp++ *= recpiv;
  196.  
  197.     ar = a;
  198.     rp = b;
  199.     for ( s=0 ; s<n ; s++, ar++, rp++ )
  200.         if ( s != ic ) {
  201.         dummy = (*ar)[ic];
  202.         (*ar)[ic] = 0;
  203.         ac = *ar;
  204.         aic = a[ic];
  205.         for ( l=0 ; l<n ; l++ )
  206.             *ac++ -= *aic++ * dummy;
  207.         cp = *rp;
  208.         bic = b[ic];
  209.         for ( l=0 ; l<m ; l++ )
  210.             *cp++ -= *bic++ * dummy;
  211.         }
  212.     }
  213.  
  214.     for ( l=n-1 ; l>=0 ; l-- )
  215.     if ( r_ix[l] != c_ix[l] )
  216.         for ( ar=a, k=0  ;    k<n  ;    k++, ar++ )
  217.         Swap ((*ar)[r_ix[l]], (*ar)[c_ix[l]])
  218.  
  219.     free (pivot_ix);
  220.     free (r_ix);
  221.     free (c_ix);
  222. }
  223.  
  224.  
  225. /*****************************************************************
  226.     LU-Decomposition of a quadratic matrix
  227. *****************************************************************/
  228. static void lu_decomp (double **a, int n, int *indx, double *d)
  229. {
  230.     int     i, imax, j, k;
  231.  
  232.     double  large, dummy, temp,
  233.         **ar, **lim,
  234.         *limc, *ac, *dp, *vscal;
  235.  
  236.     dp = vscal = vec (n);
  237.     *d = 1.0;
  238.     for ( ar=a, lim=&a[n] ; ar<lim ; ar++ ) {
  239.     large = 0.0;
  240.     for ( ac=*ar, limc=&ac[n] ; ac<limc ; )
  241.         if ( (temp = fabs (*ac++)) > large )
  242.         large = temp;
  243.     if ( large == 0.0 )
  244.         error_ex ("Singular matrix in LU-DECOMP");
  245.     *dp++ = 1/large;
  246.     }
  247.     ar = a;
  248.     for ( j=0 ; j<n ; j++, ar++ ) {
  249.     for ( i=0 ; i<j ; i++ ) {
  250.         ac = &a[i][j];
  251.         for ( k=0 ; k<i ; k++ )
  252.         *ac -= a[i][k] * a[k][j];
  253.     }
  254.     large = 0.0;
  255.     dp = &vscal[j];
  256.     for ( i=j ; i<n ; i++ ) {
  257.         ac = &a[i][j];
  258.             for ( k=0 ; k<j ; k++ )
  259.         *ac -= a[i][k] * a[k][j];
  260.         if ( (dummy = *dp++ * fabs(*ac)) >= large ) {
  261.         large = dummy;
  262.         imax = i;
  263.         }
  264.     }
  265.     if ( j != imax ) {
  266.         ac = a[imax];
  267.         dp = *ar;
  268.         for ( k=0 ; k<n ; k++, ac++, dp++ )
  269.         Swap (*ac, *dp);
  270.         *d = -(*d);
  271.         vscal[imax] = vscal[j];
  272.     }
  273.     indx[j] = imax;
  274.     if ( *(dp = &(*ar)[j]) == 0 )
  275.         *dp = WINZIG;
  276.  
  277.     if ( j != n-1 ) {
  278.         dummy = 1/(*ar)[j];
  279.         for ( i=j+1 ; i<n ; i++ )
  280.         a[i][j] *= dummy;
  281.     }
  282.     }
  283.     free (vscal);
  284. }
  285.  
  286.  
  287. /*****************************************************************
  288.     Routine for backsubstitution
  289. *****************************************************************/
  290. static void lu_backsubst (double **a, int n, int *indx, double b[])
  291. {
  292.     int     i, memi = -1, ip, j;
  293.  
  294.     double  sum, *bp, *bip, **ar, *ac;
  295.  
  296.     ar = a;
  297.     for ( i=0 ; i<n ; i++, ar++ ) {
  298.     ip = indx[i];
  299.     sum = b[ip];
  300.     b[ip] = b[i];
  301.     if (memi >= 0) {
  302.         ac = &(*ar)[memi];
  303.         bp = &b[memi];
  304.         for ( j=memi ; j<=i-1 ; j++ )
  305.         sum -= *ac++ * *bp++;
  306.     }
  307.         else
  308.         if ( sum )
  309.         memi = i;
  310.     b[i] = sum;
  311.     }
  312.     ar--;
  313.     for ( i=n-1 ; i>=0 ; i-- ) {
  314.     ac = &(*ar)[i+1];
  315.     bp = &b[i+1];
  316.     bip = &b[i];
  317.     for ( j=i+1 ; j<n ; j++ )
  318.         *bip -= *ac++ * *bp++;
  319.     *bip /= (*ar--)[i];
  320.     }
  321. }
  322.  
  323.  
  324. /*****************************************************************
  325.     matrix inversion
  326. *****************************************************************/
  327. void inverse (double **src, double **dst, int n)
  328. {
  329.     int i,j;
  330.     int *indx;
  331.     double d, *col, **tmp;
  332.  
  333.     indx = ivec (n);
  334.     col = vec (n);
  335.     tmp = matr (n, n);
  336.     for ( i=0 ; i<n ; i++ )
  337.     memcpy (tmp[i], src[i], n*sizeof(double));
  338.  
  339.     lu_decomp (tmp, n, indx, &d);
  340.  
  341.     for ( j=0 ; j<n ; j++ ) {
  342.     for ( i=0 ; i<n ; i++ )
  343.         col[i] = 0;
  344.     col[j] = 1;
  345.     lu_backsubst (tmp, n, indx, col);
  346.     for ( i=0 ; i<n ; i++ )
  347.         dst[i][j] = col[i];
  348.     }
  349.     free (indx);
  350.     free (col);
  351.     free_matr (tmp, n);
  352. }
  353.  
  354.  
  355.