1 | |
2 | |
3 | |
4 | |
5 | |
6 | |
7 | |
8 | |
9 | |
10 | |
11 | |
12 | |
13 | |
14 | |
15 | |
16 | |
17 | |
18 | |
19 | |
20 | |
21 | |
22 | |
23 | |
24 | |
25 | |
26 | |
27 | |
28 | |
29 | |
30 | |
31 | |
32 | |
33 | |
34 | |
35 | |
36 | |
37 | #ifdef HAVE_CONFIG_H1 |
38 | #include <config.h> |
39 | #endif |
40 | |
41 | #include <math.h> |
42 | #include <stdio.h> |
43 | #include <stdlib.h> |
44 | |
45 | #include "types/simple.h" |
46 | |
47 | static void nrerror(const char error_text[], gmx_bool bExit) |
48 | { |
49 | fprintf(stderrstderr, "Numerical Recipes run-time error...\n"); |
50 | fprintf(stderrstderr, "%s\n", error_text); |
51 | if (bExit) |
52 | { |
53 | fprintf(stderrstderr, "...now exiting to system...\n"); |
54 | exit(1); |
55 | } |
56 | } |
57 | |
58 | |
59 | |
60 | |
61 | |
62 | static real *rvector(int nl, int nh) |
63 | { |
64 | real *v; |
65 | |
66 | v = (real *)malloc((unsigned) (nh-nl+1)*sizeof(real)); |
67 | if (!v) |
68 | { |
69 | nrerror("allocation failure in rvector()", TRUE1); |
70 | } |
71 | return v-nl; |
72 | } |
73 | |
74 | static int *ivector(int nl, int nh) |
75 | { |
76 | int *v; |
77 | |
78 | v = (int *)malloc((unsigned) (nh-nl+1)*sizeof(int)); |
79 | if (!v) |
80 | { |
81 | nrerror("allocation failure in ivector()", TRUE1); |
82 | } |
83 | return v-nl; |
84 | } |
85 | |
86 | |
87 | static real **matrix1(int nrl, int nrh, int ncl, int nch) |
88 | { |
89 | int i; |
90 | real **m; |
91 | |
92 | m = (real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*)); |
93 | if (!m) |
94 | { |
95 | nrerror("allocation failure 1 in matrix1()", TRUE1); |
96 | } |
97 | m -= nrl; |
98 | |
99 | for (i = nrl; i <= nrh; i++) |
100 | { |
101 | m[i] = (real *) malloc((unsigned) (nch-ncl+1)*sizeof(real)); |
102 | if (!m[i]) |
103 | { |
104 | nrerror("allocation failure 2 in matrix1()", TRUE1); |
105 | } |
106 | m[i] -= ncl; |
107 | } |
108 | return m; |
109 | } |
110 | |
111 | static double **dmatrix(int nrl, int nrh, int ncl, int nch) |
112 | { |
113 | int i; |
114 | double **m; |
115 | |
116 | m = (double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*)); |
117 | if (!m) |
118 | { |
119 | nrerror("allocation failure 1 in dmatrix()", TRUE1); |
120 | } |
121 | m -= nrl; |
122 | |
123 | for (i = nrl; i <= nrh; i++) |
124 | { |
125 | m[i] = (double *) malloc((unsigned) (nch-ncl+1)*sizeof(double)); |
126 | if (!m[i]) |
127 | { |
128 | nrerror("allocation failure 2 in dmatrix()", TRUE1); |
129 | } |
130 | m[i] -= ncl; |
131 | } |
132 | return m; |
133 | } |
134 | |
135 | static int **imatrix1(int nrl, int nrh, int ncl, int nch) |
136 | { |
137 | int i, **m; |
138 | |
139 | m = (int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*)); |
| |
140 | if (!m) |
| 2 | | Assuming 'm' is non-null | |
|
| |
141 | { |
142 | nrerror("allocation failure 1 in imatrix1()", TRUE1); |
143 | } |
144 | m -= nrl; |
145 | |
146 | for (i = nrl; i <= nrh; i++) |
| 4 | | Potential leak of memory pointed to by 'm' |
|
147 | { |
148 | m[i] = (int *)malloc((unsigned) (nch-ncl+1)*sizeof(int)); |
149 | if (!m[i]) |
150 | { |
151 | nrerror("allocation failure 2 in imatrix1()", TRUE1); |
152 | } |
153 | m[i] -= ncl; |
154 | } |
155 | return m; |
156 | } |
157 | |
158 | |
159 | |
160 | static real **submatrix(real **a, int oldrl, int oldrh, int oldcl, |
161 | int newrl, int newcl) |
162 | { |
163 | int i, j; |
164 | real **m; |
165 | |
166 | m = (real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*)); |
167 | if (!m) |
168 | { |
169 | nrerror("allocation failure in submatrix()", TRUE1); |
170 | } |
171 | m -= newrl; |
172 | |
173 | for (i = oldrl, j = newrl; i <= oldrh; i++, j++) |
174 | { |
175 | m[j] = a[i]+oldcl-newcl; |
176 | } |
177 | |
178 | return m; |
179 | } |
180 | |
181 | |
182 | |
183 | static void free_vector(real *v, int nl) |
184 | { |
185 | free((char*) (v+nl)); |
186 | } |
187 | |
188 | static void free_ivector(int *v, int nl) |
189 | { |
190 | free((char*) (v+nl)); |
191 | } |
192 | |
193 | static void free_dvector(int *v, int nl) |
194 | { |
195 | free((char*) (v+nl)); |
196 | } |
197 | |
198 | |
199 | |
200 | static void free_matrix(real **m, int nrl, int nrh, int ncl) |
201 | { |
202 | int i; |
203 | |
204 | for (i = nrh; i >= nrl; i--) |
205 | { |
206 | free((char*) (m[i]+ncl)); |
207 | } |
208 | free((char*) (m+nrl)); |
209 | } |
210 | |
211 | static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch) |
212 | { |
213 | int i, j, nrow, ncol; |
214 | real **m; |
215 | |
216 | nrow = nrh-nrl+1; |
217 | ncol = nch-ncl+1; |
218 | m = (real **) malloc((unsigned) (nrow)*sizeof(real*)); |
219 | if (!m) |
220 | { |
221 | nrerror("allocation failure in convert_matrix()", TRUE1); |
222 | } |
223 | m -= nrl; |
224 | for (i = 0, j = nrl; i <= nrow-1; i++, j++) |
225 | { |
226 | m[j] = a+ncol*i-ncl; |
227 | } |
228 | return m; |
229 | } |
230 | |
231 | |
232 | |
233 | static void free_convert_matrix(real **b, int nrl) |
234 | { |
235 | free((char*) (b+nrl)); |
236 | } |
237 | |
238 | #define SWAP(a, b) {real temp = (a); (a) = (b); (b) = temp; } |
239 | |
240 | static void dump_mat(int n, real **a) |
241 | { |
242 | int i, j; |
243 | |
244 | for (i = 1; (i <= n); i++) |
245 | { |
246 | for (j = 1; (j <= n); j++) |
247 | { |
248 | fprintf(stderrstderr, " %10.3f", a[i][j]); |
249 | } |
250 | fprintf(stderrstderr, "\n"); |
251 | } |
252 | } |
253 | |
254 | gmx_bool gaussj(real **a, int n, real **b, int m) |
255 | { |
256 | int *indxc, *indxr, *ipiv; |
257 | int i, icol = 0, irow = 0, j, k, l, ll; |
258 | real big, dum, pivinv; |
259 | |
260 | indxc = ivector(1, n); |
261 | indxr = ivector(1, n); |
262 | ipiv = ivector(1, n); |
263 | for (j = 1; j <= n; j++) |
264 | { |
265 | ipiv[j] = 0; |
266 | } |
267 | for (i = 1; i <= n; i++) |
268 | { |
269 | big = 0.0; |
270 | for (j = 1; j <= n; j++) |
271 | { |
272 | if (ipiv[j] != 1) |
273 | { |
274 | for (k = 1; k <= n; k++) |
275 | { |
276 | if (ipiv[k] == 0) |
277 | { |
278 | if (fabs(a[j][k]) >= big) |
279 | { |
280 | big = fabs(a[j][k]); |
281 | irow = j; |
282 | icol = k; |
283 | } |
284 | } |
285 | else if (ipiv[k] > 1) |
286 | { |
287 | nrerror("GAUSSJ: Singular Matrix-1", FALSE0); |
288 | return FALSE0; |
289 | } |
290 | } |
291 | } |
292 | } |
293 | ++(ipiv[icol]); |
294 | if (irow != icol) |
295 | { |
296 | for (l = 1; l <= n; l++) |
297 | { |
298 | SWAP(a[irow][l], a[icol][l]); |
299 | } |
300 | for (l = 1; l <= m; l++) |
301 | { |
302 | SWAP(b[irow][l], b[icol][l]); |
303 | } |
304 | } |
305 | indxr[i] = irow; |
306 | indxc[i] = icol; |
307 | if (a[icol][icol] == 0.0) |
308 | { |
309 | fprintf(stderrstderr, "irow = %d, icol = %d\n", irow, icol); |
310 | dump_mat(n, a); |
311 | nrerror("GAUSSJ: Singular Matrix-2", FALSE0); |
312 | return FALSE0; |
313 | } |
314 | pivinv = 1.0/a[icol][icol]; |
315 | a[icol][icol] = 1.0; |
316 | for (l = 1; l <= n; l++) |
317 | { |
318 | a[icol][l] *= pivinv; |
319 | } |
320 | for (l = 1; l <= m; l++) |
321 | { |
322 | b[icol][l] *= pivinv; |
323 | } |
324 | for (ll = 1; ll <= n; ll++) |
325 | { |
326 | if (ll != icol) |
327 | { |
328 | dum = a[ll][icol]; |
329 | a[ll][icol] = 0.0; |
330 | for (l = 1; l <= n; l++) |
331 | { |
332 | a[ll][l] -= a[icol][l]*dum; |
333 | } |
334 | for (l = 1; l <= m; l++) |
335 | { |
336 | b[ll][l] -= b[icol][l]*dum; |
337 | } |
338 | } |
339 | } |
340 | } |
341 | for (l = n; l >= 1; l--) |
342 | { |
343 | if (indxr[l] != indxc[l]) |
344 | { |
345 | for (k = 1; k <= n; k++) |
346 | { |
347 | SWAP(a[k][indxr[l]], a[k][indxc[l]]); |
348 | } |
349 | } |
350 | } |
351 | free_ivector(ipiv, 1); |
352 | free_ivector(indxr, 1); |
353 | free_ivector(indxc, 1); |
354 | |
355 | return TRUE1; |
356 | } |
357 | |
358 | #undef SWAP |
359 | |
360 | |
361 | static void covsrt(real **covar, int ma, int lista[], int mfit) |
362 | { |
363 | int i, j; |
364 | real swap; |
365 | |
366 | for (j = 1; j < ma; j++) |
367 | { |
368 | for (i = j+1; i <= ma; i++) |
369 | { |
370 | covar[i][j] = 0.0; |
371 | } |
372 | } |
373 | for (i = 1; i < mfit; i++) |
374 | { |
375 | for (j = i+1; j <= mfit; j++) |
376 | { |
377 | if (lista[j] > lista[i]) |
378 | { |
379 | covar[lista[j]][lista[i]] = covar[i][j]; |
380 | } |
381 | else |
382 | { |
383 | covar[lista[i]][lista[j]] = covar[i][j]; |
384 | } |
385 | } |
386 | } |
387 | swap = covar[1][1]; |
388 | for (j = 1; j <= ma; j++) |
389 | { |
390 | covar[1][j] = covar[j][j]; |
391 | covar[j][j] = 0.0; |
392 | } |
393 | covar[lista[1]][lista[1]] = swap; |
394 | for (j = 2; j <= mfit; j++) |
395 | { |
396 | covar[lista[j]][lista[j]] = covar[1][j]; |
397 | } |
398 | for (j = 2; j <= ma; j++) |
399 | { |
400 | for (i = 1; i <= j-1; i++) |
401 | { |
402 | covar[i][j] = covar[j][i]; |
403 | } |
404 | } |
405 | } |
406 | |
407 | #define SWAP(a, b) {swap = (a); (a) = (b); (b) = swap; } |
408 | |
409 | static void covsrt_new(real **covar, int ma, int ia[], int mfit) |
410 | |
411 | |
412 | |
413 | |
414 | { |
415 | int i, j, k; |
416 | real swap; |
417 | for (i = mfit+1; i <= ma; i++) |
418 | { |
419 | for (j = 1; j <= i; j++) |
420 | { |
421 | covar[i][j] = covar[j][i] = 0.0; |
422 | } |
423 | } |
424 | k = mfit; |
425 | for (j = ma; j >= 1; j--) |
426 | { |
427 | if (ia[j]) |
428 | { |
429 | for (i = 1; i <= ma; i++) |
430 | { |
431 | SWAP(covar[i][k], covar[i][j]); |
432 | } |
433 | for (i = 1; i <= ma; i++) |
434 | { |
435 | SWAP(covar[k][i], covar[j][i]); |
436 | } |
437 | k--; |
438 | } |
439 | } |
440 | } |
441 | #undef SWAP |
442 | |
443 | static void mrqcof(real x[], real y[], real sig[], int ndata, real a[], |
444 | int ma, int lista[], int mfit, |
445 | real **alpha, real beta[], real *chisq, |
446 | void (*funcs)(real, real *, real *, real *)) |
447 | { |
448 | int k, j, i; |
449 | real ymod, wt, sig2i, dy, *dyda; |
450 | |
451 | dyda = rvector(1, ma); |
452 | for (j = 1; j <= mfit; j++) |
453 | { |
454 | for (k = 1; k <= j; k++) |
455 | { |
456 | alpha[j][k] = 0.0; |
457 | } |
458 | beta[j] = 0.0; |
459 | } |
460 | *chisq = 0.0; |
461 | for (i = 1; i <= ndata; i++) |
462 | { |
463 | (*funcs)(x[i], a, &ymod, dyda); |
464 | sig2i = 1.0/(sig[i]*sig[i]); |
465 | dy = y[i]-ymod; |
466 | for (j = 1; j <= mfit; j++) |
467 | { |
468 | wt = dyda[lista[j]]*sig2i; |
469 | for (k = 1; k <= j; k++) |
470 | { |
471 | alpha[j][k] += wt*dyda[lista[k]]; |
472 | } |
473 | beta[j] += dy*wt; |
474 | } |
475 | (*chisq) += dy*dy*sig2i; |
476 | } |
477 | for (j = 2; j <= mfit; j++) |
478 | { |
479 | for (k = 1; k <= j-1; k++) |
480 | { |
481 | alpha[k][j] = alpha[j][k]; |
482 | } |
483 | } |
484 | free_vector(dyda, 1); |
485 | } |
486 | |
487 | |
488 | gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[], |
489 | int ma, int lista[], int mfit, |
490 | real **covar, real **alpha, real *chisq, |
491 | void (*funcs)(real, real *, real *, real *), |
492 | real *alamda) |
493 | { |
494 | int k, kk, j, ihit; |
495 | static real *da, *atry, **oneda, *beta, ochisq; |
496 | |
497 | if (*alamda < 0.0) |
498 | { |
499 | oneda = matrix1(1, mfit, 1, 1); |
500 | atry = rvector(1, ma); |
501 | da = rvector(1, ma); |
502 | beta = rvector(1, ma); |
503 | kk = mfit+1; |
504 | for (j = 1; j <= ma; j++) |
505 | { |
506 | ihit = 0; |
507 | for (k = 1; k <= mfit; k++) |
508 | { |
509 | if (lista[k] == j) |
510 | { |
511 | ihit++; |
512 | } |
513 | } |
514 | if (ihit == 0) |
515 | { |
516 | lista[kk++] = j; |
517 | } |
518 | else if (ihit > 1) |
519 | { |
520 | nrerror("Bad LISTA permutation in MRQMIN-1", FALSE0); |
521 | return FALSE0; |
522 | } |
523 | } |
524 | if (kk != ma+1) |
525 | { |
526 | nrerror("Bad LISTA permutation in MRQMIN-2", FALSE0); |
527 | return FALSE0; |
528 | } |
529 | *alamda = 0.001; |
530 | mrqcof(x, y, sig, ndata, a, ma, lista, mfit, alpha, beta, chisq, funcs); |
531 | ochisq = (*chisq); |
532 | } |
533 | for (j = 1; j <= mfit; j++) |
534 | { |
535 | for (k = 1; k <= mfit; k++) |
536 | { |
537 | covar[j][k] = alpha[j][k]; |
538 | } |
539 | covar[j][j] = alpha[j][j]*(1.0+(*alamda)); |
540 | oneda[j][1] = beta[j]; |
541 | } |
542 | if (!gaussj(covar, mfit, oneda, 1)) |
543 | { |
544 | return FALSE0; |
545 | } |
546 | for (j = 1; j <= mfit; j++) |
547 | { |
548 | da[j] = oneda[j][1]; |
549 | } |
550 | if (*alamda == 0.0) |
551 | { |
552 | covsrt(covar, ma, lista, mfit); |
553 | free_vector(beta, 1); |
554 | free_vector(da, 1); |
555 | free_vector(atry, 1); |
556 | free_matrix(oneda, 1, mfit, 1); |
557 | return TRUE1; |
558 | } |
559 | for (j = 1; j <= ma; j++) |
560 | { |
561 | atry[j] = a[j]; |
562 | } |
563 | for (j = 1; j <= mfit; j++) |
564 | { |
565 | atry[lista[j]] = a[lista[j]]+da[j]; |
566 | } |
567 | mrqcof(x, y, sig, ndata, atry, ma, lista, mfit, covar, da, chisq, funcs); |
568 | if (*chisq < ochisq) |
569 | { |
570 | *alamda *= 0.1; |
571 | ochisq = (*chisq); |
572 | for (j = 1; j <= mfit; j++) |
573 | { |
574 | for (k = 1; k <= mfit; k++) |
575 | { |
576 | alpha[j][k] = covar[j][k]; |
577 | } |
578 | beta[j] = da[j]; |
579 | a[lista[j]] = atry[lista[j]]; |
580 | } |
581 | } |
582 | else |
583 | { |
584 | *alamda *= 10.0; |
585 | *chisq = ochisq; |
586 | } |
587 | return TRUE1; |
588 | } |
589 | |
590 | |
591 | gmx_bool mrqmin_new(real x[], real y[], real sig[], int ndata, real a[], |
592 | int ia[], int ma, real **covar, real **alpha, real *chisq, |
593 | void (*funcs)(real, real [], real *, real []), |
594 | real *alamda) |
595 | |
596 | |
597 | |
598 | |
599 | |
600 | |
601 | |
602 | |
603 | |
604 | |
605 | |
606 | |
607 | |
608 | |
609 | |
610 | |
611 | |
612 | |
613 | |
614 | |
615 | |
616 | |
617 | { |
618 | void covsrt(real **covar, int ma, int ia[], int mfit); |
619 | gmx_bool gaussj(real **a, int n, real **b, int m); |
620 | void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[], |
621 | int ia[], int ma, real **alpha, real beta[], real *chisq, |
622 | void (*funcs)(real, real [], real *, real [])); |
623 | int j, k, l; |
624 | static int mfit; |
625 | static real ochisq, *atry, *beta, *da, **oneda; |
626 | |
627 | if (*alamda < 0.0) |
628 | { |
629 | atry = rvector(1, ma); |
630 | beta = rvector(1, ma); |
631 | da = rvector(1, ma); |
632 | for (mfit = 0, j = 1; j <= ma; j++) |
633 | { |
634 | if (ia[j]) |
635 | { |
636 | mfit++; |
637 | } |
638 | } |
639 | oneda = matrix1(1, mfit, 1, 1); |
640 | *alamda = 0.001; |
641 | mrqcof_new(x, y, sig, ndata, a, ia, ma, alpha, beta, chisq, funcs); |
642 | ochisq = (*chisq); |
643 | for (j = 1; j <= ma; j++) |
644 | { |
645 | atry[j] = a[j]; |
646 | } |
647 | } |
648 | for (j = 1; j <= mfit; j++) |
649 | { |
650 | for (k = 1; k <= mfit; k++) |
651 | { |
652 | covar[j][k] = alpha[j][k]; |
653 | } |
654 | covar[j][j] = alpha[j][j]*(1.0+(*alamda)); |
655 | oneda[j][1] = beta[j]; |
656 | } |
657 | if (!gaussj(covar, mfit, oneda, 1)) |
658 | { |
659 | return FALSE0; |
660 | } |
661 | for (j = 1; j <= mfit; j++) |
662 | { |
663 | da[j] = oneda[j][1]; |
664 | } |
665 | if (*alamda == 0.0) |
666 | { |
667 | covsrt_new(covar, ma, ia, mfit); |
668 | free_matrix(oneda, 1, mfit, 1); |
669 | free_vector(da, 1); |
670 | free_vector(beta, 1); |
671 | free_vector(atry, 1); |
672 | return TRUE1; |
673 | } |
674 | for (j = 0, l = 1; l <= ma; l++) |
675 | { |
676 | if (ia[l]) |
677 | { |
678 | atry[l] = a[l]+da[++j]; |
679 | } |
680 | } |
681 | mrqcof_new(x, y, sig, ndata, atry, ia, ma, covar, da, chisq, funcs); |
682 | if (*chisq < ochisq) |
683 | { |
684 | |
685 | *alamda *= 0.1; |
686 | ochisq = (*chisq); |
687 | for (j = 1; j <= mfit; j++) |
688 | { |
689 | for (k = 1; k <= mfit; k++) |
690 | { |
691 | alpha[j][k] = covar[j][k]; |
692 | } |
693 | beta[j] = da[j]; |
694 | } |
695 | for (l = 1; l <= ma; l++) |
696 | { |
697 | a[l] = atry[l]; |
698 | } |
699 | } |
700 | else |
701 | { |
702 | *alamda *= 10.0; |
703 | *chisq = ochisq; |
704 | } |
705 | return TRUE1; |
706 | } |
707 | |
708 | void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[], |
709 | int ia[], int ma, real **alpha, real beta[], real *chisq, |
710 | void (*funcs)(real, real [], real *, real[])) |
711 | |
712 | |
713 | |
714 | { |
715 | int i, j, k, l, m, mfit = 0; |
716 | real ymod, wt, sig2i, dy, *dyda; |
717 | |
718 | dyda = rvector(1, ma); |
719 | for (j = 1; j <= ma; j++) |
720 | { |
721 | if (ia[j]) |
722 | { |
723 | mfit++; |
724 | } |
725 | } |
726 | for (j = 1; j <= mfit; j++) |
727 | { |
728 | for (k = 1; k <= j; k++) |
729 | { |
730 | alpha[j][k] = 0.0; |
731 | } |
732 | beta[j] = 0.0; |
733 | } |
734 | *chisq = 0.0; |
735 | for (i = 1; i <= ndata; i++) |
736 | { |
737 | (*funcs)(x[i], a, &ymod, dyda); |
738 | sig2i = 1.0/(sig[i]*sig[i]); |
739 | dy = y[i]-ymod; |
740 | for (j = 0, l = 1; l <= ma; l++) |
741 | { |
742 | if (ia[l]) |
743 | { |
744 | wt = dyda[l]*sig2i; |
745 | for (j++, k = 0, m = 1; m <= l; m++) |
746 | { |
747 | if (ia[m]) |
748 | { |
749 | alpha[j][++k] += wt*dyda[m]; |
750 | } |
751 | } |
752 | beta[j] += dy*wt; |
753 | } |
754 | } |
755 | *chisq += dy*dy*sig2i; |
756 | } |
757 | for (j = 2; j <= mfit; j++) |
758 | { |
759 | for (k = 1; k < j; k++) |
760 | { |
761 | alpha[k][j] = alpha[j][k]; |
762 | } |
763 | } |
764 | free_vector(dyda, 1); |
765 | } |