Annotation of OpenXM_contrib/gnuplot/matrix.c, Revision 1.1.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>
![[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