Merge branch 'release-4-6' into master
[alexxy/gromacs.git] / src / gromacs / gmxana / levenmar.c
1 /*
2  * $Id: levenmar.c,v 1.20 2004/01/23 18:11:02 lindahl Exp $
3  *
4  *                This source code is part of
5  *
6  *                 G   R   O   M   A   C   S
7  *
8  *          GROningen MAchine for Chemical Simulations
9  *
10  *                        VERSION 3.2.0
11  * Written by David van der Spoel, Erik Lindahl, Berk Hess, and others.
12  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
13  * Copyright (c) 2001-2004, The GROMACS development team,
14  * check out http://www.gromacs.org for more information.
15
16  * This program is free software; you can redistribute it and/or
17  * modify it under the terms of the GNU General Public License
18  * as published by the Free Software Foundation; either version 2
19  * of the License, or (at your option) any later version.
20  *
21  * If you want to redistribute modifications, please consider that
22  * scientific software is very special. Version control is crucial -
23  * bugs must be traceable. We will be happy to consider code for
24  * inclusion in the official distribution, but derived work must not
25  * be called official GROMACS. Details are found in the README & COPYING
26  * files - if they are missing, get the official version at www.gromacs.org.
27  *
28  * To help us fund GROMACS development, we humbly ask that you cite
29  * the papers on the package - you can find them in the top README file.
30  *
31  * For more info, check our website at http://www.gromacs.org
32  *
33  * And Hey:
34  * Green Red Orange Magenta Azure Cyan Skyblue
35  */
36 #ifdef HAVE_CONFIG_H
37 #include <config.h>
38 #endif
39
40 #include <math.h>
41 #include <stdio.h>
42 #include <stdlib.h>
43
44 #include "types/simple.h"
45
46 static void nrerror(const char error_text[], gmx_bool bExit)
47 {
48     fprintf(stderr, "Numerical Recipes run-time error...\n");
49     fprintf(stderr, "%s\n", error_text);
50     if (bExit)
51     {
52         fprintf(stderr, "...now exiting to system...\n");
53         exit(1);
54     }
55 }
56
57 /* dont use the keyword vector - it will clash with the
58  * altivec extensions used for powerpc processors.
59  */
60
61 static real *rvector(int nl, int nh)
62 {
63     real *v;
64
65     v = (real *)malloc((unsigned) (nh-nl+1)*sizeof(real));
66     if (!v)
67     {
68         nrerror("allocation failure in rvector()", TRUE);
69     }
70     return v-nl;
71 }
72
73 static int *ivector(int nl, int nh)
74 {
75     int *v;
76
77     v = (int *)malloc((unsigned) (nh-nl+1)*sizeof(int));
78     if (!v)
79     {
80         nrerror("allocation failure in ivector()", TRUE);
81     }
82     return v-nl;
83 }
84
85
86 static real **matrix1(int nrl, int nrh, int ncl, int nch)
87 {
88     int    i;
89     real **m;
90
91     m = (real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*));
92     if (!m)
93     {
94         nrerror("allocation failure 1 in matrix1()", TRUE);
95     }
96     m -= nrl;
97
98     for (i = nrl; i <= nrh; i++)
99     {
100         m[i] = (real *) malloc((unsigned) (nch-ncl+1)*sizeof(real));
101         if (!m[i])
102         {
103             nrerror("allocation failure 2 in matrix1()", TRUE);
104         }
105         m[i] -= ncl;
106     }
107     return m;
108 }
109
110 static double **dmatrix(int nrl, int nrh, int ncl, int nch)
111 {
112     int      i;
113     double **m;
114
115     m = (double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
116     if (!m)
117     {
118         nrerror("allocation failure 1 in dmatrix()", TRUE);
119     }
120     m -= nrl;
121
122     for (i = nrl; i <= nrh; i++)
123     {
124         m[i] = (double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
125         if (!m[i])
126         {
127             nrerror("allocation failure 2 in dmatrix()", TRUE);
128         }
129         m[i] -= ncl;
130     }
131     return m;
132 }
133
134 static int **imatrix1(int nrl, int nrh, int ncl, int nch)
135 {
136     int i, **m;
137
138     m = (int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
139     if (!m)
140     {
141         nrerror("allocation failure 1 in imatrix1()", TRUE);
142     }
143     m -= nrl;
144
145     for (i = nrl; i <= nrh; i++)
146     {
147         m[i] = (int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
148         if (!m[i])
149         {
150             nrerror("allocation failure 2 in imatrix1()", TRUE);
151         }
152         m[i] -= ncl;
153     }
154     return m;
155 }
156
157
158
159 static real **submatrix(real **a, int oldrl, int oldrh, int oldcl,
160                         int newrl, int newcl)
161 {
162     int    i, j;
163     real **m;
164
165     m = (real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*));
166     if (!m)
167     {
168         nrerror("allocation failure in submatrix()", TRUE);
169     }
170     m -= newrl;
171
172     for (i = oldrl, j = newrl; i <= oldrh; i++, j++)
173     {
174         m[j] = a[i]+oldcl-newcl;
175     }
176
177     return m;
178 }
179
180
181
182 static void free_vector(real *v, int nl)
183 {
184     free((char*) (v+nl));
185 }
186
187 static void free_ivector(int *v, int nl)
188 {
189     free((char*) (v+nl));
190 }
191
192 static void free_dvector(int *v, int nl)
193 {
194     free((char*) (v+nl));
195 }
196
197
198
199 static void free_matrix(real **m, int nrl, int nrh, int ncl)
200 {
201     int i;
202
203     for (i = nrh; i >= nrl; i--)
204     {
205         free((char*) (m[i]+ncl));
206     }
207     free((char*) (m+nrl));
208 }
209
210 static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch)
211 {
212     int    i, j, nrow, ncol;
213     real **m;
214
215     nrow = nrh-nrl+1;
216     ncol = nch-ncl+1;
217     m    = (real **) malloc((unsigned) (nrow)*sizeof(real*));
218     if (!m)
219     {
220         nrerror("allocation failure in convert_matrix()", TRUE);
221     }
222     m -= nrl;
223     for (i = 0, j = nrl; i <= nrow-1; i++, j++)
224     {
225         m[j] = a+ncol*i-ncl;
226     }
227     return m;
228 }
229
230
231
232 static void free_convert_matrix(real **b, int nrl)
233 {
234     free((char*) (b+nrl));
235 }
236
237 #define SWAP(a, b) {real temp = (a); (a) = (b); (b) = temp; }
238
239 static void dump_mat(int n, real **a)
240 {
241     int i, j;
242
243     for (i = 1; (i <= n); i++)
244     {
245         for (j = 1; (j <= n); j++)
246         {
247             fprintf(stderr, "  %10.3f", a[i][j]);
248         }
249         fprintf(stderr, "\n");
250     }
251 }
252
253 gmx_bool gaussj(real **a, int n, real **b, int m)
254 {
255     int *indxc, *indxr, *ipiv;
256     int  i, icol = 0, irow = 0, j, k, l, ll;
257     real big, dum, pivinv;
258
259     indxc = ivector(1, n);
260     indxr = ivector(1, n);
261     ipiv  = ivector(1, n);
262     for (j = 1; j <= n; j++)
263     {
264         ipiv[j] = 0;
265     }
266     for (i = 1; i <= n; i++)
267     {
268         big = 0.0;
269         for (j = 1; j <= n; j++)
270         {
271             if (ipiv[j] != 1)
272             {
273                 for (k = 1; k <= n; k++)
274                 {
275                     if (ipiv[k] == 0)
276                     {
277                         if (fabs(a[j][k]) >= big)
278                         {
279                             big  = fabs(a[j][k]);
280                             irow = j;
281                             icol = k;
282                         }
283                     }
284                     else if (ipiv[k] > 1)
285                     {
286                         nrerror("GAUSSJ: Singular Matrix-1", FALSE);
287                         return FALSE;
288                     }
289                 }
290             }
291         }
292         ++(ipiv[icol]);
293         if (irow != icol)
294         {
295             for (l = 1; l <= n; l++)
296             {
297                 SWAP(a[irow][l], a[icol][l]);
298             }
299             for (l = 1; l <= m; l++)
300             {
301                 SWAP(b[irow][l], b[icol][l]);
302             }
303         }
304         indxr[i] = irow;
305         indxc[i] = icol;
306         if (a[icol][icol] == 0.0)
307         {
308             fprintf(stderr, "irow = %d, icol = %d\n", irow, icol);
309             dump_mat(n, a);
310             nrerror("GAUSSJ: Singular Matrix-2", FALSE);
311             return FALSE;
312         }
313         pivinv        = 1.0/a[icol][icol];
314         a[icol][icol] = 1.0;
315         for (l = 1; l <= n; l++)
316         {
317             a[icol][l] *= pivinv;
318         }
319         for (l = 1; l <= m; l++)
320         {
321             b[icol][l] *= pivinv;
322         }
323         for (ll = 1; ll <= n; ll++)
324         {
325             if (ll != icol)
326             {
327                 dum         = a[ll][icol];
328                 a[ll][icol] = 0.0;
329                 for (l = 1; l <= n; l++)
330                 {
331                     a[ll][l] -= a[icol][l]*dum;
332                 }
333                 for (l = 1; l <= m; l++)
334                 {
335                     b[ll][l] -= b[icol][l]*dum;
336                 }
337             }
338         }
339     }
340     for (l = n; l >= 1; l--)
341     {
342         if (indxr[l] != indxc[l])
343         {
344             for (k = 1; k <= n; k++)
345             {
346                 SWAP(a[k][indxr[l]], a[k][indxc[l]]);
347             }
348         }
349     }
350     free_ivector(ipiv, 1);
351     free_ivector(indxr, 1);
352     free_ivector(indxc, 1);
353
354     return TRUE;
355 }
356
357 #undef SWAP
358
359
360 static void covsrt(real **covar, int ma, int lista[], int mfit)
361 {
362     int  i, j;
363     real swap;
364
365     for (j = 1; j < ma; j++)
366     {
367         for (i = j+1; i <= ma; i++)
368         {
369             covar[i][j] = 0.0;
370         }
371     }
372     for (i = 1; i < mfit; i++)
373     {
374         for (j = i+1; j <= mfit; j++)
375         {
376             if (lista[j] > lista[i])
377             {
378                 covar[lista[j]][lista[i]] = covar[i][j];
379             }
380             else
381             {
382                 covar[lista[i]][lista[j]] = covar[i][j];
383             }
384         }
385     }
386     swap = covar[1][1];
387     for (j = 1; j <= ma; j++)
388     {
389         covar[1][j] = covar[j][j];
390         covar[j][j] = 0.0;
391     }
392     covar[lista[1]][lista[1]] = swap;
393     for (j = 2; j <= mfit; j++)
394     {
395         covar[lista[j]][lista[j]] = covar[1][j];
396     }
397     for (j = 2; j <= ma; j++)
398     {
399         for (i = 1; i <= j-1; i++)
400         {
401             covar[i][j] = covar[j][i];
402         }
403     }
404 }
405
406 #define SWAP(a, b) {swap = (a); (a) = (b); (b) = swap; }
407
408 static void covsrt_new(real **covar, int ma, int ia[], int mfit)
409 /* Expand in storage the covariance matrix covar, so as to take
410  * into account parameters that are being held fixed. (For the
411  * latter, return zero covariances.)
412  */
413 {
414     int  i, j, k;
415     real swap;
416     for (i = mfit+1; i <= ma; i++)
417     {
418         for (j = 1; j <= i; j++)
419         {
420             covar[i][j] = covar[j][i] = 0.0;
421         }
422     }
423     k = mfit;
424     for (j = ma; j >= 1; j--)
425     {
426         if (ia[j])
427         {
428             for (i = 1; i <= ma; i++)
429             {
430                 SWAP(covar[i][k], covar[i][j]);
431             }
432             for (i = 1; i <= ma; i++)
433             {
434                 SWAP(covar[k][i], covar[j][i]);
435             }
436             k--;
437         }
438     }
439 }
440 #undef SWAP
441
442 static void mrqcof(real x[], real y[], real sig[], int ndata, real a[],
443                    int ma, int lista[], int mfit,
444                    real **alpha, real beta[], real *chisq,
445                    void (*funcs)(real, real *, real *, real *))
446 {
447     int  k, j, i;
448     real ymod, wt, sig2i, dy, *dyda;
449
450     dyda = rvector(1, ma);
451     for (j = 1; j <= mfit; j++)
452     {
453         for (k = 1; k <= j; k++)
454         {
455             alpha[j][k] = 0.0;
456         }
457         beta[j] = 0.0;
458     }
459     *chisq = 0.0;
460     for (i = 1; i <= ndata; i++)
461     {
462         (*funcs)(x[i], a, &ymod, dyda);
463         sig2i = 1.0/(sig[i]*sig[i]);
464         dy    = y[i]-ymod;
465         for (j = 1; j <= mfit; j++)
466         {
467             wt = dyda[lista[j]]*sig2i;
468             for (k = 1; k <= j; k++)
469             {
470                 alpha[j][k] += wt*dyda[lista[k]];
471             }
472             beta[j] += dy*wt;
473         }
474         (*chisq) += dy*dy*sig2i;
475     }
476     for (j = 2; j <= mfit; j++)
477     {
478         for (k = 1; k <= j-1; k++)
479         {
480             alpha[k][j] = alpha[j][k];
481         }
482     }
483     free_vector(dyda, 1);
484 }
485
486
487 gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
488                 int ma, int lista[], int mfit,
489                 real **covar, real **alpha, real *chisq,
490                 void (*funcs)(real, real *, real *, real *),
491                 real *alamda)
492 {
493     int          k, kk, j, ihit;
494     static real *da, *atry, **oneda, *beta, ochisq;
495
496     if (*alamda < 0.0)
497     {
498         oneda = matrix1(1, mfit, 1, 1);
499         atry  = rvector(1, ma);
500         da    = rvector(1, ma);
501         beta  = rvector(1, ma);
502         kk    = mfit+1;
503         for (j = 1; j <= ma; j++)
504         {
505             ihit = 0;
506             for (k = 1; k <= mfit; k++)
507             {
508                 if (lista[k] == j)
509                 {
510                     ihit++;
511                 }
512             }
513             if (ihit == 0)
514             {
515                 lista[kk++] = j;
516             }
517             else if (ihit > 1)
518             {
519                 nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
520                 return FALSE;
521             }
522         }
523         if (kk != ma+1)
524         {
525             nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
526             return FALSE;
527         }
528         *alamda = 0.001;
529         mrqcof(x, y, sig, ndata, a, ma, lista, mfit, alpha, beta, chisq, funcs);
530         ochisq = (*chisq);
531     }
532     for (j = 1; j <= mfit; j++)
533     {
534         for (k = 1; k <= mfit; k++)
535         {
536             covar[j][k] = alpha[j][k];
537         }
538         covar[j][j] = alpha[j][j]*(1.0+(*alamda));
539         oneda[j][1] = beta[j];
540     }
541     if (!gaussj(covar, mfit, oneda, 1))
542     {
543         return FALSE;
544     }
545     for (j = 1; j <= mfit; j++)
546     {
547         da[j] = oneda[j][1];
548     }
549     if (*alamda == 0.0)
550     {
551         covsrt(covar, ma, lista, mfit);
552         free_vector(beta, 1);
553         free_vector(da, 1);
554         free_vector(atry, 1);
555         free_matrix(oneda, 1, mfit, 1);
556         return TRUE;
557     }
558     for (j = 1; j <= ma; j++)
559     {
560         atry[j] = a[j];
561     }
562     for (j = 1; j <= mfit; j++)
563     {
564         atry[lista[j]] = a[lista[j]]+da[j];
565     }
566     mrqcof(x, y, sig, ndata, atry, ma, lista, mfit, covar, da, chisq, funcs);
567     if (*chisq < ochisq)
568     {
569         *alamda *= 0.1;
570         ochisq   = (*chisq);
571         for (j = 1; j <= mfit; j++)
572         {
573             for (k = 1; k <= mfit; k++)
574             {
575                 alpha[j][k] = covar[j][k];
576             }
577             beta[j]     = da[j];
578             a[lista[j]] = atry[lista[j]];
579         }
580     }
581     else
582     {
583         *alamda *= 10.0;
584         *chisq   = ochisq;
585     }
586     return TRUE;
587 }
588
589
590 gmx_bool mrqmin_new(real x[], real y[], real sig[], int ndata, real a[],
591                     int ia[], int ma, real **covar, real **alpha, real *chisq,
592                     void (*funcs)(real, real [], real *, real []),
593                     real *alamda)
594 /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
595  * of a fit between a set of data points x[1..ndata], y[1..ndata]
596  * with individual standard deviations sig[1..ndata], and a nonlinear
597  * function dependent on ma coefficients a[1..ma]. The input array
598  * ia[1..ma] indicates by nonzero entries those components of a that
599  * should be fitted for, and by zero entries those components that
600  * should be held fixed at their input values. The program returns
601  * current best-fit values for the parameters a[1..ma], and
602  * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
603  * are used as working space during most iterations. Supply a routine
604  * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
605  * and its derivatives dyda[1..ma] with respect to the fitting
606  * parameters a at x. On the first call provide an initial guess for
607  * the parameters a, and set alamda < 0 for initialization (which then
608  * sets alamda=.001). If a step succeeds chisq becomes smaller and
609  * alamda de-creases by a factor of 10. If a step fails alamda grows by
610  * a factor of 10. You must call this routine repeatedly until
611  * convergence is achieved. Then, make one final call with alamda=0,
612  * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
613  * the curvature matrix.
614  * (Parameters held fixed will return zero covariances.)
615  */
616 {
617     void covsrt(real **covar, int ma, int ia[], int mfit);
618     gmx_bool gaussj(real **a, int n, real **b, int m);
619     void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
620                     int ia[], int ma, real **alpha, real beta[], real *chisq,
621                     void (*funcs)(real, real [], real *, real []));
622     int         j, k, l;
623     static int  mfit;
624     static real ochisq, *atry, *beta, *da, **oneda;
625
626     if (*alamda < 0.0)                    /* Initialization. */
627     {
628         atry = rvector(1, ma);
629         beta = rvector(1, ma);
630         da   = rvector(1, ma);
631         for (mfit = 0, j = 1; j <= ma; j++)
632         {
633             if (ia[j])
634             {
635                 mfit++;
636             }
637         }
638         oneda   = matrix1(1, mfit, 1, 1);
639         *alamda = 0.001;
640         mrqcof_new(x, y, sig, ndata, a, ia, ma, alpha, beta, chisq, funcs);
641         ochisq = (*chisq);
642         for (j = 1; j <= ma; j++)
643         {
644             atry[j] = a[j];
645         }
646     }
647     for (j = 1; j <= mfit; j++) /* Alter linearized fitting matrix, by augmenting. */
648     {
649         for (k = 1; k <= mfit; k++)
650         {
651             covar[j][k] = alpha[j][k]; /* diagonal elements. */
652         }
653         covar[j][j] = alpha[j][j]*(1.0+(*alamda));
654         oneda[j][1] = beta[j];
655     }
656     if (!gaussj(covar, mfit, oneda, 1)) /* Matrix solution. */
657     {
658         return FALSE;
659     }
660     for (j = 1; j <= mfit; j++)
661     {
662         da[j] = oneda[j][1];
663     }
664     if (*alamda == 0.0) /* Once converged, evaluate covariance matrix. */
665     {
666         covsrt_new(covar, ma, ia, mfit);
667         free_matrix(oneda, 1, mfit, 1);
668         free_vector(da, 1);
669         free_vector(beta, 1);
670         free_vector(atry, 1);
671         return TRUE;
672     }
673     for (j = 0, l = 1; l <= ma; l++) /* Did the trial succeed? */
674     {
675         if (ia[l])
676         {
677             atry[l] = a[l]+da[++j];
678         }
679     }
680     mrqcof_new(x, y, sig, ndata, atry, ia, ma, covar, da, chisq, funcs);
681     if (*chisq < ochisq)
682     {
683         /* Success, accept the new solution. */
684         *alamda *= 0.1;
685         ochisq   = (*chisq);
686         for (j = 1; j <= mfit; j++)
687         {
688             for (k = 1; k <= mfit; k++)
689             {
690                 alpha[j][k] = covar[j][k];
691             }
692             beta[j] = da[j];
693         }
694         for (l = 1; l <= ma; l++)
695         {
696             a[l] = atry[l];
697         }
698     }
699     else                 /* Failure, increase alamda and return. */
700     {
701         *alamda *= 10.0;
702         *chisq   = ochisq;
703     }
704     return TRUE;
705 }
706
707 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
708                 int ia[], int ma, real **alpha, real beta[], real *chisq,
709                 void (*funcs)(real, real [], real *, real[]))
710 /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
711  * vector beta as in (15.5.8), and calculate Chi^2.
712  */
713 {
714     int  i, j, k, l, m, mfit = 0;
715     real ymod, wt, sig2i, dy, *dyda;
716
717     dyda = rvector(1, ma);
718     for (j = 1; j <= ma; j++)
719     {
720         if (ia[j])
721         {
722             mfit++;
723         }
724     }
725     for (j = 1; j <= mfit; j++) /* Initialize (symmetric) alpha), beta. */
726     {
727         for (k = 1; k <= j; k++)
728         {
729             alpha[j][k] = 0.0;
730         }
731         beta[j] = 0.0;
732     }
733     *chisq = 0.0;
734     for (i = 1; i <= ndata; i++) /* Summation loop over all data. */
735     {
736         (*funcs)(x[i], a, &ymod, dyda);
737         sig2i = 1.0/(sig[i]*sig[i]);
738         dy    = y[i]-ymod;
739         for (j = 0, l = 1; l <= ma; l++)
740         {
741             if (ia[l])
742             {
743                 wt = dyda[l]*sig2i;
744                 for (j++, k = 0, m = 1; m <= l; m++)
745                 {
746                     if (ia[m])
747                     {
748                         alpha[j][++k] += wt*dyda[m];
749                     }
750                 }
751                 beta[j] += dy*wt;
752             }
753         }
754         *chisq += dy*dy*sig2i;  /* And find Chi^2. */
755     }
756     for (j = 2; j <= mfit; j++) /* Fill in the symmetric side. */
757     {
758         for (k = 1; k < j; k++)
759         {
760             alpha[k][j] = alpha[j][k];
761         }
762     }
763     free_vector(dyda, 1);
764 }