Merge release-4-6 into master
[alexxy/gromacs.git] / src / gromacs / gmxana / levenmar.c
1 /*
2  * This file is part of the GROMACS molecular simulation package.
3  *
4  * Copyright (c) 1991-2000, University of Groningen, The Netherlands.
5  * Copyright (c) 2001-2004, The GROMACS development team.
6  * Copyright (c) 2013, by the GROMACS development team, led by
7  * Mark Abraham, David van der Spoel, Berk Hess, and Erik Lindahl,
8  * and including many others, as listed in the AUTHORS file in the
9  * top-level source directory and at http://www.gromacs.org.
10  *
11  * GROMACS is free software; you can redistribute it and/or
12  * modify it under the terms of the GNU Lesser General Public License
13  * as published by the Free Software Foundation; either version 2.1
14  * of the License, or (at your option) any later version.
15  *
16  * GROMACS is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
19  * Lesser General Public License for more details.
20  *
21  * You should have received a copy of the GNU Lesser General Public
22  * License along with GROMACS; if not, see
23  * http://www.gnu.org/licenses, or write to the Free Software Foundation,
24  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA.
25  *
26  * If you want to redistribute modifications to GROMACS, please
27  * consider that scientific software is very special. Version
28  * control is crucial - bugs must be traceable. We will be happy to
29  * consider code for inclusion in the official distribution, but
30  * derived work must not be called official GROMACS. Details are found
31  * in the README & COPYING files - if they are missing, get the
32  * official version at http://www.gromacs.org.
33  *
34  * To help us fund GROMACS development, we humbly ask that you cite
35  * the research papers on the package. Check out http://www.gromacs.org.
36  */
37 #ifdef HAVE_CONFIG_H
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(stderr, "Numerical Recipes run-time error...\n");
50     fprintf(stderr, "%s\n", error_text);
51     if (bExit)
52     {
53         fprintf(stderr, "...now exiting to system...\n");
54         exit(1);
55     }
56 }
57
58 /* dont use the keyword vector - it will clash with the
59  * altivec extensions used for powerpc processors.
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()", TRUE);
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()", TRUE);
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()", TRUE);
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()", TRUE);
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()", TRUE);
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()", TRUE);
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)
141     {
142         nrerror("allocation failure 1 in imatrix1()", TRUE);
143     }
144     m -= nrl;
145
146     for (i = nrl; i <= nrh; i++)
147     {
148         m[i] = (int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
149         if (!m[i])
150         {
151             nrerror("allocation failure 2 in imatrix1()", TRUE);
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()", TRUE);
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()", TRUE);
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(stderr, "  %10.3f", a[i][j]);
249         }
250         fprintf(stderr, "\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", FALSE);
288                         return FALSE;
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(stderr, "irow = %d, icol = %d\n", irow, icol);
310             dump_mat(n, a);
311             nrerror("GAUSSJ: Singular Matrix-2", FALSE);
312             return FALSE;
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 TRUE;
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 /* Expand in storage the covariance matrix covar, so as to take
411  * into account parameters that are being held fixed. (For the
412  * latter, return zero covariances.)
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", FALSE);
521                 return FALSE;
522             }
523         }
524         if (kk != ma+1)
525         {
526             nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
527             return FALSE;
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 FALSE;
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 TRUE;
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 TRUE;
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 /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
596  * of a fit between a set of data points x[1..ndata], y[1..ndata]
597  * with individual standard deviations sig[1..ndata], and a nonlinear
598  * function dependent on ma coefficients a[1..ma]. The input array
599  * ia[1..ma] indicates by nonzero entries those components of a that
600  * should be fitted for, and by zero entries those components that
601  * should be held fixed at their input values. The program returns
602  * current best-fit values for the parameters a[1..ma], and
603  * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
604  * are used as working space during most iterations. Supply a routine
605  * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
606  * and its derivatives dyda[1..ma] with respect to the fitting
607  * parameters a at x. On the first call provide an initial guess for
608  * the parameters a, and set alamda < 0 for initialization (which then
609  * sets alamda=.001). If a step succeeds chisq becomes smaller and
610  * alamda de-creases by a factor of 10. If a step fails alamda grows by
611  * a factor of 10. You must call this routine repeatedly until
612  * convergence is achieved. Then, make one final call with alamda=0,
613  * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
614  * the curvature matrix.
615  * (Parameters held fixed will return zero covariances.)
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)                    /* Initialization. */
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++) /* Alter linearized fitting matrix, by augmenting. */
649     {
650         for (k = 1; k <= mfit; k++)
651         {
652             covar[j][k] = alpha[j][k]; /* diagonal elements. */
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)) /* Matrix solution. */
658     {
659         return FALSE;
660     }
661     for (j = 1; j <= mfit; j++)
662     {
663         da[j] = oneda[j][1];
664     }
665     if (*alamda == 0.0) /* Once converged, evaluate covariance matrix. */
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 TRUE;
673     }
674     for (j = 0, l = 1; l <= ma; l++) /* Did the trial succeed? */
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         /* Success, accept the new solution. */
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                 /* Failure, increase alamda and return. */
701     {
702         *alamda *= 10.0;
703         *chisq   = ochisq;
704     }
705     return TRUE;
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 /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
712  * vector beta as in (15.5.8), and calculate Chi^2.
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++) /* Initialize (symmetric) alpha), beta. */
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++) /* Summation loop over all data. */
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;  /* And find Chi^2. */
756     }
757     for (j = 2; j <= mfit; j++) /* Fill in the symmetric side. */
758     {
759         for (k = 1; k < j; k++)
760         {
761             alpha[k][j] = alpha[j][k];
762         }
763     }
764     free_vector(dyda, 1);
765 }