LCFIPlus  0.6.5
algo.h
Go to the documentation of this file.
1 #ifndef lcfiplus_algo_h
2 #define lcfiplus_algo_h 1
3 
4 #include <math.h>
5 #include <stdio.h>
6 #include <vector>
7 #include <iostream>
8 
9 using namespace std;
10 
11 class TrackPocaXY;
12 
13 namespace lcfiplus {
14 
15 const double R=0.61803399;
16 const double C=(1.0-R);
17 const double GOLD=1.618034;
18 const double GLIMIT=100.0;
19 const double TINY=1.e-25;
20 const double CGOLD=0.3819660;
21 //const double ZEPS = 1e-10;
22 const double ZEPS = 1e-20;
23 const int ITMAX=200;
24 
25 #define SHFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
26 #define SHFT2(a,b,c) (a)=(b);(b)=(c);
27 #define SHFT3(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
28 #define SIGN(a,b) ((b)>=0.0 ? fabs(a) : -fabs(a))
29 #define MOV3(a,b,c, d,e,f) (a)=(d);(b)=(e);(c)=(f);
30 
31 template<class T>
32 double golden(double ax, double bx, double cx, T* obj, double tol, double& xmin) {
33  double f1,f2,x0,x1,x2,x3;
34  x0=ax;
35  x3=cx;
36  if (fabs(cx-bx) > fabs(bx-ax)) {
37  x1=bx;
38  x2=bx+C*(cx-bx);
39  } else {
40  x2=bx;
41  x1=bx-C*(bx-ax);
42  }
43  f1=(*obj)(&x1);
44  f2=(*obj)(&x2);
45  while (fabs(x3-x0) > tol*(fabs(x1)+fabs(x2))) {
46  if (f2 < f1) {
47  SHFT3(x0,x1,x2,R*x1+C*x3);
48  SHFT2(f1,f2,(*obj)(&x2));
49  } else {
50  SHFT3(x3,x2,x1,R*x2+C*x0);
51  SHFT2(f2,f1,(*obj)(&x1));
52  }
53  }
54  if (f1 < f2) {
55  xmin=x1;
56  return f1;
57  } else {
58  xmin=x2;
59  return f2;
60  }
61 }
62 
63 
64 template<class T>
65 double brent(double ax, double bx, double cx, T* obj, double tol, double& xmin) {
66  int iter;
67  double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
68  double e=0.0;
69 
70  a=(ax < cx ? ax : cx);
71  b=(ax > cx ? ax : cx);
72  x=w=v=bx;
73  fw=fv=fx=(*obj)(&x);
74  for (iter=1; iter<=ITMAX; iter++) {
75  xm=0.5*(a+b);
76  tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
77  if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
78  xmin=x;
79  return fx;
80  }
81  if (fabs(e) > tol1) {
82  r=(x-w)*(fx-fv);
83  q=(x-v)*(fx-fw);
84  p=(x-v)*q-(x-w)*r;
85  q=2.0*(q-r);
86  if (q > 0.0) p = -p;
87  q=fabs(q);
88  etemp=e;
89  e=d;
90  if (fabs(p) >= fabs(0.5*q*etemp) || p<= q*(a-x) || p >= q*(b-x))
91  d=CGOLD*(e=(x >= xm ? a-x : b-x));
92  else {
93  d=p/q;
94  u=x+d;
95  if (u-a < tol2 || b-u < tol2)
96  d=SIGN(tol1,xm-x);
97  }
98  } else {
99  d=CGOLD*(e=(x >= xm ? a-x : b-x));
100  }
101  u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
102  fu=(*obj)(&u);
103  if (fu <= fx) {
104  if (u>= x) a=x;
105  else b=x;
106  SHFT(v,w,x,u);
107  SHFT(fv,fw,fx,fu);
108  } else {
109  if (u < x) a=u;
110  else b=u;
111  if (fu <= fw || w == x) {
112  v=w;
113  w=u;
114  fv=fw;
115  fw=fu;
116  } else if (fu <= fv || v == x || v == w) {
117  v=u;
118  fv=fu;
119  }
120  }
121  }
122  fprintf(stderr,"too many iterations in brent()\n");
123  xmin=x;
124  return fx;
125 }
126 
127 template<class T, class U>
128 double dbrent(double ax, double bx, double cx, T* obj, U* dobj, double tol, double& xmin) {
129  int iter,ok1,ok2;
130  double a,b,d,d1,d2,du,dv,dw,dx,e=0.0;
131  double fu,fv,fw,fx,olde,tol1,tol2,u,u1,u2,v,w,x,xm;
132 
133  a=(ax < cx ? ax : cx);
134  b=(ax > cx ? ax : cx);
135  x=w=v=bx;
136  fw=fv=fx=(*obj)(&x);
137  dw=dv=dx=(*dobj)(&x);
138  for (iter=1; iter<=ITMAX; iter++) {
139  xm=0.5*(a+b);
140  tol1=tol*fabs(x)+ZEPS;
141  tol2=2.0*tol1;
142  if (fabs(x-xm) <= (tol2-0.5*(b-a))) {
143  xmin=x;
144  return fx;
145  }
146  if (fabs(e) > tol1) {
147  d1=2.0*(b-a);
148  d2=d1;
149  if (dw != dx) d1=(w-x)*dx/(dx-dw);
150  if (dv != dx) d2=(v-x)*dx/(dx-dv);
151  u1=x+d1;
152  u2=x+d2;
153  ok1 = (a-u1)*(u1-b) > 0.0 && dx*d1 <= 0.0;
154  ok2 = (a-u2)*(u2-b) > 0.0 && dx*d2 <= 0.0;
155  olde=e;
156  e=d;
157  if (ok1 || ok2) {
158  if (ok1 && ok2)
159  d=(fabs(d1) < fabs(d2) ? d1 : d2);
160  else if (ok1)
161  d=d1;
162  else
163  d=d2;
164  if (fabs(d) <= fabs(0.5*olde)) {
165  u=x+d;
166  if (u-a < tol2 || b-u < tol2)
167  d=SIGN(tol1,xm-x);
168  } else {
169  d=0.5*(e=(dx >= 0.0 ? a-x : b-x));
170  }
171  } else {
172  d=0.5*(e=(dx >= 0.0 ? a-x : b-x ));
173  }
174  } else {
175  d=0.5*(e=(dx >= 0.0 ? a-x : b-x ));
176  }
177  if (fabs(d) >= tol1) {
178  u=x+d;
179  fu=(*obj)(&u);
180  } else {
181  u=x+SIGN(tol1,d);
182  fu=(*obj)(&u);
183  if (fu > fx) {
184  xmin = x;
185  return fx;
186  }
187  }
188  du = (*dobj)(&u);
189  if (fu <= fx) {
190  if (u >= x) a=x;
191  else b=x;
192  MOV3(v,fv,dv, w,fw,dw);
193  MOV3(w,fw,dw, x,fx,dx);
194  MOV3(x,fx,dx, u,fu,du);
195  } else {
196  if (u < x) a=u;
197  else b=u;
198  if (fu <= fw || w == x) {
199  MOV3(v,fv,dv, w,fw,dw);
200  MOV3(w,fw,dw, u,fu,du);
201  } else if (fu < fv || v == x || v == w) {
202  MOV3(v,fv,dv, u,fu,du);
203  }
204  }
205  }
206  fprintf(stderr,"too many iterations in dbrent\n");
207  return 0.0;
208 }
209 
210 template<class T>
211 void lnsrch(int n, double xold[], double fold, double g[], double p[], double x[],
212  double* f, double stpmax, int* check, T* obj) {
213  const double ALF=1.0e-4;
214  const double TOLX=1.0e-7;
215 
216  static double maxarg1,maxarg2;
217 #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
218 
219  int i;
220  double a,alam,alam2,alamin,b,disc,f2,fold2,rhs1,rhs2,slope,sum,temp,
221  test,tmplam;
222 
223  *check=0;
224  for (sum=0.0,i=0; i<n; i++) sum += p[i]*p[i];
225  sum=sqrt(sum);
226  if (sum > stpmax)
227  for (i=0; i<n; i++) p[i] *= stpmax/sum;
228  for (slope=0.0,i=0; i<n; i++)
229  slope += g[i]*p[i];
230  test=0.0;
231  for (i=0; i<n; i++) {
232  temp=fabs(p[i])/FMAX(fabs(xold[i]),1.0);
233  if (temp > test) test=temp;
234  }
235  alamin=TOLX/test;
236  alam=1.0;
237  for (;;) {
238  for (i=0; i<n; i++) x[i]=xold[i]+alam*p[i];
239  *f=(*obj)(x);
240  if (alam < alamin) {
241  for (i=0; i<n; i++) x[i]=xold[i];
242  *check=1;
243  return;
244  } else if (*f <= fold+ALF*alam*slope) return;
245  else {
246  if (alam == 1.0)
247  tmplam = -slope/(2.0*(*f-fold-slope));
248  else {
249  rhs1 = *f-fold-alam*slope;
250  rhs2=f2-fold2-alam2*slope;
251  a=(rhs1/(alam*alam)-rhs2/(alam2*alam2))/(alam-alam2);
252  b=(-alam2*rhs1/(alam*alam)+alam*rhs2/(alam2*alam2))/(alam-alam2);
253  if (a == 0.0) tmplam = -slope/(2.0*b);
254  else {
255  disc=b*b-3.0*a*slope;
256  if (disc<0.0) fprintf(stderr,"Roundoff problem in lnsrch.\n");
257  else tmplam=(-b+sqrt(disc))/(3.0*a);
258  }
259  if (tmplam>0.5*alam)
260  tmplam=0.5*alam;
261  }
262  }
263  alam2=alam;
264  f2 = *f;
265  fold2=fold;
266  alam=FMAX(tmplam,0.1*alam);
267  }
268 }
269 
270 template<class T, class U>
271 void dfpmin(double p[], int n, double gtol, int* iter, double* fret, T* obj, U* dobj) {
272  bool verbose = false;
273 
274  const double EPS=3.0e-8;
275  const double TOLX=4*EPS;
276  const double STPMAX=100.0;
277 
278  static double maxarg1,maxarg2;
279 #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
280 
281  int check,i,its,j;
282  double den,fac,fad,fae,fp,stpmax,sum=0.0,sumdg,sumxi,temp,test;
283  double* dg, *g, *hdg, **hessin, *pnew, *xi;
284 
285  dg = new double[n];
286  g = new double[n];
287  hdg = new double[n];
288  hessin = new double*[n];
289  for (i=0; i<n; ++i)
290  hessin[i] = new double[n];
291  pnew = new double[n];
292  xi = new double[n];
293 
294  fp=(*obj)(p);
295 
296  (*dobj)(p,g);
297 
298  for (i=0; i<n; i++) {
299  for (j=0; j<n; j++) hessin[i][j]=0.0;
300  hessin[i][i]=1.0;
301  xi[i] = -g[i];
302  sum += p[i]*p[i];
303  }
304  stpmax=STPMAX*FMAX(sqrt(sum),(double)n);
305 
306  for (its=1; its<=ITMAX; its++) {
307  if (verbose) {
308  cout << "dfpmin: iteration " << its << ", p = ";
309  for (i=0; i<n; i++)cout << p[i] << " ";
310  cout << ", xi = ";
311  for (i=0; i<n; i++)cout << xi[i] << " ";
312  }
313 
314  *iter=its;
315  lnsrch(n,p,fp,g,xi,pnew,fret,stpmax,&check,obj);
316  fp = *fret;
317  for (i=0; i<n; i++) {
318  xi[i]=pnew[i]-p[i];
319  p[i]=pnew[i];
320  }
321  test=0.0;
322  for (i=0; i<n; i++) {
323  temp=fabs(xi[i])/FMAX(fabs(p[i]),1.0);
324  if (temp > test) test=temp;
325  }
326  if (verbose) cout << "testtol = " << test;
327  if (test < TOLX) {
328  if (verbose) cout << endl;
329 
330  delete[] dg;
331  delete[] g;
332  delete[] hdg;
333  for (i=0; i<n; ++i) {
334  delete[] hessin[i];
335  }
336  delete[] hessin;
337  delete[] pnew;
338  delete[] xi;
339  return;
340  }
341  for (i=0; i<n; i++) dg[i]=g[i];
342  (*dobj)(p,g);
343  test=0.0;
344  den=FMAX(*fret,1.0);
345  for (i=0; i<n; i++) {
346  temp=fabs(g[i])*FMAX(fabs(p[i]),1.0)/den;
347  if (temp > test) test=temp;
348  }
349 
350  if (verbose) cout << " testgtol = " << test << endl;
351 
352  if (test < gtol) {
353  delete[] dg;
354  delete[] g;
355  delete[] hdg;
356  for (i=0; i<n; ++i) {
357  delete[] hessin[i];
358  }
359  delete[] hessin;
360  delete[] pnew;
361  delete[] xi;
362  return;
363  }
364  for (i=0; i<n; i++) dg[i]=g[i]-dg[i];
365  for (i=0; i<n; i++) {
366  hdg[i]=0.0;
367  for (j=0; j<n; j++) hdg[i] += hessin[i][j]*dg[j];
368  }
369  fac=fae=sumdg=sumxi=0.0;
370  for (i=0; i<n; i++) {
371  fac += dg[i]*xi[i];
372  fae += dg[i]*hdg[i];
373  sumdg += (dg[i]*dg[i]);
374  sumxi += (xi[i]*xi[i]);
375  }
376  if (fac > sqrt(EPS*sumdg*sumxi)) {
377  fac=1.0/fac;
378  fad=1.0/fae;
379  for (i=0; i<n; i++) dg[i]=fac*xi[i]-fad*hdg[i];
380  for (i=0; i<n; i++) {
381  for (j=i; j<n; j++) {
382  hessin[i][j] += fac*xi[i]*xi[j]
383  -fad*hdg[i]*hdg[j]+fae*dg[i]*dg[j];
384  hessin[j][i]=hessin[i][j];
385  }
386  }
387  }
388  for (i=0; i<n; i++) {
389  xi[i]=0.0;
390  for (j=0; j<n; j++) xi[i] -= hessin[i][j]*g[j];
391  }
392  }
393  fprintf(stderr,"too many iterations in dfpmin");
394  delete[] dg;
395  delete[] g;
396  delete[] hdg;
397  for (i=0; i<n; ++i) {
398  delete[] hessin[i];
399  }
400  delete[] hessin;
401  delete[] pnew;
402  delete[] xi;
403 }
404 
405 }
406 
407 #endif
#define SHFT3(a, b, c, d)
Definition: algo.h:27
void dfpmin(double p[], int n, double gtol, int *iter, double *fret, T *obj, U *dobj)
Definition: algo.h:271
const double CGOLD
Definition: algo.h:20
#define SIGN(a, b)
Definition: algo.h:28
#define MOV3(a, b, c, d, e, f)
Definition: algo.h:29
const double GOLD
Definition: algo.h:17
const double R
Definition: algo.h:15
const double C
Definition: algo.h:16
const double ZEPS
Definition: algo.h:22
double dbrent(double ax, double bx, double cx, T *obj, U *dobj, double tol, double &xmin)
Definition: algo.h:128
double brent(double ax, double bx, double cx, T *obj, double tol, double &xmin)
Definition: algo.h:65
#define SHFT2(a, b, c)
Definition: algo.h:26
const int ITMAX
Definition: algo.h:23
void lnsrch(int n, double xold[], double fold, double g[], double p[], double x[], double *f, double stpmax, int *check, T *obj)
Definition: algo.h:211
double golden(double ax, double bx, double cx, T *obj, double tol, double &xmin)
Definition: algo.h:32
const double TINY
Definition: algo.h:19
#define FMAX(a, b)
const double GLIMIT
Definition: algo.h:18
#define SHFT(a, b, c, d)
Definition: algo.h:25