Merge branch 'release-4-5-patches' into orderparm
[gromacs/rigid-bodies.git] / src / tools / levenmar.c
blobb967a404198a7aee3832becc6ada71b1a36d5123
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.
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.
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.
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.
31 * For more info, check our website at http://www.gromacs.org
33 * And Hey:
34 * Green Red Orange Magenta Azure Cyan Skyblue
36 #ifdef HAVE_CONFIG_H
37 #include <config.h>
38 #endif
40 #include <math.h>
41 #include <stdio.h>
42 #include <stdlib.h>
44 #include "types/simple.h"
46 static void nrerror(const char error_text[], gmx_bool bExit)
48 fprintf(stderr,"Numerical Recipes run-time error...\n");
49 fprintf(stderr,"%s\n",error_text);
50 if (bExit) {
51 fprintf(stderr,"...now exiting to system...\n");
52 exit(1);
56 /* dont use the keyword vector - it will clash with the
57 * altivec extensions used for powerpc processors.
60 static real *rvector(int nl,int nh)
62 real *v;
64 v=(real *)malloc((unsigned) (nh-nl+1)*sizeof(real));
65 if (!v) nrerror("allocation failure in rvector()", TRUE);
66 return v-nl;
69 static int *ivector(int nl, int nh)
71 int *v;
73 v=(int *)malloc((unsigned) (nh-nl+1)*sizeof(int));
74 if (!v) nrerror("allocation failure in ivector()", TRUE);
75 return v-nl;
78 static double *dvector(int nl, int nh)
80 double *v;
82 v=(double *)malloc((unsigned) (nh-nl+1)*sizeof(double));
83 if (!v) nrerror("allocation failure in dvector()", TRUE);
84 return v-nl;
89 static real **matrix1(int nrl, int nrh, int ncl, int nch)
91 int i;
92 real **m;
94 m=(real **) malloc((unsigned) (nrh-nrl+1)*sizeof(real*));
95 if (!m) nrerror("allocation failure 1 in matrix1()", TRUE);
96 m -= nrl;
98 for(i=nrl;i<=nrh;i++) {
99 m[i]=(real *) malloc((unsigned) (nch-ncl+1)*sizeof(real));
100 if (!m[i]) nrerror("allocation failure 2 in matrix1()", TRUE);
101 m[i] -= ncl;
103 return m;
106 static double **dmatrix(int nrl, int nrh, int ncl, int nch)
108 int i;
109 double **m;
111 m=(double **) malloc((unsigned) (nrh-nrl+1)*sizeof(double*));
112 if (!m) nrerror("allocation failure 1 in dmatrix()", TRUE);
113 m -= nrl;
115 for(i=nrl;i<=nrh;i++) {
116 m[i]=(double *) malloc((unsigned) (nch-ncl+1)*sizeof(double));
117 if (!m[i]) nrerror("allocation failure 2 in dmatrix()", TRUE);
118 m[i] -= ncl;
120 return m;
123 static int **imatrix1(int nrl, int nrh, int ncl, int nch)
125 int i,**m;
127 m=(int **)malloc((unsigned) (nrh-nrl+1)*sizeof(int*));
128 if (!m) nrerror("allocation failure 1 in imatrix1()", TRUE);
129 m -= nrl;
131 for(i=nrl;i<=nrh;i++) {
132 m[i]=(int *)malloc((unsigned) (nch-ncl+1)*sizeof(int));
133 if (!m[i]) nrerror("allocation failure 2 in imatrix1()", TRUE);
134 m[i] -= ncl;
136 return m;
141 static real **submatrix(real **a, int oldrl, int oldrh, int oldcl,
142 int newrl, int newcl)
144 int i,j;
145 real **m;
147 m=(real **) malloc((unsigned) (oldrh-oldrl+1)*sizeof(real*));
148 if (!m) nrerror("allocation failure in submatrix()", TRUE);
149 m -= newrl;
151 for(i=oldrl,j=newrl;i<=oldrh;i++,j++) m[j]=a[i]+oldcl-newcl;
153 return m;
158 static void free_vector(real *v, int nl)
160 free((char*) (v+nl));
163 static void free_ivector(int *v, int nl)
165 free((char*) (v+nl));
168 static void free_dvector(int *v, int nl)
170 free((char*) (v+nl));
175 static void free_matrix(real **m, int nrl, int nrh, int ncl)
177 int i;
179 for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
180 free((char*) (m+nrl));
183 static void free_dmatrix(double **m, int nrl, int nrh, int ncl)
185 int i;
187 for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
188 free((char*) (m+nrl));
191 static void free_imatrix(int **m, int nrl, int nrh, int ncl)
193 int i;
195 for(i=nrh;i>=nrl;i--) free((char*) (m[i]+ncl));
196 free((char*) (m+nrl));
201 static void free_submatrix(real **b, int nrl)
203 free((char*) (b+nrl));
208 static real **convert_matrix(real *a, int nrl, int nrh, int ncl, int nch)
210 int i,j,nrow,ncol;
211 real **m;
213 nrow=nrh-nrl+1;
214 ncol=nch-ncl+1;
215 m = (real **) malloc((unsigned) (nrow)*sizeof(real*));
216 if (!m) nrerror("allocation failure in convert_matrix()", TRUE);
217 m -= nrl;
218 for(i=0,j=nrl;i<=nrow-1;i++,j++) m[j]=a+ncol*i-ncl;
219 return m;
224 static void free_convert_matrix(real **b, int nrl)
226 free((char*) (b+nrl));
229 #define SWAP(a,b) {real temp=(a);(a)=(b);(b)=temp;}
231 static void dump_mat(int n,real **a)
233 int i,j;
235 for(i=1; (i<=n); i++) {
236 for(j=1; (j<=n); j++)
237 fprintf(stderr," %10.3f",a[i][j]);
238 fprintf(stderr,"\n");
242 gmx_bool gaussj(real **a, int n, real **b, int m)
244 int *indxc,*indxr,*ipiv;
245 int i,icol=0,irow=0,j,k,l,ll;
246 real big,dum,pivinv;
248 indxc=ivector(1,n);
249 indxr=ivector(1,n);
250 ipiv=ivector(1,n);
251 for (j=1;j<=n;j++) ipiv[j]=0;
252 for (i=1;i<=n;i++) {
253 big=0.0;
254 for (j=1;j<=n;j++)
255 if (ipiv[j] != 1)
256 for (k=1;k<=n;k++) {
257 if (ipiv[k] == 0) {
258 if (fabs(a[j][k]) >= big) {
259 big=fabs(a[j][k]);
260 irow=j;
261 icol=k;
263 } else if (ipiv[k] > 1) {
264 nrerror("GAUSSJ: Singular Matrix-1", FALSE);
265 return FALSE;
268 ++(ipiv[icol]);
269 if (irow != icol) {
270 for (l=1;l<=n;l++) SWAP(a[irow][l],a[icol][l])
271 for (l=1;l<=m;l++) SWAP(b[irow][l],b[icol][l])
273 indxr[i]=irow;
274 indxc[i]=icol;
275 if (a[icol][icol] == 0.0) {
276 fprintf(stderr,"irow = %d, icol = %d\n",irow,icol);
277 dump_mat(n,a);
278 nrerror("GAUSSJ: Singular Matrix-2", FALSE);
279 return FALSE;
281 pivinv=1.0/a[icol][icol];
282 a[icol][icol]=1.0;
283 for (l=1;l<=n;l++) a[icol][l] *= pivinv;
284 for (l=1;l<=m;l++) b[icol][l] *= pivinv;
285 for (ll=1;ll<=n;ll++)
286 if (ll != icol) {
287 dum=a[ll][icol];
288 a[ll][icol]=0.0;
289 for (l=1;l<=n;l++) a[ll][l] -= a[icol][l]*dum;
290 for (l=1;l<=m;l++) b[ll][l] -= b[icol][l]*dum;
293 for (l=n;l>=1;l--) {
294 if (indxr[l] != indxc[l])
295 for (k=1;k<=n;k++)
296 SWAP(a[k][indxr[l]],a[k][indxc[l]]);
298 free_ivector(ipiv,1);
299 free_ivector(indxr,1);
300 free_ivector(indxc,1);
302 return TRUE;
305 #undef SWAP
308 static void covsrt(real **covar, int ma, int lista[], int mfit)
310 int i,j;
311 real swap;
313 for (j=1;j<ma;j++)
314 for (i=j+1;i<=ma;i++) covar[i][j]=0.0;
315 for (i=1;i<mfit;i++)
316 for (j=i+1;j<=mfit;j++) {
317 if (lista[j] > lista[i])
318 covar[lista[j]][lista[i]]=covar[i][j];
319 else
320 covar[lista[i]][lista[j]]=covar[i][j];
322 swap=covar[1][1];
323 for (j=1;j<=ma;j++) {
324 covar[1][j]=covar[j][j];
325 covar[j][j]=0.0;
327 covar[lista[1]][lista[1]]=swap;
328 for (j=2;j<=mfit;j++) covar[lista[j]][lista[j]]=covar[1][j];
329 for (j=2;j<=ma;j++)
330 for (i=1;i<=j-1;i++) covar[i][j]=covar[j][i];
333 #define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}
335 static void covsrt_new(real **covar,int ma, int ia[], int mfit)
336 /* Expand in storage the covariance matrix covar, so as to take
337 * into account parameters that are being held fixed. (For the
338 * latter, return zero covariances.)
341 int i,j,k;
342 real swap;
343 for (i=mfit+1;i<=ma;i++)
344 for (j=1;j<=i;j++) covar[i][j]=covar[j][i]=0.0;
345 k=mfit;
346 for (j=ma;j>=1;j--) {
347 if (ia[j]) {
348 for (i=1;i<=ma;i++) SWAP(covar[i][k],covar[i][j])
349 for (i=1;i<=ma;i++) SWAP(covar[k][i],covar[j][i])
350 k--;
354 #undef SWAP
356 static void mrqcof(real x[], real y[], real sig[], int ndata, real a[],
357 int ma, int lista[], int mfit,
358 real **alpha, real beta[], real *chisq,
359 void (*funcs)(real,real *,real *,real *))
361 int k,j,i;
362 real ymod,wt,sig2i,dy,*dyda;
364 dyda=rvector(1,ma);
365 for (j=1;j<=mfit;j++) {
366 for (k=1;k<=j;k++) alpha[j][k]=0.0;
367 beta[j]=0.0;
369 *chisq=0.0;
370 for (i=1;i<=ndata;i++) {
371 (*funcs)(x[i],a,&ymod,dyda);
372 sig2i=1.0/(sig[i]*sig[i]);
373 dy=y[i]-ymod;
374 for (j=1;j<=mfit;j++) {
375 wt=dyda[lista[j]]*sig2i;
376 for (k=1;k<=j;k++)
377 alpha[j][k] += wt*dyda[lista[k]];
378 beta[j] += dy*wt;
380 (*chisq) += dy*dy*sig2i;
382 for (j=2;j<=mfit;j++)
383 for (k=1;k<=j-1;k++) alpha[k][j]=alpha[j][k];
384 free_vector(dyda,1);
388 gmx_bool mrqmin(real x[], real y[], real sig[], int ndata, real a[],
389 int ma, int lista[], int mfit,
390 real **covar, real **alpha, real *chisq,
391 void (*funcs)(real,real *,real *,real *),
392 real *alamda)
394 int k,kk,j,ihit;
395 static real *da,*atry,**oneda,*beta,ochisq;
397 if (*alamda < 0.0) {
398 oneda=matrix1(1,mfit,1,1);
399 atry=rvector(1,ma);
400 da=rvector(1,ma);
401 beta=rvector(1,ma);
402 kk=mfit+1;
403 for (j=1;j<=ma;j++) {
404 ihit=0;
405 for (k=1;k<=mfit;k++)
406 if (lista[k] == j) ihit++;
407 if (ihit == 0)
408 lista[kk++]=j;
409 else if (ihit > 1) {
410 nrerror("Bad LISTA permutation in MRQMIN-1", FALSE);
411 return FALSE;
414 if (kk != ma+1) {
415 nrerror("Bad LISTA permutation in MRQMIN-2", FALSE);
416 return FALSE;
418 *alamda=0.001;
419 mrqcof(x,y,sig,ndata,a,ma,lista,mfit,alpha,beta,chisq,funcs);
420 ochisq=(*chisq);
422 for (j=1;j<=mfit;j++) {
423 for (k=1;k<=mfit;k++) covar[j][k]=alpha[j][k];
424 covar[j][j]=alpha[j][j]*(1.0+(*alamda));
425 oneda[j][1]=beta[j];
427 if (!gaussj(covar,mfit,oneda,1))
428 return FALSE;
429 for (j=1;j<=mfit;j++)
430 da[j]=oneda[j][1];
431 if (*alamda == 0.0) {
432 covsrt(covar,ma,lista,mfit);
433 free_vector(beta,1);
434 free_vector(da,1);
435 free_vector(atry,1);
436 free_matrix(oneda,1,mfit,1);
437 return TRUE;
439 for (j=1;j<=ma;j++) atry[j]=a[j];
440 for (j=1;j<=mfit;j++)
441 atry[lista[j]] = a[lista[j]]+da[j];
442 mrqcof(x,y,sig,ndata,atry,ma,lista,mfit,covar,da,chisq,funcs);
443 if (*chisq < ochisq) {
444 *alamda *= 0.1;
445 ochisq=(*chisq);
446 for (j=1;j<=mfit;j++) {
447 for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
448 beta[j]=da[j];
449 a[lista[j]]=atry[lista[j]];
451 } else {
452 *alamda *= 10.0;
453 *chisq=ochisq;
455 return TRUE;
459 gmx_bool mrqmin_new(real x[],real y[],real sig[],int ndata,real a[],
460 int ia[],int ma,real **covar,real **alpha,real *chisq,
461 void (*funcs)(real, real [], real *, real []),
462 real *alamda)
463 /* Levenberg-Marquardt method, attempting to reduce the value Chi^2
464 * of a fit between a set of data points x[1..ndata], y[1..ndata]
465 * with individual standard deviations sig[1..ndata], and a nonlinear
466 * function dependent on ma coefficients a[1..ma]. The input array
467 * ia[1..ma] indicates by nonzero entries those components of a that
468 * should be fitted for, and by zero entries those components that
469 * should be held fixed at their input values. The program returns
470 * current best-fit values for the parameters a[1..ma], and
471 * Chi^2 = chisq. The arrays covar[1..ma][1..ma], alpha[1..ma][1..ma]
472 * are used as working space during most iterations. Supply a routine
473 * funcs(x,a,yfit,dyda,ma) that evaluates the fitting function yfit,
474 * and its derivatives dyda[1..ma] with respect to the fitting
475 * parameters a at x. On the first call provide an initial guess for
476 * the parameters a, and set alamda < 0 for initialization (which then
477 * sets alamda=.001). If a step succeeds chisq becomes smaller and
478 * alamda de-creases by a factor of 10. If a step fails alamda grows by
479 * a factor of 10. You must call this routine repeatedly until
480 * convergence is achieved. Then, make one final call with alamda=0,
481 * so that covar[1..ma][1..ma] returns the covariance matrix, and alpha
482 * the curvature matrix.
483 * (Parameters held fixed will return zero covariances.)
486 void covsrt(real **covar, int ma, int ia[], int mfit);
487 gmx_bool gaussj(real **a, int n, real **b,int m);
488 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
489 int ia[], int ma, real **alpha, real beta[], real *chisq,
490 void (*funcs)(real, real [], real *, real []));
491 int j,k,l;
492 static int mfit;
493 static real ochisq,*atry,*beta,*da,**oneda;
495 if (*alamda < 0.0) { /* Initialization. */
496 atry=rvector(1,ma);
497 beta=rvector(1,ma);
498 da=rvector(1,ma);
499 for (mfit=0,j=1;j<=ma;j++)
500 if (ia[j]) mfit++;
501 oneda=matrix1(1,mfit,1,1);
502 *alamda=0.001;
503 mrqcof_new(x,y,sig,ndata,a,ia,ma,alpha,beta,chisq,funcs);
504 ochisq=(*chisq);
505 for (j=1;j<=ma;j++)
506 atry[j]=a[j];
508 for (j=1;j<=mfit;j++) { /* Alter linearized fitting matrix, by augmenting. */
509 for (k=1;k<=mfit;k++)
510 covar[j][k]=alpha[j][k]; /* diagonal elements. */
511 covar[j][j]=alpha[j][j]*(1.0+(*alamda));
512 oneda[j][1]=beta[j];
514 if (!gaussj(covar,mfit,oneda,1)) /* Matrix solution. */
515 return FALSE;
516 for (j=1;j<=mfit;j++)
517 da[j]=oneda[j][1];
518 if (*alamda == 0.0) { /* Once converged, evaluate covariance matrix. */
519 covsrt_new(covar,ma,ia,mfit);
520 free_matrix(oneda,1,mfit,1);
521 free_vector(da,1);
522 free_vector(beta,1);
523 free_vector(atry,1);
524 return TRUE;
526 for (j=0,l=1;l<=ma;l++) /* Did the trial succeed? */
527 if (ia[l]) atry[l]=a[l]+da[++j];
528 mrqcof_new(x,y,sig,ndata,atry,ia,ma,covar,da,chisq,funcs);
529 if (*chisq < ochisq) {
530 /* Success, accept the new solution. */
531 *alamda *= 0.1;
532 ochisq=(*chisq);
533 for (j=1;j<=mfit;j++) {
534 for (k=1;k<=mfit;k++) alpha[j][k]=covar[j][k];
535 beta[j]=da[j];
537 for (l=1;l<=ma;l++) a[l]=atry[l];
538 } else { /* Failure, increase alamda and return. */
539 *alamda *= 10.0;
540 *chisq=ochisq;
542 return TRUE;
545 void mrqcof_new(real x[], real y[], real sig[], int ndata, real a[],
546 int ia[], int ma, real **alpha, real beta[], real *chisq,
547 void (*funcs)(real, real [], real *, real[]))
548 /* Used by mrqmin to evaluate the linearized fitting matrix alpha, and
549 * vector beta as in (15.5.8), and calculate Chi^2.
552 int i,j,k,l,m,mfit=0;
553 real ymod,wt,sig2i,dy,*dyda;
555 dyda=rvector(1,ma);
556 for (j=1;j<=ma;j++)
557 if (ia[j]) mfit++;
558 for (j=1;j<=mfit;j++) { /* Initialize (symmetric) alpha), beta. */
559 for (k=1;k<=j;k++) alpha[j][k]=0.0;
560 beta[j]=0.0;
562 *chisq=0.0;
563 for (i=1;i<=ndata;i++) { /* Summation loop over all data. */
564 (*funcs)(x[i],a,&ymod,dyda);
565 sig2i=1.0/(sig[i]*sig[i]);
566 dy=y[i]-ymod;
567 for (j=0,l=1;l<=ma;l++) {
568 if (ia[l]) {
569 wt=dyda[l]*sig2i;
570 for (j++,k=0,m=1;m<=l;m++)
571 if (ia[m]) alpha[j][++k] += wt*dyda[m];
572 beta[j] += dy*wt;
575 *chisq += dy*dy*sig2i; /* And find Chi^2. */
577 for (j=2;j<=mfit;j++) /* Fill in the symmetric side. */
578 for (k=1;k<j;k++) alpha[k][j]=alpha[j][k];
579 free_vector(dyda,1);