Annotation of OpenXM_contrib/gnuplot/matrix.c, Revision 1.1
1.1 ! maekawa 1: #ifndef lint
! 2: static char *RCSid = "$Id: matrix.c,v 1.15 1998/04/14 00:16:01 drd Exp $";
! 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: #include "fit.h"
! 30: #include "matrix.h"
! 31: #include "stdfn.h"
! 32: #include "alloc.h"
! 33:
! 34: /*****************************************************************/
! 35:
! 36: #define Swap(a,b) {double temp = (a); (a) = (b); (b) = temp;}
! 37: #define WINZIG 1e-30
! 38:
! 39:
! 40: /*****************************************************************
! 41: internal prototypes
! 42: *****************************************************************/
! 43:
! 44: static GP_INLINE int fsign __PROTO((double x));
! 45:
! 46: /*****************************************************************
! 47: first straightforward vector and matrix allocation functions
! 48: *****************************************************************/
! 49: double *vec (n)
! 50: int n;
! 51: {
! 52: /* allocates a double vector with n elements */
! 53: double *dp;
! 54: if( n < 1 )
! 55: return (double *) NULL;
! 56: dp = (double *) gp_alloc ( n * sizeof(double), "vec");
! 57: return dp;
! 58: }
! 59:
! 60:
! 61: double **matr (rows, cols)
! 62: int rows;
! 63: int cols;
! 64: {
! 65: /* allocates a double matrix */
! 66:
! 67: register int i;
! 68: register double **m;
! 69:
! 70: if ( rows < 1 || cols < 1 )
! 71: return NULL;
! 72: m = (double **) gp_alloc ( rows * sizeof(double *) , "matrix row pointers");
! 73: m[0] = (double *) gp_alloc ( rows * cols * sizeof(double), "matrix elements");
! 74: for ( i = 1; i<rows ; i++ )
! 75: m[i] = m[i-1] + cols;
! 76: return m;
! 77: }
! 78:
! 79:
! 80: void free_matr (m)
! 81: double **m;
! 82: {
! 83: free (m[0]);
! 84: free (m);
! 85: }
! 86:
! 87:
! 88: double *redim_vec (v, n)
! 89: double **v;
! 90: int n;
! 91: {
! 92: if ( n < 1 )
! 93: *v = NULL;
! 94: else
! 95: *v = (double *) gp_realloc (*v, n * sizeof(double), "vec");
! 96: return *v;
! 97: }
! 98:
! 99: void redim_ivec (v, n)
! 100: int **v;
! 101: int n;
! 102: {
! 103: if ( n < 1 ) {
! 104: *v = NULL;
! 105: return;
! 106: }
! 107: *v = (int *) gp_realloc (*v, n * sizeof(int), "ivec");
! 108: }
! 109:
! 110:
! 111: /* HBB: TODO: is there a better value for 'epsilon'? how to specify
! 112: * 'inline'? is 'fsign' really not available elsewhere? use
! 113: * row-oriented version (p. 309) instead?
! 114: */
! 115:
! 116: static GP_INLINE int fsign(x)
! 117: double x;
! 118: {
! 119: return( x>0 ? 1 : (x < 0) ? -1 : 0) ;
! 120: }
! 121:
! 122: /*****************************************************************
! 123:
! 124: Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
! 125: (QR-decomposition). Direct implementation of the algorithm
! 126: presented in H.R.Schwarz: Numerische Mathematik, 'equation'
! 127: number (7.33)
! 128:
! 129: If 'd == NULL', d is not accesed: the routine just computes the QR
! 130: decomposition of C and exits.
! 131:
! 132: If 'want_r == 0', r is not rotated back (\hat{r} is returned
! 133: instead).
! 134:
! 135: *****************************************************************/
! 136:
! 137: void Givens (C, d, x, r, N, n, want_r)
! 138: double **C;
! 139: double *d;
! 140: double *x;
! 141: double *r;
! 142: int N;
! 143: int n;
! 144: int want_r;
! 145: {
! 146: int i, j, k;
! 147: double w, gamma, sigma, rho, temp;
! 148: double epsilon = DBL_EPSILON; /* FIXME (?)*/
! 149:
! 150: /*
! 151: * First, construct QR decomposition of C, by 'rotating away'
! 152: * all elements of C below the diagonal. The rotations are
! 153: * stored in place as Givens coefficients rho.
! 154: * Vector d is also rotated in this same turn, if it exists
! 155: */
! 156: for (j = 0; j<n; j++)
! 157: for (i = j+1; i<N; i++)
! 158: if (C[i][j]) {
! 159: if (fabs(C[j][j])<epsilon*fabs(C[i][j])) { /* find the rotation parameters */
! 160: w = -C[i][j];
! 161: gamma = 0;
! 162: sigma = 1;
! 163: rho = 1;
! 164: } else {
! 165: w = fsign(C[j][j])*sqrt(C[j][j]*C[j][j] + C[i][j]*C[i][j]);
! 166: if (w == 0)
! 167: Eex3 ( "w = 0 in Givens(); Cjj = %g, Cij = %g", C[j][j], C[i][j]);
! 168: gamma = C[j][j]/w;
! 169: sigma = -C[i][j]/w;
! 170: rho = (fabs(sigma)<gamma) ? sigma : fsign(sigma)/gamma;
! 171: }
! 172: C[j][j] = w;
! 173: C[i][j] = rho; /* store rho in place, for later use */
! 174: for (k = j+1; k<n; k++) { /* rotation on index pair (i,j) */
! 175: temp = gamma*C[j][k] - sigma*C[i][k];
! 176: C[i][k] = sigma*C[j][k] + gamma*C[i][k];
! 177: C[j][k] = temp;
! 178:
! 179: }
! 180: if (d) { /* if no d vector given, don't use it */
! 181: temp = gamma*d[j] - sigma*d[i]; /* rotate d */
! 182: d[i] = sigma*d[j] + gamma*d[i];
! 183: d[j] = temp;
! 184: }
! 185: }
! 186: if (!d) /* stop here if no d was specified */
! 187: return;
! 188:
! 189: for (i = n-1; i >= 0; i--) { /* solve R*x+d = 0, by backsubstitution */
! 190: double s = d[i];
! 191: r[i] = 0; /* ... and also set r[i] = 0 for i<n */
! 192: for (k = i+1; k<n; k++)
! 193: s += C[i][k]*x[k];
! 194: if (C[i][i] == 0)
! 195: Eex ( "Singular matrix in Givens()");
! 196: x[i] = - s / C[i][i];
! 197: }
! 198: for (i = n; i < N; i++)
! 199: r[i] = d[i]; /* set the other r[i] to d[i] */
! 200:
! 201: if (!want_r) /* if r isn't needed, stop here */
! 202: return;
! 203:
! 204: /* rotate back the r vector */
! 205: for (j = n-1; j >= 0; j--)
! 206: for (i = N-1; i >= 0; i--) {
! 207: if ((rho = C[i][j]) == 1) { /* reconstruct gamma, sigma from stored rho */
! 208: gamma = 0;
! 209: sigma = 1;
! 210: } else if (fabs(rho)<1) {
! 211: sigma = rho;
! 212: gamma = sqrt(1-sigma*sigma);
! 213: } else {
! 214: gamma = 1/fabs(rho);
! 215: sigma = fsign(rho)*sqrt(1-gamma*gamma);
! 216: }
! 217: temp = gamma*r[j] + sigma*r[i]; /* rotate back indices (i,j) */
! 218: r[i] = -sigma*r[j] + gamma*r[i];
! 219: r[j] = temp;
! 220: }
! 221: }
! 222:
! 223:
! 224: /* Given a triangular Matrix R, compute (R^T * R)^(-1), by forward
! 225: * then back substitution
! 226: *
! 227: * R, I are n x n Matrices, I is for the result. Both must already be
! 228: * allocated.
! 229: *
! 230: * Will only calculate the lower triangle of I, as it is symmetric
! 231: */
! 232:
! 233: void Invert_RtR ( R, I, n)
! 234: double **R;
! 235: double **I;
! 236: int n;
! 237: {
! 238: int i, j, k;
! 239:
! 240: /* fill in the I matrix, and check R for regularity : */
! 241:
! 242: for (i = 0; i<n; i++) {
! 243: for (j = 0; j<i; j++) /* upper triangle isn't needed */
! 244: I[i][j] = 0;
! 245: I[i][i] = 1;
! 246: if (! R[i][i])
! 247: Eex ("Singular matrix in Invert_RtR")
! 248: }
! 249:
! 250: /* Forward substitution: Solve R^T * B = I, store B in place of I */
! 251:
! 252: for (k = 0; k<n; k++)
! 253: for (i = k; i<n; i++) { /* upper half needn't be computed */
! 254: double s = I[i][k];
! 255: for (j = k; j<i; j++) /* for j<k, I[j][k] always stays zero! */
! 256: s -= R[j][i] * I[j][k];
! 257: I[i][k] = s / R[i][i];
! 258: }
! 259:
! 260: /* Backward substitution: Solve R * A = B, store A in place of B */
! 261:
! 262: for (k = 0; k<n; k++)
! 263: for (i = n-1; i >= k; i--) { /* don't compute upper triangle of A */
! 264: double s = I[i][k];
! 265: for (j = i+1; j<n; j++)
! 266: s -= R[i][j] * I[j][k];
! 267: I[i][k] = s / R[i][i];
! 268: }
! 269: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>