cg.c

来自「快速傅立叶变换程序代码,学信号的同学,可要注意了」· C语言 代码 · 共 1,027 行 · 第 1/2 页

/* #include <stdio.h>
#include <math.h> */
#include "./r.h" 
/* #include "../ansi/nrutil.h" */
#include "./mynr.h"

/* CONJUGATE GRADIENT ALGORITHMS */

/* Rewritten starting 9 June 1992 to make them ANSI */
/* And also to make them non-broken --- no external functions allowed 
   Temporarily reduced in size leaving full file as cg.c.all */

/* frprmn.c */
/* The NR conjugate gradients algorithm */

#define EPS 1.0e-10
#define FREEALL free_dvector(xi,1,n);free_dvector(h,1,n);free_dvector(g,1,n);
#define FRPVERBOSE 0

void frprmn
  (double *p,
   int    n,
   double ftol,
   double flinmintol,
   int    *iter,
   int    itmax,
   double *fret,
   double (*func)(double *, void *),
   void   *func_arg,
   void   (*dfunc)(double *,double *, void *),
   void   *dfunc_arg
)
/* double p[],ftol,*fret,(*func)(),flinmintol; */
{
  int j,its;
  double gg,gam,fp,dgg;
  double *g,*h,*xi;
  
  g=dvector(1,n);
  h=dvector(1,n);
  xi=dvector(1,n);
  fp=(*func)(p,func_arg);
  (*dfunc)(p,xi,dfunc_arg);
  for (j=1;j<=n;j++) {
    g[j] = -xi[j];
    xi[j]=h[j]=g[j];
  }
  for (its=1;its<=itmax;its++) {
    *iter=its;
    *fret = fp ; /* 21 05 94: pass the current value through to mnbrak */
    linmin(p,xi,n,fret,func,func_arg,flinmintol); 
    /* linmin * creates new vectors for p and xi (not necessary - cut)
              * calls mnbrak and then brent 
              * moves p to the new minimum when it is done,
              * and changes xi to the step used. (Note no use is made
	        of xi later).
	      * the initial step in the bracketing is equal to xi! 
	      * I now include a factor on that step. 
       mnbrak * evaluates func at p, unnecessarily */
    if (2.0*fabs(*fret-fp) <= ftol*(fabs(*fret)+fabs(fp)+EPS)) {
      FREEALL
      return;
    }
    fp=(*func)(p,func_arg);
    (*dfunc)(p,xi,dfunc_arg);
    dgg=gg=0.0;
    for (j=1;j<=n;j++) {
      gg += g[j]*g[j];
      /*		  dgg += xi[j]*xi[j];	*/
      dgg += (xi[j]+g[j])*xi[j];
    }
    if (gg == 0.0) {
      FREEALL
      return;
    }
    gam=dgg/gg;
    for (j=1;j<=n;j++) {
      g[j] = -xi[j];
      xi[j]=h[j]=g[j]+gam*h[j];
    }
  }
  printf("Too many iterations in FRPRMN, but continuing.\n"); 
  FREEALL	
  return;
}

#undef EPS
#undef FREEALL



/* linmin */

/* 
typedef struct {
  int n;
  double *p , *xi , *xt ;
  double (*nfunc)() ;
  void *func_arg ;
} f1dim_arg ; 
*/

void linmin
  (double *p,
   double *xi,
   int    n,
   double *fret,
   double (*func)(double *,void *),
   void   *func_arg,
   double linmin_tol
)
{
  int j;
  double xx,xmin,fx,fb,fa,bx,ax;
  double f1dim(double,void *);
  static double lastxx = 0.01 ; /* 1.0 might make more sense, but hey! */
  f1dim_arg f_arg;
  
  f_arg.n=n;
  f_arg.nfunc=func;
  f_arg.func_arg=func_arg;
  f_arg.p=p;              /* NB in NR, these are copied into global. 
			     Here I just copy the pointers */
  f_arg.xi=xi;
  f_arg.xt=dvector(1,n) ; /* new addition 21 05 94 : scratch pad for f1dim
			     to use */

  fa = *fret ; /* 21 05 94: pass the current value through to mnbrak */
  ax=0.0;
  xx=lastxx ;
  bx=2.0 * xx ;
  mnbrak ( &ax , &xx , &bx , &fa , &fx , &fb , f1dim , &f_arg); 
  if ( FRPVERBOSE > 1 ) printf ( "B %6.3g %6.3g %6.3g " , ax , xx , bx  ) ; 
/* typically these numbers are 1.0 0.0 -1.6
    but they can be 0.0 1.0 2.7 also. */
/* my guess is it would be good for the latter to happen sometimes ! */
/* Keep a note of the actual stretch factor needed.
   If I set it to xx (stable state), and then if ax is bigger, set it 
   to ax/2.0 ? */

  lastxx = fabs ( xx ) ;
  if ( fabs ( ax ) > lastxx ) lastxx =  fabs ( ax ) * 0.5 ;

  *fret=brent ( ax , xx , bx , f1dim , &f_arg , linmin_tol , &xmin);
  for (j=1;j<=n;j++) {
    xi[j] *= xmin; 
    p[j] += xi[j];
  }
  free_dvector(f_arg.xt , 1 , n);
}

/* brent.c */

#define ITMAX 100
#define CGOLD 0.3819660
#define ZEPS 1.0e-10
#define SIGN(a,b) ((b) > 0.0 ? fabs(a) : -fabs(a))
#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);

double brent
  (double ax,
   double bx,
   double cx,
   double (*f)(double,void *),
   void *f_arg,
   double tol,
   double *xmin
)
{
  int iter;
  double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
  double e=0.0;

  a=((ax < cx) ? ax : cx);
  b=((ax > cx) ? ax : cx);
  x=w=v=bx;
  fw=fv=fx=(*f)(x,f_arg);
  for (iter=1;iter<=ITMAX;iter++) {
    xm=0.5*(a+b);
    tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
    if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
      *xmin=x;
      return fx;
    }
    if (fabs(e) > tol1) {
      r=(x-w)*(fx-fv);
      q=(x-v)*(fx-fw);
      p=(x-v)*q-(x-w)*r;
      q=2.0*(q-r);
      if (q > 0.0) p = -p;
      q=fabs(q);
      etemp=e;
      e=d;    /* is this an uninitialized use of d ? */
      if (fabs(p) >= fabs(0.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x))
	d=CGOLD*(e=(x >= xm ? a-x : b-x));
      else {
	d=p/q;
	u=x+d;
	if (u-a < tol2 || b-u < tol2)
	  d=SIGN(tol1,xm-x);
      }
    } else {
      d=CGOLD*(e=(x >= xm ? a-x : b-x));
    }
    u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
		fu=(*f)(u,f_arg);
    if (fu <= fx) {
      if (u >= x) a=x; else b=x;
      SHFT(v,w,x,u)
	SHFT(fv,fw,fx,fu)
	} else {
	  if (u < x) a=u; else b=u;
	  if (fu <= fw || w == x) {
	    v=w;
	    w=u;
	    fv=fw;
	    fw=fu;
	  } else if (fu <= fv || v == x || v == w) {
	    v=u;
	    fv=fu;
	  }
	}
  }
  fprintf(stderr,"Too many iterations in BRENT");
  *xmin=x;
  return fx;
}

#undef ITMAX
#undef CGOLD
#undef ZEPS
#undef SIGN

/* dbrent.c */

#define ITMAX 100
#define ZEPS 1.0e-10
#define SIGN(a,b) ((b) > 0.0 ? fabs(a) : -fabs(a))
#define MOV3(a,b,c, d,e,f) (a)=(d);(b)=(e);(c)=(f);

double dbrent
  (double ax,
   double bx,
   double cx,
   double (*f)(double),
   double (*df)(double),
   double tol,
   double *xmin
)
{
  int iter,ok1,ok2;
  double a,b,d,d1,d2,du,dv,dw,dx,e=0.0;
  double fu,fv,fw,fx,olde,tol1,tol2,u,u1,u2,v,w,x,xm;
  
  a=(ax < cx ? ax : cx);
  b=(ax > cx ? ax : cx);
  x=w=v=bx;
  fw=fv=fx=(*f)(x);
  dw=dv=dx=(*df)(x);
  for (iter=1;iter<=ITMAX;iter++) {
    xm=0.5*(a+b);
    tol1=tol*fabs(x)+ZEPS;
    tol2=2.0*tol1;
    if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
      *xmin=x;
      return fx;
    }
    if (fabs(e) > tol1) {
      d1=2.0*(b-a);
      d2=d1;
      if (dw != dx)  d1=(w-x)*dx/(dx-dw);
      if (dv != dx)  d2=(v-x)*dx/(dx-dv);
      u1=x+d1;
      u2=x+d2;
      ok1 = (a-u1)*(u1-b) > 0.0 && dx*d1 <= 0.0;
      ok2 = (a-u2)*(u2-b) > 0.0 && dx*d2 <= 0.0;
      olde=e;
      e=d;
      if (ok1 || ok2) {
	if (ok1 && ok2)
	  d=(fabs(d1) < fabs(d2) ? d1 : d2);
	else if (ok1)
	  d=d1;
	else
	  d=d2;
	if (fabs(d) <= fabs(0.5*olde)) {
	  u=x+d;
	  if (u-a < tol2 || b-u < tol2)
	    d=SIGN(tol1,xm-x);
	} else {
	  d=0.5*(e=(dx >= 0.0 ? a-x : b-x));
	}
      } else {
	d=0.5*(e=(dx >= 0.0 ? a-x : b-x));
      }
    } else {
      d=0.5*(e=(dx >= 0.0 ? a-x : b-x));
    }
    if (fabs(d) >= tol1) {
      u=x+d;
      fu=(*f)(u);
    } else {
      u=x+SIGN(tol1,d);
      fu=(*f)(u);
      if (fu > fx) {
	*xmin=x;
	return fx;
      }
    }
    du=(*df)(u);
    if (fu <= fx) {
      if (u >= x) a=x; else b=x;
      MOV3(v,fv,dv, w,fw,dw)
	MOV3(w,fw,dw, x,fx,dx)
	  MOV3(x,fx,dx, u,fu,du)
    } else {
      if (u < x) a=u; else b=u;
      if (fu <= fw || w == x) {
	MOV3(v,fv,dv, w,fw,dw)
	  MOV3(w,fw,dw, u,fu,du)
      } else if (fu < fv || v == x || v == w) {
	MOV3(v,fv,dv, u,fu,du)
      }
    }
  }
  nrerror("Too many iterations in routine DBRENT");
  return (0.0) ;
}

#undef ITMAX
#undef ZEPS
#undef SIGN
#undef MOV3



/* mnbrak.c */

#define GOLD 1.618034
#define GLIMIT 100.0
#define TINY 1.0e-20
#define SIGN(a,b) ((b) > 0.0 ? fabs(a) : -fabs(a))
#define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
#ifndef MAX
#define MAX(a,b) ((a) > (b) ? (a) : (b) )
#endif

void mnbrak
(double *ax,
 double *bx,
 double *cx,
 double *fa,
 double *fb,
 double *fc,
 double (*func)(double,void *),
 void   *f_arg
 )
{
  double ulim,u,r,q,fu,dum ;
  
/*  *fa = ( *func ) ( *ax,f_arg ) ; */
/* I have cut this because in my applications, func has been evaluated just */
/* I pass this value instead */
  *fb = ( *func ) ( *bx,f_arg ) ;
  if  ( *fb > *fa ) {
    SHFT ( dum,*ax,*bx,dum )
      SHFT ( dum,*fb,*fa,dum )
  }
  *cx = ( *bx ) + GOLD * ( *bx - *ax ) ;
  *fc = ( *func ) ( *cx,f_arg ) ;
  while  ( *fb > *fc ) {
    r = ( *bx - *ax ) * ( *fb - *fc ) ;
    q = ( *bx - *cx ) * ( *fb - *fa ) ;
    u = ( *bx ) -  (  ( *bx - *cx ) *q -  ( *bx - *ax ) *r )/
       ( 2.0*SIGN ( MAX ( fabs ( q - r ),TINY ),q - r ) ) ;
    ulim = ( *bx ) + GLIMIT * ( *cx - *bx ) ;
    if  (  ( *bx - u ) * ( u - *cx ) > 0.0 ) {
      fu = ( *func ) ( u , f_arg ) ;
      if  ( fu < *fc ) {
	*ax = ( *bx ) ;
	*bx = u ;
	*fa = ( *fb ) ;
	*fb = fu ;
	return ;
      } else if  ( fu > *fb ) {
	*cx = u ;
	*fc = fu ;
	return ;
      }
      u = ( *cx ) + GOLD* ( *cx - *bx ) ;
      fu = ( *func ) ( u,f_arg ) ;
    } else if  (  ( *cx - u ) * ( u - ulim ) > 0.0 ) {
      fu = ( *func ) ( u,f_arg ) ;
      if  ( fu < *fc ) {
	SHFT ( *bx,*cx,u,*cx + GOLD* ( *cx - *bx ) )
	  SHFT ( *fb,*fc,fu, ( *func ) ( u,f_arg ) )
      }
    } else if  (  ( u - ulim ) * ( ulim - *cx ) >= 0.0 ) {
      u =ulim ;
      fu = ( *func ) ( u,f_arg ) ;
    } else {
      u = ( *cx ) + GOLD* ( *cx - *bx ) ;
      fu = ( *func ) ( u,f_arg ) ;
    }
    SHFT ( *ax,*bx,*cx,u )
      SHFT ( *fa,*fb,*fc,fu )
  }
}

#undef GOLD
#undef GLIMIT
#undef TINY
#undef MAX
#undef SIGN
#undef SHFT



double f1dim
  (double x,
   void   *f_argp
) 
{
  int j;
  double f,*xt;
  f1dim_arg f_arg;
  
  f_arg = *( (f1dim_arg *)f_argp);
  xt= f_arg.xt ; 
  for (j=1;j<=f_arg.n;j++) xt[j]=f_arg.p[j]+x*f_arg.xi[j];
  f=(*f_arg.nfunc)(xt,f_arg.func_arg);
  return f;
}

/*
double df1dim
  (double x,
   void   *f_argp
) 
{
  int j ;
  double f = 0.0 , *xt , *df ;
  df1dim_arg f_arg ;
  
  f_arg = *( (df1dim_arg *)f_argp);
  xt=dvector(1,f_arg.n);
  df=dvector(1,f_arg.n);
  for ( j = 1 ; j <= f_arg.n ; j++ ) xt[j] = f_arg.p[j] + x * f_arg.xi[j] ;
  (*f_arg.ndfunc)( xt , df , f_arg.func_arg ) ;
  for ( j = 1 ; j <= f_arg.n ; j++ ) f += df[j] * f_arg.xi[j] ;
  free_dvector(xt,1,f_arg.n);
  free_dvector(df,1,f_arg.n);
  return f;
}
*/

/* dfpmin.c */
/* Modified 30 Nov 90 so that it uses allocated space for the Hessian */  
/* 	hessin=matrix(1,n,1,n);	must be declared and initialised previously */
/* also modified to force it to do at least *iter loops, (so that a good hessian 
estimate is made) */ 
/* also modified to pass a tolerance to linmin when it is called */
/* Modified 16 3 92 from dfpmin.c so that the functions func and dfunc both 
   include an additional structure argument that contains all the other variables */
/* So that globals are no longer needed */

#define ITMAX 400
#define EPS 1.0e-10

/* void dfpmin(p,n,ftol,iter,fret,func,func_arg,dfunc,dfunc_arg,hessin,frac_lin_tol)
double p[],ftol,*fret,(*func)(),**hessin,frac_lin_tol;
void (*dfunc)();
void *func_arg,*dfunc_arg;
int n,*iter; */
void dfpmin
  (double *p,
   int    n,
   double ftol,
   int    *iter,
   double *fret,
   double (*func)(double *,void *),
   void   *func_arg,
   void   (*dfunc)(double *, double *,void *),
   void   *dfunc_arg,
   double **hessin,
   double frac_lin_tol
)
{
  int j,i,its;
  double fp,fae,fad,fac;
  double *xi,*g,*dg,*hdg;
  int	min_its;
  
  min_its= *iter; 
  
  xi=dvector(1,n);
  g=dvector(1,n);
  dg=dvector(1,n);
  hdg=dvector(1,n);
  fp=(*func)(p,func_arg);
  (*dfunc)(p,g,dfunc_arg);
  
  for (i=1;i<=n;i++) {
    xi[i]=0.0;
    for (j=1;j<=n;j++) xi[i] -= hessin[i][j]*g[j];
  }
  for (its=1;its<=ITMAX;its++) {
    *iter=its;
    linmin(p,xi,n,fret,func,func_arg,frac_lin_tol);
    if ((its>=min_its)&&(2.0*fabs(*fret-fp) <= ftol*(fabs(*fret)+fabs(fp)+EPS))) {
      free_dvector(hdg,1,n);
      free_dvector(dg,1,n);
      free_dvector(g,1,n);
      free_dvector(xi,1,n);