nr_conjgrad.c

来自「LastWave」· C语言 代码 · 共 317 行

#include "lastwave.h"
#include "signals.h"


static LWFLOAT *vector(int nl, int nh)
{
	LWFLOAT *v;

	v=(LWFLOAT *) Malloc((unsigned) (nh-nl+1)*sizeof(LWFLOAT));
	if (!v) Errorf("allocation failure in vector()");
	return v-nl;
}

static void free_vector(LWFLOAT *v,int nl,int nh)
{
	Free((char*) (v+nl));
}



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

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

	a=((ax < cx) ? ax : cx);
	b=((ax > cx) ? ax : cx);
	x=w=v=bx;
	fw=fv=fx=(*f)(x);
	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;
			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=SIGN1(tol1,xm-x);
			}
		} else {
			d=CGOLD*(e=(x >= xm ? a-x : b-x));
		}
		u=(fabs(d) >= tol1 ? x+d : x+SIGN1(tol1,d));
		fu=(*f)(u);
		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;
			}
		}
	}
	Errorf("Too many iterations in BRENT");
	*xmin=x;
	return fx;
}

#undef ITMAX
#undef CGOLD
#undef ZEPS
#undef SIGN1


#define GOLD 1.618034
#define GLIMIT 100.0
#define TINY 1.0e-20
#define SIGN1(a,b) ((b) > 0.0 ? fabs(a) : -fabs(a))

static void mnbrak(LWFLOAT *ax,LWFLOAT *bx,LWFLOAT *cx,LWFLOAT *fa,LWFLOAT *fb,LWFLOAT *fc,LWFLOAT (*func)(LWFLOAT))
{
	LWFLOAT ulim,u,r,q,fu,dum;

	*fa=(*func)(*ax);
	*fb=(*func)(*bx);
	if (*fb > *fa) {
		SHFT(dum,*ax,*bx,dum)
		SHFT(dum,*fb,*fa,dum)
	}
	*cx=(*bx)+GOLD*(*bx-*ax);
	*fc=(*func)(*cx);
	while (*fb > *fc) {
		r=(*bx-*ax)*(*fb-*fc);
		q=(*bx-*cx)*(*fb-*fa);
		u=(*bx)-((*bx-*cx)*q-(*bx-*ax)*r)/
			(2.0*SIGN1(MAX(fabs(q-r),TINY),q-r));
		ulim=(*bx)+GLIMIT*(*cx-*bx);
		if ((*bx-u)*(u-*cx) > 0.0) {
			fu=(*func)(u);
			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);
		} else if ((*cx-u)*(u-ulim) > 0.0) {
			fu=(*func)(u);
			if (fu < *fc) {
				SHFT(*bx,*cx,u,*cx+GOLD*(*cx-*bx))
				SHFT(*fb,*fc,fu,(*func)(u))
			}
		} else if ((u-ulim)*(ulim-*cx) >= 0.0) {
			u=ulim;
			fu=(*func)(u);
		} else {
			u=(*cx)+GOLD*(*cx-*bx);
			fu=(*func)(u);
		}
		SHFT(*ax,*bx,*cx,u)
		SHFT(*fa,*fb,*fc,fu)
	}
}

#undef GOLD
#undef GLIMIT
#undef TINY
#undef SIGN1
#undef SHFT

/*
 * Minimization along a direction
 */
 
 #define TOL 2.0e-4
 
static int ncom=0;	/* defining declarations */
static LWFLOAT *pcom=0,*xicom=0,(*nrfunc)();

static LWFLOAT f1dim(LWFLOAT x)
{
	int j;
	LWFLOAT f,*xt;

	xt=vector(1,ncom);
	for (j=1;j<=ncom;j++) xt[j]=pcom[j]+x*xicom[j];
	f=(*nrfunc)(xt);
	free_vector(xt,1,ncom);
	return f;
}


static void linmin(LWFLOAT p[],LWFLOAT xi[],int n,LWFLOAT *fret,LWFLOAT (*func)(LWFLOAT p[]))
{
	int j;
	LWFLOAT xx,xmin,fx,fb,fa,bx,ax;
	ncom=n;
	pcom=vector(1,n);
	xicom=vector(1,n);
	nrfunc=func;
	for (j=1;j<=n;j++) {
		pcom[j]=p[j];
		xicom[j]=xi[j];
	}
	ax=0.0;
	xx=1.0;
	bx=2.0;
	mnbrak(&ax,&xx,&bx,&fa,&fx,&fb,f1dim);
	*fret=brent(ax,xx,bx,f1dim,TOL,&xmin);
	for (j=1;j<=n;j++) {
		xi[j] *= xmin;
		p[j] += xi[j];
	}
	free_vector(xicom,1,n);
	free_vector(pcom,1,n);
}

#undef TOL


/*
 * Conjugate gradient method
 */

 
#define ITMAX 2000         // Maximum number of iterations
#define EPS 1.0e-10       // Precision
#define FREEALL free_vector(xi,1,n);free_vector(h,1,n);free_vector(g,1,n);

static void frprmn(LWFLOAT p[], int n, LWFLOAT ftol, int *iter, LWFLOAT *fret, LWFLOAT (*func)(LWFLOAT p[]),void (*dfunc)(LWFLOAT p[], LWFLOAT xi[]), char flagPrint)
{
	int j,its,i;
	LWFLOAT gg,gam,fp,dgg;
	LWFLOAT *g,*h,*xi;
	void linmin();

	g=vector(1,n);
	h=vector(1,n);
	xi=vector(1,n);
	fp=(*func)(p);
	(*dfunc)(p,xi);
	for (j=1;j<=n;j++) {
		g[j] = -xi[j];
		xi[j]=h[j]=g[j];
	}
	for (its=1;its<=ITMAX;its++) {
		*iter=its;
		linmin(p,xi,n,fret,func);
		if (flagPrint) {
		  Printf("##Iteration %d : val = %f\n",its,*fret);
		  for (i=0;i<n;i++) Printf("%g ",p[i+1]);
		  Printf("\n");
		}
		if (2.0*fabs(*fret-fp) <= ftol*(fabs(*fret)+fabs(fp)+EPS)) {
			FREEALL
			return;
		}
		fp=(*func)(p);
		(*dfunc)(p,xi);
		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];
		}
	}
	Errorf("Too many iterations in FRPRMN");
}

#undef ITMAX
#undef EPS
#undef FREEALL


static SIGNAL theSig = NULL;
static SIGNAL theGrad = NULL;
static int theN;
static LWFLOAT (*theF)(SIGNAL p);
static void  (*theDF)(SIGNAL p, SIGNAL grad);

static LWFLOAT theFunc(LWFLOAT p[])
{
  LWFLOAT f;
  
  theSig->Y = p+1;
  theSig->size = theN;
  f = (*theF)(theSig);
  theSig->size = 0;
  theSig->Y = NULL;
    
  return(f);
}

static void theDFunc(LWFLOAT p[], LWFLOAT g[])
{
  theSig->Y = p+1;
  theSig->size = theN;
  theGrad->Y = g+1;
  theGrad->size = theN;
  (*theDF)(theSig,theGrad);
  theSig->size = 0;
  theSig->Y = NULL;
  theGrad->size = 0;  
  theGrad->Y = NULL;
}

void ConjGrad(SIGNAL startPoint, LWFLOAT tolerance, LWFLOAT (*f)(SIGNAL point),void (*df)(SIGNAL point,SIGNAL grad), int *nIter, LWFLOAT *minVal, char flagPrint)
{
  if (theSig == NULL) {
    theSig = NewSignal();
    theGrad = NewSignal();
  }
  theN = startPoint->size;
  theF = f;
  theDF = df;
  
  frprmn(startPoint->Y-1,startPoint->size,tolerance,nIter,minVal,theFunc,theDFunc,flagPrint);
  
  DeleteSignal(theSig);
  DeleteSignal(theGrad);
  theSig = theGrad = NULL;
}