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