git.maemo.org Git - gnuplot/blob - src/matrix.c

   1 #ifndef lint
   2 static char *RCSid() { return RCSid("$Id: matrix.c,v 1.10 2004/07/01 17:10:06 broeker Exp $"); }
   3 #endif
   4
   5 /*  NOTICE: Change of Copyright Status
   6  *
   7  *  The author of this module, Carsten Grammes, has expressed in
   8  *  personal email that he has no more interest in this code, and
   9  *  doesn't claim any copyright. He has agreed to put this module
  10  *  into the public domain.
  11  *
  12  *  Lars Hecking  15-02-1999
  13  */
  14
  15 /*
  16  *      Matrix algebra, part of
  17  *
  18  *      Nonlinear least squares fit according to the
  19  *      Marquardt-Levenberg-algorithm
  20  *
  21  *      added as Patch to Gnuplot (v3.2 and higher)
  22  *      by Carsten Grammes
  23  *      Experimental Physics, University of Saarbruecken, Germany
  24  *
  25  *      Previous copyright of this module:   Carsten Grammes, 1993
  26  *
  27  */
  28
  29 #include "matrix.h"
  30
  31 #include "alloc.h"
  32 #include "fit.h"
  33 #include "util.h"
  34
  35 /*****************************************************************/
  36
  37 #define Swap(a,b)   {double temp = (a); (a) = (b); (b) = temp;}
  38 /* HBB 20010424: unused: */
  39 /* #define WINZIG             1e-30 */
  40
  41
  42 /*****************************************************************
  43     internal prototypes
  44 *****************************************************************/
  45
  46 static GP_INLINE int fsign __PROTO((double x));
  47
  48 /*****************************************************************
  49     first straightforward vector and matrix allocation functions
  50 *****************************************************************/
  51
  52 /* allocates a double vector with n elements */
  53 double *
  54 vec(int n)
  55 {
  56     double *dp;
  57
  58     if (n < 1)
  59         return NULL;
  60     dp = gp_alloc(n * sizeof(double), "vec");
  61     return dp;
  62 }
  63
  64
  65 /* allocates a double matrix */
  66 double **
  67 matr(int rows, int cols)
  68 {
  69     int i;
  70     double **m;
  71
  72     if (rows < 1 || cols < 1)
  73         return NULL;
  74     m = gp_alloc(rows * sizeof(m[0]), "matrix row pointers");
  75     m[0] = gp_alloc(rows * cols * sizeof(m[0][0]), "matrix elements");
  76     for (i = 1; i < rows; i++)
  77         m[i] = m[i - 1] + cols;
  78     return m;
  79 }
  80
  81
  82 void
  83 free_matr(double **m)
  84 {
  85     free(m[0]);
  86     free(m);
  87 }
  88
  89
  90 double *
  91 redim_vec(double **v, int n)
  92 {
  93     if (n < 1)
  94         *v = NULL;
  95     else
  96         *v = gp_realloc(*v, n * sizeof((*v)[0]), "vec");
  97     return *v;
  98 }
  99
 100 /* HBB: TODO: is there a better value for 'epsilon'? how to specify
 101  * 'inline'?  is 'fsign' really not available elsewhere? use
 102  * row-oriented version (p. 309) instead?
 103  */
 104
 105 static GP_INLINE int
 106 fsign(double x)
 107 {
 108     return (x > 0 ? 1 : (x < 0) ? -1 : 0);
 109 }
 110
 111 /*****************************************************************
 112
 113      Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
 114      (QR-decomposition). Direct implementation of the algorithm
 115      presented in H.R.Schwarz: Numerische Mathematik, 'equation'
 116      number (7.33)
 117
 118      If 'd == NULL', d is not accesed: the routine just computes the QR
 119      decomposition of C and exits.
 120
 121      If 'want_r == 0', r is not rotated back (\hat{r} is returned
 122      instead).
 123
 124 *****************************************************************/
 125
 126 void
 127 Givens(
 128     double **C,
 129     double *d,
 130     double *x,
 131     double *r,
 132     int N,
 133     int n,
 134     int want_r)
 135 {
 136     int i, j, k;
 137     double w, gamma, sigma, rho, temp;
 138     double epsilon = DBL_EPSILON;       /* FIXME (?) */
 139
 140 /*
 141  * First, construct QR decomposition of C, by 'rotating away'
 142  * all elements of C below the diagonal. The rotations are
 143  * stored in place as Givens coefficients rho.
 144  * Vector d is also rotated in this same turn, if it exists
 145  */
 146     for (j = 0; j < n; j++) {
 147         for (i = j + 1; i < N; i++) {
 148             if (C[i][j]) {
 149                 if (fabs(C[j][j]) < epsilon * fabs(C[i][j])) {
 150                     /* find the rotation parameters */
 151                     w = -C[i][j];
 152                     gamma = 0;
 153                     sigma = 1;
 154                     rho = 1;
 155                 } else {
 156                     w = fsign(C[j][j]) * sqrt(C[j][j] * C[j][j] + C[i][j] * C[i][j]);
 157                     if (w == 0)
 158                         Eex3("w = 0 in Givens();  Cjj = %g,  Cij = %g", C[j][j], C[i][j]);
 159                     gamma = C[j][j] / w;
 160                     sigma = -C[i][j] / w;
 161                     rho = (fabs(sigma) < gamma) ? sigma : fsign(sigma) / gamma;
 162                 }
 163                 C[j][j] = w;
 164                 C[i][j] = rho;  /* store rho in place, for later use */
 165                 for (k = j + 1; k < n; k++) {
 166                     /* rotation on index pair (i,j) */
 167                     temp = gamma * C[j][k] - sigma * C[i][k];
 168                     C[i][k] = sigma * C[j][k] + gamma * C[i][k];
 169                     C[j][k] = temp;
 170
 171                 }
 172                 if (d) {        /* if no d vector given, don't use it */
 173                     temp = gamma * d[j] - sigma * d[i];         /* rotate d */
 174                     d[i] = sigma * d[j] + gamma * d[i];
 175                     d[j] = temp;
 176                 }
 177             }
 178         }
 179     }
 180
 181     if (!d)                     /* stop here if no d was specified */
 182         return;
 183
 184     /* solve R*x+d = 0, by backsubstitution */
 185     for (i = n - 1; i >= 0; i--) {
 186         double s = d[i];
 187
 188         r[i] = 0;               /* ... and also set r[i] = 0 for i<n */
 189         for (k = i + 1; k < n; k++)
 190             s += C[i][k] * x[k];
 191         if (C[i][i] == 0)
 192             Eex("Singular matrix in Givens()");
 193         x[i] = -s / C[i][i];
 194     }
 195
 196     for (i = n; i < N; i++)
 197         r[i] = d[i];            /* set the other r[i] to d[i] */
 198
 199     if (!want_r)                /* if r isn't needed, stop here */
 200         return;
 201
 202     /* rotate back the r vector */
 203     for (j = n - 1; j >= 0; j--) {
 204         for (i = N - 1; i >= 0; i--) {
 205             if ((rho = C[i][j]) == 1) {         /* reconstruct gamma, sigma from stored rho */
 206                 gamma = 0;
 207                 sigma = 1;
 208             } else if (fabs(rho) < 1) {
 209                 sigma = rho;
 210                 gamma = sqrt(1 - sigma * sigma);
 211             } else {
 212                 gamma = 1 / fabs(rho);
 213                 sigma = fsign(rho) * sqrt(1 - gamma * gamma);
 214             }
 215             temp = gamma * r[j] + sigma * r[i];         /* rotate back indices (i,j) */
 216             r[i] = -sigma * r[j] + gamma * r[i];
 217             r[j] = temp;
 218         }
 219     }
 220 }
 221
 222
 223 /* Given a triangular Matrix R, compute (R^T * R)^(-1), by forward
 224  * then back substitution
 225  *
 226  * R, I are n x n Matrices, I is for the result. Both must already be
 227  * allocated.
 228  *
 229  * Will only calculate the lower triangle of I, as it is symmetric
 230  */
 231
 232 void
 233 Invert_RtR(double **R, double **I, int n)
 234 {
 235     int i, j, k;
 236
 237     /* fill in the I matrix, and check R for regularity : */
 238
 239     for (i = 0; i < n; i++) {
 240         for (j = 0; j < i; j++) /* upper triangle isn't needed */
 241             I[i][j] = 0;
 242         I[i][i] = 1;
 243         if (!R[i][i])
 244             Eex("Singular matrix in Invert_RtR");
 245     }
 246
 247     /* Forward substitution: Solve R^T * B = I, store B in place of I */
 248
 249     for (k = 0; k < n; k++) {
 250         for (i = k; i < n; i++) {       /* upper half needn't be computed */
 251             double s = I[i][k];
 252             for (j = k; j < i; j++)     /* for j<k, I[j][k] always stays zero! */
 253                 s -= R[j][i] * I[j][k];
 254             I[i][k] = s / R[i][i];
 255         }
 256     }
 257     /* Backward substitution: Solve R * A = B, store A in place of B */
 258
 259     for (k = 0; k < n; k++) {
 260         for (i = n - 1; i >= k; i--) {  /* don't compute upper triangle of A */
 261             double s = I[i][k];
 262             for (j = i + 1; j < n; j++)
 263                 s -= R[i][j] * I[j][k];
 264             I[i][k] = s / R[i][i];
 265         }
 266     }
 267 }
 268
 269 /* HBB 20010424: Functions that used to be here in matrix.c, but were
 270  * replaced by others and deleted, later.  But the
 271  * THIN_PLATE_SPLINES_GRID needed them, later, so they appeared in
 272  * plot3d.c, where they don't belong --> moved them back here. */
 273
 274 void
 275 lu_decomp(double **a, int n, int *indx, double *d)
 276 {
 277     int i, imax = -1, j, k;     /* HBB: added initial value, to shut up gcc -Wall */
 278     double large, dummy, temp, **ar, **lim, *limc, *ac, *dp, *vscal;
 279
 280     dp = vscal = vec(n);
 281     *d = 1.0;
 282     for (ar = a, lim = &(a[n]); ar < lim; ar++) {
 283         large = 0.0;
 284         for (ac = *ar, limc = &(ac[n]); ac < limc;)
 285             if ((temp = fabs(*ac++)) > large)
 286                 large = temp;
 287         if (large == 0.0)
 288             int_error(NO_CARET, "Singular matrix in LU-DECOMP");
 289         *dp++ = 1 / large;
 290     }
 291     ar = a;
 292     for (j = 0; j < n; j++, ar++) {
 293         for (i = 0; i < j; i++) {
 294             ac = &(a[i][j]);
 295             for (k = 0; k < i; k++)
 296                 *ac -= a[i][k] * a[k][j];
 297         }
 298         large = 0.0;
 299         dp = &(vscal[j]);
 300         for (i = j; i < n; i++) {
 301             ac = &(a[i][j]);
 302             for (k = 0; k < j; k++)
 303                 *ac -= a[i][k] * a[k][j];
 304             if ((dummy = *dp++ * fabs(*ac)) >= large) {
 305                 large = dummy;
 306                 imax = i;
 307             }
 308         }
 309         if (j != imax) {
 310             ac = a[imax];
 311             dp = *ar;
 312             for (k = 0; k < n; k++, ac++, dp++)
 313                 Swap(*ac, *dp);
 314             *d = -(*d);
 315             vscal[imax] = vscal[j];
 316         }
 317         indx[j] = imax;
 318         if (*(dp = &(*ar)[j]) == 0)
 319             *dp = 1e-30;
 320
 321         if (j != n - 1) {
 322             dummy = 1 / (*ar)[j];
 323             for (i = j + 1; i < n; i++)
 324                 a[i][j] *= dummy;
 325         }
 326     }
 327     free(vscal);
 328 }
 329
 330 void
 331 lu_backsubst(double **a, int n, int *indx, double *b)
 332 {
 333     int i, memi = -1, ip, j;
 334
 335     double sum, *bp, *bip, **ar, *ac;
 336
 337     ar = a;
 338
 339     for (i = 0; i < n; i++, ar++) {
 340         ip = indx[i];
 341         sum = b[ip];
 342         b[ip] = b[i];
 343         if (memi >= 0) {
 344             ac = &((*ar)[memi]);
 345             bp = &(b[memi]);
 346             for (j = memi; j <= i - 1; j++)
 347                 sum -= *ac++ * *bp++;
 348         } else if (sum)
 349             memi = i;
 350         b[i] = sum;
 351     }
 352     ar--;
 353     for (i = n - 1; i >= 0; i--) {
 354         ac = &(*ar)[i + 1];
 355         bp = &(b[i + 1]);
 356         bip = &(b[i]);
 357         for (j = i + 1; j < n; j++)
 358             *bip -= *ac++ * *bp++;
 359         *bip /= (*ar--)[i];
 360     }
 361 }