Back to index

plt-scheme  4.2.1
matrix.c
Go to the documentation of this file.
00001 #ifndef lint
00002 static char *RCSid = "$Id: matrix.c,v 1.4 2005/03/15 23:21:26 eli Exp $";
00003 #endif
00004 
00005 /*  NOTICE: Change of Copyright Status
00006  *
00007  *  The author of this module, Carsten Grammes, has expressed in
00008  *  personal email that he has no more interest in this code, and
00009  *  doesn't claim any copyright. He has agreed to put this module
00010  *  into the public domain.
00011  *
00012  *  Lars Hecking  15-02-1999
00013  */
00014 
00015 /*
00016  *     Matrix algebra, part of
00017  *
00018  *     Nonlinear least squares fit according to the
00019  *     Marquardt-Levenberg-algorithm
00020  *
00021  *     added as Patch to Gnuplot (v3.2 and higher)
00022  *     by Carsten Grammes
00023  *     Experimental Physics, University of Saarbruecken, Germany
00024  *
00025  *     Internet address: cagr@rz.uni-sb.de
00026  *
00027  *     Copyright of this module:   Carsten Grammes, 1993
00028  *
00029  *     Permission to use, copy, and distribute this software and its
00030  *     documentation for any purpose with or without fee is hereby granted,
00031  *     provided that the above copyright notice appear in all copies and
00032  *     that both that copyright notice and this permission notice appear
00033  *     in supporting documentation.
00034  *
00035  *     This software is provided "as is" without express or implied warranty.
00036  */
00037 
00038 #define NULL 0
00039 #define null 0
00040 
00041 #include "fit.h"
00042 #include "matrix.h"
00043 #include <math.h>
00044 
00045 // create a simple gc malloc...
00046 typedef struct Node {
00047   struct Node * next;
00048   void * ptr;
00049 } Node;
00050 
00051 Node * head = null;
00052 
00053 void * my_gc_malloc(int size) {
00054   void * ptr = malloc(size);
00055   Node * n = (Node *)malloc(sizeof (Node));
00056   n->ptr = ptr;
00057   n->next = head;
00058   head = n;
00059   return ptr;
00060 }
00061 
00062 void gc_cleanup(){
00063   while(head) {
00064     Node * current = head;
00065     head = current->next;
00066     free(current->ptr);
00067     free(current);
00068   }
00069 }
00070 
00071 
00072 
00073 /*****************************************************************/
00074 
00075 #define Swap(a,b)   {double temp = (a); (a) = (b); (b) = temp;}
00076 #define WINZIG             1e-30
00077 
00078 
00079 /*****************************************************************
00080     internal prototypes
00081 *****************************************************************/
00082 
00083 static int fsign (double x);
00084 
00085 /*****************************************************************
00086     first straightforward vector and matrix allocation functions
00087 *****************************************************************/
00088 MZ_DLLEXPORT
00089 double *vec (n)
00090 int n;
00091 {
00092     /* allocates a double vector with n elements */
00093     double *dp;
00094     if( n < 1 )
00095        return (double *) NULL;
00096     dp = (double *) my_gc_malloc (n * sizeof(double));
00097     return dp;
00098 }
00099 
00100 
00101 MZ_DLLEXPORT
00102 double **matr (rows, cols)
00103 int rows;
00104 int cols;
00105 {
00106     /* allocates a double matrix */
00107 
00108     register int i;
00109     register double **m;
00110 
00111     if ( rows < 1  ||  cols < 1 )
00112         return NULL;
00113     m = (double **) my_gc_malloc (rows * sizeof(double *));
00114     m[0] = (double *) my_gc_malloc (rows * cols * sizeof(double));
00115     for ( i = 1; i<rows ; i++ )
00116        m[i] = m[i-1] + cols;
00117     return m;
00118 }
00119 
00120 
00121 void free_matr (m)
00122 double **m;
00123 {
00124     free (m[0]);
00125     free (m);
00126 }
00127 
00128 
00129 MZ_DLLEXPORT
00130 double *redim_vec (v, n)
00131 double **v;
00132 int n;
00133 {
00134     if ( n < 1 ) 
00135       *v = NULL;
00136     else       
00137       *v = (double *) my_gc_malloc( n * sizeof(double));
00138     return *v;
00139 }
00140 
00141 MZ_DLLEXPORT
00142 void redim_ivec (v, n)
00143 int **v;
00144 int n;
00145 {
00146     if ( n < 1 ) {
00147        *v = NULL;
00148        return;
00149     }
00150     *v = (int *) my_gc_malloc ( n * sizeof(int));
00151 }
00152 
00153 
00154 /* HBB: TODO: is there a better value for 'epsilon'? how to specify
00155  * 'inline'?  is 'fsign' really not available elsewhere? use
00156  * row-oriented version (p. 309) instead?
00157  */
00158 
00159 static int fsign(x)
00160   double x;
00161 {
00162     return( x>0 ? 1 : (x < 0) ? -1 : 0) ;
00163 }
00164 
00165 /*****************************************************************
00166 
00167      Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
00168      (QR-decomposition). Direct implementation of the algorithm
00169      presented in H.R.Schwarz: Numerische Mathematik, 'equation'
00170      number (7.33)
00171 
00172      If 'd == NULL', d is not accesed: the routine just computes the QR
00173      decomposition of C and exits.
00174 
00175      If 'want_r == 0', r is not rotated back (\hat{r} is returned
00176      instead).
00177 
00178 *****************************************************************/
00179 
00180 MZ_DLLEXPORT
00181 void Givens (C, d, x, r, N, n, want_r)
00182 double **C;
00183 double *d;
00184 double *x;
00185 double *r;
00186 int N;
00187 int n;
00188 int want_r;
00189 {
00190     int i, j, k;
00191     double w, gamma, sigma, rho, temp;
00192     double epsilon = 1e-5; /* FIXME (?)*/
00193 
00194 /* 
00195  * First, construct QR decomposition of C, by 'rotating away'
00196  * all elements of C below the diagonal. The rotations are
00197  * stored in place as Givens coefficients rho.
00198  * Vector d is also rotated in this same turn, if it exists 
00199  */
00200     for (j = 0; j<n; j++) 
00201        for (i = j+1; i<N; i++) 
00202            if (C[i][j]) {
00203               if (fabs(C[j][j])<epsilon*fabs(C[i][j])) { /* find the rotation parameters */
00204                   w = -C[i][j];
00205                   gamma = 0;
00206                   sigma = 1;
00207                   rho = 1;
00208               } else {
00209                   w = fsign(C[j][j])*sqrt(C[j][j]*C[j][j] + C[i][j]*C[i][j]);
00210                   if (w == 0) {
00211                     //                    Eex3 ( "w = 0 in Givens();  Cjj = %g,  Cij = %g", C[j][j], C[i][j]);
00212                   }
00213                   gamma = C[j][j]/w;
00214                   sigma = -C[i][j]/w;
00215                   rho = (fabs(sigma)<gamma) ? sigma : fsign(sigma)/gamma;
00216               }
00217               C[j][j] = w;
00218               C[i][j] = rho;           /* store rho in place, for later use */
00219               for (k = j+1; k<n; k++) {   /* rotation on index pair (i,j) */
00220                   temp =    gamma*C[j][k] - sigma*C[i][k];
00221                   C[i][k] = sigma*C[j][k] + gamma*C[i][k];
00222                   C[j][k] = temp;
00223                   
00224               }
00225               if (d) {               /* if no d vector given, don't use it */
00226                   temp = gamma*d[j] - sigma*d[i];  /* rotate d */
00227                   d[i] = sigma*d[j] + gamma*d[i];
00228                   d[j] = temp;
00229                }
00230            }
00231     if (!d)               /* stop here if no d was specified */
00232          return;
00233 
00234     for (i = n-1; i >= 0; i--) {   /* solve R*x+d = 0, by backsubstitution */
00235         double s = d[i];
00236         r[i] = 0;              /* ... and also set r[i] = 0 for i<n */
00237         for (k = i+1; k<n; k++) 
00238             s += C[i][k]*x[k];
00239        if (C[i][i] == 0) {
00240          //Eex ( "Singular matrix in Givens()");
00241        }
00242         x[i] = - s / C[i][i];
00243        }
00244     for (i = n; i < N; i++) 
00245        r[i] = d[i];         /* set the other r[i] to d[i] */
00246        
00247     if (!want_r)            /* if r isn't needed, stop here */
00248        return;
00249        
00250     /* rotate back the r vector */
00251     for (j = n-1; j >= 0; j--)
00252        for (i = N-1; i >= 0; i--) {
00253            if ((rho = C[i][j]) == 1) { /* reconstruct gamma, sigma from stored rho */
00254               gamma = 0;
00255               sigma = 1;
00256            } else if (fabs(rho)<1) {
00257               sigma = rho; 
00258               gamma = sqrt(1-sigma*sigma);
00259            } else {
00260               gamma = 1/fabs(rho);
00261               sigma = fsign(rho)*sqrt(1-gamma*gamma);
00262            }
00263            temp = gamma*r[j] + sigma*r[i];       /* rotate back indices (i,j) */
00264            r[i] = -sigma*r[j] + gamma*r[i];
00265            r[j] = temp;
00266     }
00267 }
00268 
00269 
00270 /* Given a triangular Matrix R, compute (R^T * R)^(-1), by forward
00271  * then back substitution
00272  * 
00273  * R, I are n x n Matrices, I is for the result. Both must already be
00274  * allocated.
00275  * 
00276  * Will only calculate the lower triangle of I, as it is symmetric 
00277  */
00278 
00279 MZ_DLLEXPORT
00280 void Invert_RtR ( R, I, n)
00281 double **R;
00282 double **I;
00283 int n;
00284 {
00285   int i, j, k;
00286 
00287   /* fill in the I matrix, and check R for regularity : */
00288 
00289   for (i = 0; i<n; i++) {
00290     for (j = 0; j<i; j++)  /* upper triangle isn't needed */
00291       I[i][j] = 0;
00292     I[i][i] = 1;
00293     if (! R[i][i])
00294       {
00295       //      Eex ("Singular matrix in Invert_RtR");
00296       }
00297       }
00298   
00299   /* Forward substitution: Solve R^T * B = I, store B in place of I */
00300   
00301   for (k = 0; k<n; k++) 
00302     for (i = k; i<n; i++) {  /* upper half needn't be computed */
00303       double s = I[i][k];
00304       for (j = k; j<i; j++)  /* for j<k, I[j][k] always stays zero! */
00305        s -= R[j][i] * I[j][k];
00306       I[i][k] = s / R[i][i];
00307     }
00308 
00309   /* Backward substitution: Solve R * A = B, store A in place of B */
00310 
00311   for (k = 0; k<n; k++)
00312     for (i = n-1; i >= k; i--) {  /* don't compute upper triangle of A */
00313       double s = I[i][k];
00314       for (j = i+1; j<n; j++)
00315        s -= R[i][j] * I[j][k];
00316       I[i][k] = s / R[i][i]; 
00317     }
00318 }