|
0
|
1 /*
|
|
|
2 * Copyright 1996-2006 Catherine Loader.
|
|
|
3 */
|
|
|
4
|
|
|
5 #include "mex.h"
|
|
|
6 /*
|
|
|
7 * Copyright 1996-2006 Catherine Loader.
|
|
|
8 */
|
|
|
9 #include <math.h>
|
|
|
10 #include "mut.h"
|
|
|
11
|
|
|
12 /* stirlerr(n) = log(n!) - log( sqrt(2*pi*n)*(n/e)^n ) */
|
|
|
13
|
|
|
14 #define S0 0.083333333333333333333 /* 1/12 */
|
|
|
15 #define S1 0.00277777777777777777778 /* 1/360 */
|
|
|
16 #define S2 0.00079365079365079365079365 /* 1/1260 */
|
|
|
17 #define S3 0.000595238095238095238095238 /* 1/1680 */
|
|
|
18 #define S4 0.0008417508417508417508417508 /* 1/1188 */
|
|
|
19
|
|
|
20 /*
|
|
|
21 error for 0, 0.5, 1.0, 1.5, ..., 14.5, 15.0.
|
|
|
22 */
|
|
|
23 static double sferr_halves[31] = {
|
|
|
24 0.0, /* n=0 - wrong, place holder only */
|
|
|
25 0.1534264097200273452913848, /* 0.5 */
|
|
|
26 0.0810614667953272582196702, /* 1.0 */
|
|
|
27 0.0548141210519176538961390, /* 1.5 */
|
|
|
28 0.0413406959554092940938221, /* 2.0 */
|
|
|
29 0.03316287351993628748511048, /* 2.5 */
|
|
|
30 0.02767792568499833914878929, /* 3.0 */
|
|
|
31 0.02374616365629749597132920, /* 3.5 */
|
|
|
32 0.02079067210376509311152277, /* 4.0 */
|
|
|
33 0.01848845053267318523077934, /* 4.5 */
|
|
|
34 0.01664469118982119216319487, /* 5.0 */
|
|
|
35 0.01513497322191737887351255, /* 5.5 */
|
|
|
36 0.01387612882307074799874573, /* 6.0 */
|
|
|
37 0.01281046524292022692424986, /* 6.5 */
|
|
|
38 0.01189670994589177009505572, /* 7.0 */
|
|
|
39 0.01110455975820691732662991, /* 7.5 */
|
|
|
40 0.010411265261972096497478567, /* 8.0 */
|
|
|
41 0.009799416126158803298389475, /* 8.5 */
|
|
|
42 0.009255462182712732917728637, /* 9.0 */
|
|
|
43 0.008768700134139385462952823, /* 9.5 */
|
|
|
44 0.008330563433362871256469318, /* 10.0 */
|
|
|
45 0.007934114564314020547248100, /* 10.5 */
|
|
|
46 0.007573675487951840794972024, /* 11.0 */
|
|
|
47 0.007244554301320383179543912, /* 11.5 */
|
|
|
48 0.006942840107209529865664152, /* 12.0 */
|
|
|
49 0.006665247032707682442354394, /* 12.5 */
|
|
|
50 0.006408994188004207068439631, /* 13.0 */
|
|
|
51 0.006171712263039457647532867, /* 13.5 */
|
|
|
52 0.005951370112758847735624416, /* 14.0 */
|
|
|
53 0.005746216513010115682023589, /* 14.5 */
|
|
|
54 0.005554733551962801371038690 /* 15.0 */
|
|
|
55 };
|
|
|
56
|
|
|
57 double stirlerr(n)
|
|
|
58 double n;
|
|
|
59 { double nn;
|
|
|
60
|
|
|
61 if (n<15.0)
|
|
|
62 { nn = 2.0*n;
|
|
|
63 if (nn==(int)nn) return(sferr_halves[(int)nn]);
|
|
|
64 return(mut_lgamma(n+1.0) - (n+0.5)*log((double)n)+n - HF_LG_PIx2);
|
|
|
65 }
|
|
|
66
|
|
|
67 nn = (double)n;
|
|
|
68 nn = nn*nn;
|
|
|
69 if (n>500) return((S0-S1/nn)/n);
|
|
|
70 if (n>80) return((S0-(S1-S2/nn)/nn)/n);
|
|
|
71 if (n>35) return((S0-(S1-(S2-S3/nn)/nn)/nn)/n);
|
|
|
72 return((S0-(S1-(S2-(S3-S4/nn)/nn)/nn)/nn)/n);
|
|
|
73 }
|
|
|
74
|
|
|
75 double bd0(x,np)
|
|
|
76 double x, np;
|
|
|
77 { double ej, s, s1, v;
|
|
|
78 int j;
|
|
|
79 if (fabs(x-np)<0.1*(x+np))
|
|
|
80 {
|
|
|
81 s = (x-np)*(x-np)/(x+np);
|
|
|
82 v = (x-np)/(x+np);
|
|
|
83 ej = 2*x*v; v = v*v;
|
|
|
84 for (j=1; ;++j)
|
|
|
85 { ej *= v;
|
|
|
86 s1 = s+ej/((j<<1)+1);
|
|
|
87 if (s1==s) return(s1);
|
|
|
88 s = s1;
|
|
|
89 }
|
|
|
90 }
|
|
|
91 return(x*log(x/np)+np-x);
|
|
|
92 }
|
|
|
93
|
|
|
94 /*
|
|
|
95 Raw binomial probability calculation.
|
|
|
96 (1) This has both p and q arguments, when one may be represented
|
|
|
97 more accurately than the other (in particular, in df()).
|
|
|
98 (2) This should NOT check that inputs x and n are integers. This
|
|
|
99 should be done in the calling function, where necessary.
|
|
|
100 (3) Does not check for 0<=p<=1 and 0<=q<=1 or NaN's. Do this in
|
|
|
101 the calling function.
|
|
|
102 */
|
|
|
103 double dbinom_raw(x,n,p,q,give_log)
|
|
|
104 double x, n, p, q;
|
|
|
105 int give_log;
|
|
|
106 { double f, lc;
|
|
|
107
|
|
|
108 if (p==0.0) return((x==0) ? D_1 : D_0);
|
|
|
109 if (q==0.0) return((x==n) ? D_1 : D_0);
|
|
|
110
|
|
|
111 if (x==0)
|
|
|
112 { lc = (p<0.1) ? -bd0(n,n*q) - n*p : n*log(q);
|
|
|
113 return( DEXP(lc) );
|
|
|
114 }
|
|
|
115
|
|
|
116 if (x==n)
|
|
|
117 { lc = (q<0.1) ? -bd0(n,n*p) - n*q : n*log(p);
|
|
|
118 return( DEXP(lc) );
|
|
|
119 }
|
|
|
120
|
|
|
121 if ((x<0) | (x>n)) return( D_0 );
|
|
|
122
|
|
|
123 lc = stirlerr(n) - stirlerr(x) - stirlerr(n-x)
|
|
|
124 - bd0(x,n*p) - bd0(n-x,n*q);
|
|
|
125 f = (PIx2*x*(n-x))/n;
|
|
|
126
|
|
|
127 return( FEXP(f,lc) );
|
|
|
128 }
|
|
|
129
|
|
|
130 double dbinom(x,n,p,give_log)
|
|
|
131 int x, n;
|
|
|
132 double p;
|
|
|
133 int give_log;
|
|
|
134 {
|
|
|
135 if ((p<0) | (p>1) | (n<0)) return(INVALID_PARAMS);
|
|
|
136 if (x<0) return( D_0 );
|
|
|
137
|
|
|
138 return( dbinom_raw((double)x,(double)n,p,1-p,give_log) );
|
|
|
139 }
|
|
|
140
|
|
|
141 /*
|
|
|
142 Poisson probability lb^x exp(-lb) / x!.
|
|
|
143 I don't check that x is an integer, since other functions
|
|
|
144 that call dpois_raw() (i.e. dgamma) may use a fractional
|
|
|
145 x argument.
|
|
|
146 */
|
|
|
147 double dpois_raw(x,lambda,give_log)
|
|
|
148 int give_log;
|
|
|
149 double x, lambda;
|
|
|
150 {
|
|
|
151 if (lambda==0) return( (x==0) ? D_1 : D_0 );
|
|
|
152 if (x==0) return( DEXP(-lambda) );
|
|
|
153 if (x<0) return( D_0 );
|
|
|
154
|
|
|
155 return(FEXP( PIx2*x, -stirlerr(x)-bd0(x,lambda) ));
|
|
|
156 }
|
|
|
157
|
|
|
158 double dpois(x,lambda,give_log)
|
|
|
159 int x, give_log;
|
|
|
160 double lambda;
|
|
|
161 {
|
|
|
162 if (lambda<0) return(INVALID_PARAMS);
|
|
|
163 if (x<0) return( D_0 );
|
|
|
164
|
|
|
165 return( dpois_raw((double)x,lambda,give_log) );
|
|
|
166 }
|
|
|
167
|
|
|
168 double dbeta(x,a,b,give_log)
|
|
|
169 double x, a, b;
|
|
|
170 int give_log;
|
|
|
171 { double f, p;
|
|
|
172
|
|
|
173 if ((a<=0) | (b<=0)) return(INVALID_PARAMS);
|
|
|
174 if ((x<=0) | (x>=1)) return(D_0);
|
|
|
175
|
|
|
176 if (a<1)
|
|
|
177 { if (b<1) /* a<1, b<1 */
|
|
|
178 { f = a*b/((a+b)*x*(1-x));
|
|
|
179 p = dbinom_raw(a,a+b,x,1-x,give_log);
|
|
|
180 }
|
|
|
181 else /* a<1, b>=1 */
|
|
|
182 { f = a/x;
|
|
|
183 p = dbinom_raw(a,a+b-1,x,1-x,give_log);
|
|
|
184 }
|
|
|
185 }
|
|
|
186 else
|
|
|
187 { if (b<1) /* a>=1, b<1 */
|
|
|
188 { f = b/(1-x);
|
|
|
189 p = dbinom_raw(a-1,a+b-1,x,1-x,give_log);
|
|
|
190 }
|
|
|
191 else /* a>=1, b>=1 */
|
|
|
192 { f = a+b-1;
|
|
|
193 p = dbinom_raw(a-1,(a-1)+(b-1),x,1-x,give_log);
|
|
|
194 }
|
|
|
195 }
|
|
|
196
|
|
|
197 return( (give_log) ? p + log(f) : p*f );
|
|
|
198 }
|
|
|
199
|
|
|
200 /*
|
|
|
201 * To evaluate the F density, write it as a Binomial probability
|
|
|
202 * with p = x*m/(n+x*m). For m>=2, use the simplest conversion.
|
|
|
203 * For m<2, (m-2)/2<0 so the conversion will not work, and we must use
|
|
|
204 * a second conversion. Note the division by p; this seems unavoidable
|
|
|
205 * for m < 2, since the F density has a singularity as x (or p) -> 0.
|
|
|
206 */
|
|
|
207 double df(x,m,n,give_log)
|
|
|
208 double x, m, n;
|
|
|
209 int give_log;
|
|
|
210 { double p, q, f, dens;
|
|
|
211
|
|
|
212 if ((m<=0) | (n<=0)) return(INVALID_PARAMS);
|
|
|
213 if (x <= 0.0) return(D_0);
|
|
|
214
|
|
|
215 f = 1.0/(n+x*m);
|
|
|
216 q = n*f;
|
|
|
217 p = x*m*f;
|
|
|
218
|
|
|
219 if (m>=2)
|
|
|
220 { f = m*q/2;
|
|
|
221 dens = dbinom_raw((m-2)/2.0, (m+n-2)/2.0, p, q, give_log);
|
|
|
222 }
|
|
|
223 else
|
|
|
224 { f = m*m*q / (2*p*(m+n));
|
|
|
225 dens = dbinom_raw(m/2.0, (m+n)/2.0, p, q, give_log);
|
|
|
226 }
|
|
|
227
|
|
|
228 return((give_log) ? log(f)+dens : f*dens);
|
|
|
229 }
|
|
|
230
|
|
|
231 /*
|
|
|
232 * Gamma density,
|
|
|
233 * lb^r x^{r-1} exp(-lb*x)
|
|
|
234 * p(x;r,lb) = -----------------------
|
|
|
235 * (r-1)!
|
|
|
236 *
|
|
|
237 * If USE_SCALE is defined below, the lb argument will be interpreted
|
|
|
238 * as a scale parameter (i.e. replace lb by 1/lb above). Otherwise,
|
|
|
239 * it is interpreted as a rate parameter, as above.
|
|
|
240 */
|
|
|
241
|
|
|
242 /* #define USE_SCALE */
|
|
|
243
|
|
|
244 double dgamma(x,r,lambda,give_log)
|
|
|
245 int give_log;
|
|
|
246 double x, r, lambda;
|
|
|
247 { double pr;
|
|
|
248
|
|
|
249 if ((r<=0) | (lambda<0)) return(INVALID_PARAMS);
|
|
|
250 if (x<=0.0) return( D_0 );
|
|
|
251
|
|
|
252 #ifdef USE_SCALE
|
|
|
253 lambda = 1.0/lambda;
|
|
|
254 #endif
|
|
|
255
|
|
|
256 if (r<1)
|
|
|
257 { pr = dpois_raw(r,lambda*x,give_log);
|
|
|
258 return( (give_log) ? pr + log(r/x) : pr*r/x );
|
|
|
259 }
|
|
|
260
|
|
|
261 pr = dpois_raw(r-1.0,lambda*x,give_log);
|
|
|
262 return( (give_log) ? pr + log(lambda) : lambda*pr);
|
|
|
263 }
|
|
|
264
|
|
|
265 double dchisq(x, df, give_log)
|
|
|
266 double x, df;
|
|
|
267 int give_log;
|
|
|
268 {
|
|
|
269 return(dgamma(x, df/2.0,
|
|
|
270 0.5
|
|
|
271 ,give_log));
|
|
|
272 /*
|
|
|
273 #ifdef USE_SCALE
|
|
|
274 2.0
|
|
|
275 #else
|
|
|
276 0.5
|
|
|
277 #endif
|
|
|
278 ,give_log));
|
|
|
279 */
|
|
|
280 }
|
|
|
281
|
|
|
282 /*
|
|
|
283 * Given a sequence of r successes and b failures, we sample n (\le b+r)
|
|
|
284 * items without replacement. The hypergeometric probability is the
|
|
|
285 * probability of x successes:
|
|
|
286 *
|
|
|
287 * dbinom(x,r,p) * dbinom(n-x,b,p)
|
|
|
288 * p(x;r,b,n) = ---------------------------------
|
|
|
289 * dbinom(n,r+b,p)
|
|
|
290 *
|
|
|
291 * for any p. For numerical stability, we take p=n/(r+b); with this choice,
|
|
|
292 * the denominator is not exponentially small.
|
|
|
293 */
|
|
|
294 double dhyper(x,r,b,n,give_log)
|
|
|
295 int x, r, b, n, give_log;
|
|
|
296 { double p, q, p1, p2, p3;
|
|
|
297
|
|
|
298 if ((r<0) | (b<0) | (n<0) | (n>r+b))
|
|
|
299 return( INVALID_PARAMS );
|
|
|
300
|
|
|
301 if (x<0) return(D_0);
|
|
|
302
|
|
|
303 if (n==0) return((x==0) ? D_1 : D_0);
|
|
|
304
|
|
|
305 p = ((double)n)/((double)(r+b));
|
|
|
306 q = ((double)(r+b-n))/((double)(r+b));
|
|
|
307
|
|
|
308 p1 = dbinom_raw((double)x,(double)r,p,q,give_log);
|
|
|
309 p2 = dbinom_raw((double)(n-x),(double)b,p,q,give_log);
|
|
|
310 p3 = dbinom_raw((double)n,(double)(r+b),p,q,give_log);
|
|
|
311
|
|
|
312 return( (give_log) ? p1 + p2 - p3 : p1*p2/p3 );
|
|
|
313 }
|
|
|
314
|
|
|
315 /*
|
|
|
316 probability of x failures before the nth success.
|
|
|
317 */
|
|
|
318 double dnbinom(x,n,p,give_log)
|
|
|
319 double n, p;
|
|
|
320 int x, give_log;
|
|
|
321 { double prob, f;
|
|
|
322
|
|
|
323 if ((p<0) | (p>1) | (n<=0)) return(INVALID_PARAMS);
|
|
|
324
|
|
|
325 if (x<0) return( D_0 );
|
|
|
326
|
|
|
327 prob = dbinom_raw(n,x+n,p,1-p,give_log);
|
|
|
328 f = n/(n+x);
|
|
|
329
|
|
|
330 return((give_log) ? log(f) + prob : f*prob);
|
|
|
331 }
|
|
|
332
|
|
|
333 double dt(x, df, give_log)
|
|
|
334 double x, df;
|
|
|
335 int give_log;
|
|
|
336 { double t, u, f;
|
|
|
337
|
|
|
338 if (df<=0.0) return(INVALID_PARAMS);
|
|
|
339
|
|
|
340 /*
|
|
|
341 exp(t) = Gamma((df+1)/2) /{ sqrt(df/2) * Gamma(df/2) }
|
|
|
342 = sqrt(df/2) / ((df+1)/2) * Gamma((df+3)/2) / Gamma((df+2)/2).
|
|
|
343 This form leads to a computation that should be stable for all
|
|
|
344 values of df, including df -> 0 and df -> infinity.
|
|
|
345 */
|
|
|
346 t = -bd0(df/2.0,(df+1)/2.0) + stirlerr((df+1)/2.0) - stirlerr(df/2.0);
|
|
|
347
|
|
|
348 if (x*x>df)
|
|
|
349 u = log( 1+ x*x/df ) * df/2;
|
|
|
350 else
|
|
|
351 u = -bd0(df/2.0,(df+x*x)/2.0) + x*x/2.0;
|
|
|
352
|
|
|
353 f = PIx2*(1+x*x/df);
|
|
|
354
|
|
|
355 return( FEXP(f,t-u) );
|
|
|
356 }
|
|
|
357 /*
|
|
|
358 * Copyright 1996-2006 Catherine Loader.
|
|
|
359 */
|
|
|
360 /*
|
|
|
361 * Provides mut_erf() and mut_erfc() functions. Also mut_pnorm().
|
|
|
362 * Had too many problems with erf()'s built into math libraries
|
|
|
363 * (when they existed). Hence the need to write my own...
|
|
|
364 *
|
|
|
365 * Algorithm from Walter Kr\"{a}mer, Frithjof Blomquist.
|
|
|
366 * "Algorithms with Guaranteed Error Bounds for the Error Function
|
|
|
367 * and Complementary Error Function"
|
|
|
368 * Preprint 2000/2, Bergische Universt\"{a}t GH Wuppertal
|
|
|
369 * http://www.math.uni-wuppertal.de/wrswt/preprints/prep_00_2.pdf
|
|
|
370 *
|
|
|
371 * Coded by Catherine Loader, September 2006.
|
|
|
372 */
|
|
|
373
|
|
|
374 #include "mut.h"
|
|
|
375
|
|
|
376 double erf1(double x) /* erf; 0 < x < 0.65) */
|
|
|
377 { double p[5] = {1.12837916709551256e0, /* 2/sqrt(pi) */
|
|
|
378 1.35894887627277916e-1,
|
|
|
379 4.03259488531795274e-2,
|
|
|
380 1.20339380863079457e-3,
|
|
|
381 6.49254556481904354e-5};
|
|
|
382 double q[5] = {1.00000000000000000e0,
|
|
|
383 4.53767041780002545e-1,
|
|
|
384 8.69936222615385890e-2,
|
|
|
385 8.49717371168693357e-3,
|
|
|
386 3.64915280629351082e-4};
|
|
|
387 double x2, p4, q4;
|
|
|
388 x2 = x*x;
|
|
|
389 p4 = p[0] + p[1]*x2 + p[2]*x2*x2 + p[3]*x2*x2*x2 + p[4]*x2*x2*x2*x2;
|
|
|
390 q4 = q[0] + q[1]*x2 + q[2]*x2*x2 + q[3]*x2*x2*x2 + q[4]*x2*x2*x2*x2;
|
|
|
391 return(x*p4/q4);
|
|
|
392 }
|
|
|
393
|
|
|
394 double erf2(double x) /* erfc; 0.65 <= x < 2.2 */
|
|
|
395 { double p[6] = {9.99999992049799098e-1,
|
|
|
396 1.33154163936765307e0,
|
|
|
397 8.78115804155881782e-1,
|
|
|
398 3.31899559578213215e-1,
|
|
|
399 7.14193832506776067e-2,
|
|
|
400 7.06940843763253131e-3};
|
|
|
401 double q[7] = {1.00000000000000000e0,
|
|
|
402 2.45992070144245533e0,
|
|
|
403 2.65383972869775752e0,
|
|
|
404 1.61876655543871376e0,
|
|
|
405 5.94651311286481502e-1,
|
|
|
406 1.26579413030177940e-1,
|
|
|
407 1.25304936549413393e-2};
|
|
|
408 double x2, p5, q6;
|
|
|
409 x2 = x*x;
|
|
|
410 p5 = p[0] + p[1]*x + p[2]*x2 + p[3]*x2*x + p[4]*x2*x2 + p[5]*x2*x2*x;
|
|
|
411 q6 = q[0] + q[1]*x + q[2]*x2 + q[3]*x2*x + q[4]*x2*x2 + q[5]*x2*x2*x + q[6]*x2*x2*x2;
|
|
|
412 return( exp(-x2)*p5/q6 );
|
|
|
413 }
|
|
|
414
|
|
|
415 double erf3(double x) /* erfc; 2.2 < x <= 6 */
|
|
|
416 { double p[6] = {9.99921140009714409e-1,
|
|
|
417 1.62356584489366647e0,
|
|
|
418 1.26739901455873222e0,
|
|
|
419 5.81528574177741135e-1,
|
|
|
420 1.57289620742838702e-1,
|
|
|
421 2.25716982919217555e-2};
|
|
|
422 double q[7] = {1.00000000000000000e0,
|
|
|
423 2.75143870676376208e0,
|
|
|
424 3.37367334657284535e0,
|
|
|
425 2.38574194785344389e0,
|
|
|
426 1.05074004614827206e0,
|
|
|
427 2.78788439273628983e-1,
|
|
|
428 4.00072964526861362e-2};
|
|
|
429 double x2, p5, q6;
|
|
|
430 x2 = x*x;
|
|
|
431 p5 = p[0] + p[1]*x + p[2]*x2 + p[3]*x2*x + p[4]*x2*x2 + p[5]*x2*x2*x;
|
|
|
432 q6 = q[0] + q[1]*x + q[2]*x2 + q[3]*x2*x + q[4]*x2*x2 + q[5]*x2*x2*x + q[6]*x2*x2*x2;
|
|
|
433 return( exp(-x2)*p5/q6 );
|
|
|
434 }
|
|
|
435
|
|
|
436 double erf4(double x) /* erfc; x > 6.0 */
|
|
|
437 { double p[5] = {5.64189583547756078e-1,
|
|
|
438 8.80253746105525775e0,
|
|
|
439 3.84683103716117320e1,
|
|
|
440 4.77209965874436377e1,
|
|
|
441 8.08040729052301677e0};
|
|
|
442 double q[5] = {1.00000000000000000e0,
|
|
|
443 1.61020914205869003e1,
|
|
|
444 7.54843505665954743e1,
|
|
|
445 1.12123870801026015e2,
|
|
|
446 3.73997570145040850e1};
|
|
|
447 double x2, p4, q4;
|
|
|
448 if (x>26.5432) return(0.0);
|
|
|
449 x2 = 1.0/(x*x);
|
|
|
450 p4 = p[0] + p[1]*x2 + p[2]*x2*x2 + p[3]*x2*x2*x2 + p[4]*x2*x2*x2*x2;
|
|
|
451 q4 = q[0] + q[1]*x2 + q[2]*x2*x2 + q[3]*x2*x2*x2 + q[4]*x2*x2*x2*x2;
|
|
|
452 return(exp(-x*x)*p4/(x*q4));
|
|
|
453 }
|
|
|
454
|
|
|
455 double mut_erfc(double x)
|
|
|
456 { if (x<0.0) return(2.0-mut_erfc(-x));
|
|
|
457 if (x==0.0) return(1.0);
|
|
|
458 if (x<0.65) return(1.0-erf1(x));
|
|
|
459 if (x<2.2) return(erf2(x));
|
|
|
460 if (x<6.0) return(erf3(x));
|
|
|
461 return(erf4(x));
|
|
|
462 }
|
|
|
463
|
|
|
464 double mut_erf(double x)
|
|
|
465 {
|
|
|
466 if (x<0.0) return(-mut_erf(-x));
|
|
|
467 if (x==0.0) return(0.0);
|
|
|
468 if (x<0.65) return(erf1(x));
|
|
|
469 if (x<2.2) return(1.0-erf2(x));
|
|
|
470 if (x<6.0) return(1.0-erf3(x));
|
|
|
471 return(1.0-erf4(x));
|
|
|
472 }
|
|
|
473
|
|
|
474 double mut_pnorm(double x)
|
|
|
475 { if (x<0.0) return(mut_erfc(-x/SQRT2)/2);
|
|
|
476 return((1.0+mut_erf(x/SQRT2))/2);
|
|
|
477 }
|
|
|
478 /*
|
|
|
479 * Copyright 1996-2006 Catherine Loader.
|
|
|
480 */
|
|
|
481 #include "mut.h"
|
|
|
482
|
|
|
483 static double lookup_gamma[21] = {
|
|
|
484 0.0, /* place filler */
|
|
|
485 0.572364942924699971, /* log(G(0.5)) = log(sqrt(pi)) */
|
|
|
486 0.000000000000000000, /* log(G(1)) = log(0!) */
|
|
|
487 -0.120782237635245301, /* log(G(3/2)) = log(sqrt(pi)/2)) */
|
|
|
488 0.000000000000000000, /* log(G(2)) = log(1!) */
|
|
|
489 0.284682870472919181, /* log(G(5/2)) = log(3sqrt(pi)/4) */
|
|
|
490 0.693147180559945286, /* log(G(3)) = log(2!) */
|
|
|
491 1.200973602347074287, /* etc */
|
|
|
492 1.791759469228054957,
|
|
|
493 2.453736570842442344,
|
|
|
494 3.178053830347945752,
|
|
|
495 3.957813967618716511,
|
|
|
496 4.787491742782045812,
|
|
|
497 5.662562059857141783,
|
|
|
498 6.579251212010101213,
|
|
|
499 7.534364236758732680,
|
|
|
500 8.525161361065414667,
|
|
|
501 9.549267257300996903,
|
|
|
502 10.604602902745250859,
|
|
|
503 11.689333420797268559,
|
|
|
504 12.801827480081469091 };
|
|
|
505
|
|
|
506 /*
|
|
|
507 * coefs are B(2n)/(2n(2n-1)) 2n(2n-1) =
|
|
|
508 * 2n B(2n) 2n(2n-1) coef
|
|
|
509 * 2 1/6 2 1/12
|
|
|
510 * 4 -1/30 12 -1/360
|
|
|
511 * 6 1/42 30 1/1260
|
|
|
512 * 8 -1/30 56 -1/1680
|
|
|
513 * 10 5/66 90 1/1188
|
|
|
514 * 12 -691/2730 132 691/360360
|
|
|
515 */
|
|
|
516
|
|
|
517 double mut_lgamma(double x)
|
|
|
518 { double f, z, x2, se;
|
|
|
519 int ix;
|
|
|
520
|
|
|
521 /* lookup table for common values.
|
|
|
522 */
|
|
|
523 ix = (int)(2*x);
|
|
|
524 if (((ix>=1) & (ix<=20)) && (ix==2*x)) return(lookup_gamma[ix]);
|
|
|
525
|
|
|
526 f = 1.0;
|
|
|
527 while (x <= 15)
|
|
|
528 { f *= x;
|
|
|
529 x += 1.0;
|
|
|
530 }
|
|
|
531
|
|
|
532 x2 = 1.0/(x*x);
|
|
|
533 z = (x-0.5)*log(x) - x + HF_LG_PIx2;
|
|
|
534 se = (13860 - x2*(462 - x2*(132 - x2*(99 - 140*x2))))/(166320*x);
|
|
|
535
|
|
|
536 return(z + se - log(f));
|
|
|
537 }
|
|
|
538
|
|
|
539 double mut_lgammai(int i) /* log(Gamma(i/2)) for integer i */
|
|
|
540 { if (i>20) return(mut_lgamma(i/2.0));
|
|
|
541 return(lookup_gamma[i]);
|
|
|
542 }
|
|
|
543 /*
|
|
|
544 * Copyright 1996-2006 Catherine Loader.
|
|
|
545 */
|
|
|
546 /*
|
|
|
547 * A is a n*p matrix, find the cholesky decomposition
|
|
|
548 * of the first p rows. In most applications, will want n=p.
|
|
|
549 *
|
|
|
550 * chol_dec(A,n,p) computes the decomoposition R'R=A.
|
|
|
551 * (note that R is stored in the input A).
|
|
|
552 * chol_solve(A,v,n,p) computes (R'R)^{-1}v
|
|
|
553 * chol_hsolve(A,v,n,p) computes (R')^{-1}v
|
|
|
554 * chol_isolve(A,v,n,p) computes (R)^{-1}v
|
|
|
555 * chol_qf(A,v,n,p) computes ||(R')^{-1}v||^2.
|
|
|
556 * chol_mult(A,v,n,p) computes (R'R)v
|
|
|
557 *
|
|
|
558 * The solve functions assume that A is already decomposed.
|
|
|
559 * chol_solve(A,v,n,p) is equivalent to applying chol_hsolve()
|
|
|
560 * and chol_isolve() in sequence.
|
|
|
561 */
|
|
|
562
|
|
|
563 #include <math.h>
|
|
|
564 #include "mut.h"
|
|
|
565
|
|
|
566 void chol_dec(A,n,p)
|
|
|
567 double *A;
|
|
|
568 int n, p;
|
|
|
569 { int i, j, k;
|
|
|
570
|
|
|
571 for (j=0; j<p; j++)
|
|
|
572 { k = n*j+j;
|
|
|
573 for (i=0; i<j; i++) A[k] -= A[n*j+i]*A[n*j+i];
|
|
|
574 if (A[k]<=0)
|
|
|
575 { for (i=j; i<p; i++) A[n*i+j] = 0.0; }
|
|
|
576 else
|
|
|
577 { A[k] = sqrt(A[k]);
|
|
|
578 for (i=j+1; i<p; i++)
|
|
|
579 { for (k=0; k<j; k++)
|
|
|
580 A[n*i+j] -= A[n*i+k]*A[n*j+k];
|
|
|
581 A[n*i+j] /= A[n*j+j];
|
|
|
582 }
|
|
|
583 }
|
|
|
584 }
|
|
|
585 for (j=0; j<p; j++)
|
|
|
586 for (i=j+1; i<p; i++) A[n*j+i] = 0.0;
|
|
|
587 }
|
|
|
588
|
|
|
589 int chol_solve(A,v,n,p)
|
|
|
590 double *A, *v;
|
|
|
591 int n, p;
|
|
|
592 { int i, j;
|
|
|
593
|
|
|
594 for (i=0; i<p; i++)
|
|
|
595 { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j];
|
|
|
596 v[i] /= A[i*n+i];
|
|
|
597 }
|
|
|
598 for (i=p-1; i>=0; i--)
|
|
|
599 { for (j=i+1; j<p; j++) v[i] -= A[j*n+i]*v[j];
|
|
|
600 v[i] /= A[i*n+i];
|
|
|
601 }
|
|
|
602 return(p);
|
|
|
603 }
|
|
|
604
|
|
|
605 int chol_hsolve(A,v,n,p)
|
|
|
606 double *A, *v;
|
|
|
607 int n, p;
|
|
|
608 { int i, j;
|
|
|
609
|
|
|
610 for (i=0; i<p; i++)
|
|
|
611 { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j];
|
|
|
612 v[i] /= A[i*n+i];
|
|
|
613 }
|
|
|
614 return(p);
|
|
|
615 }
|
|
|
616
|
|
|
617 int chol_isolve(A,v,n,p)
|
|
|
618 double *A, *v;
|
|
|
619 int n, p;
|
|
|
620 { int i, j;
|
|
|
621
|
|
|
622 for (i=p-1; i>=0; i--)
|
|
|
623 { for (j=i+1; j<p; j++) v[i] -= A[j*n+i]*v[j];
|
|
|
624 v[i] /= A[i*n+i];
|
|
|
625 }
|
|
|
626 return(p);
|
|
|
627 }
|
|
|
628
|
|
|
629 double chol_qf(A,v,n,p)
|
|
|
630 double *A, *v;
|
|
|
631 int n, p;
|
|
|
632 { int i, j;
|
|
|
633 double sum;
|
|
|
634
|
|
|
635 sum = 0.0;
|
|
|
636 for (i=0; i<p; i++)
|
|
|
637 { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j];
|
|
|
638 v[i] /= A[i*n+i];
|
|
|
639 sum += v[i]*v[i];
|
|
|
640 }
|
|
|
641 return(sum);
|
|
|
642 }
|
|
|
643
|
|
|
644 int chol_mult(A,v,n,p)
|
|
|
645 double *A, *v;
|
|
|
646 int n, p;
|
|
|
647 { int i, j;
|
|
|
648 double sum;
|
|
|
649 for (i=0; i<p; i++)
|
|
|
650 { sum = 0;
|
|
|
651 for (j=i; j<p; j++) sum += A[j*n+i]*v[j];
|
|
|
652 v[i] = sum;
|
|
|
653 }
|
|
|
654 for (i=p-1; i>=0; i--)
|
|
|
655 { sum = 0;
|
|
|
656 for (j=0; j<=i; j++) sum += A[i*n+j]*v[j];
|
|
|
657 v[i] = sum;
|
|
|
658 }
|
|
|
659 return(1);
|
|
|
660 }
|
|
|
661 /*
|
|
|
662 * Copyright 1996-2006 Catherine Loader.
|
|
|
663 */
|
|
|
664 #include <stdio.h>
|
|
|
665 #include <math.h>
|
|
|
666 #include "mut.h"
|
|
|
667 #define E_MAXIT 20
|
|
|
668 #define E_TOL 1.0e-10
|
|
|
669 #define SQR(x) ((x)*(x))
|
|
|
670
|
|
|
671 double e_tol(D,p)
|
|
|
672 double *D;
|
|
|
673 int p;
|
|
|
674 { double mx;
|
|
|
675 int i;
|
|
|
676 if (E_TOL <= 0.0) return(0.0);
|
|
|
677 mx = D[0];
|
|
|
678 for (i=1; i<p; i++) if (D[i*(p+1)]>mx) mx = D[i*(p+1)];
|
|
|
679 return(E_TOL*mx);
|
|
|
680 }
|
|
|
681
|
|
|
682 void eig_dec(X,P,d)
|
|
|
683 double *X, *P;
|
|
|
684 int d;
|
|
|
685 { int i, j, k, iter, ms;
|
|
|
686 double c, s, r, u, v;
|
|
|
687
|
|
|
688 for (i=0; i<d; i++)
|
|
|
689 for (j=0; j<d; j++) P[i*d+j] = (i==j);
|
|
|
690
|
|
|
691 for (iter=0; iter<E_MAXIT; iter++)
|
|
|
692 { ms = 0;
|
|
|
693 for (i=0; i<d; i++)
|
|
|
694 for (j=i+1; j<d; j++)
|
|
|
695 if (SQR(X[i*d+j]) > 1.0e-15*fabs(X[i*d+i]*X[j*d+j]))
|
|
|
696 { c = (X[j*d+j]-X[i*d+i])/2;
|
|
|
697 s = -X[i*d+j];
|
|
|
698 r = sqrt(c*c+s*s);
|
|
|
699 c /= r;
|
|
|
700 s = sqrt((1-c)/2)*(2*(s>0)-1);
|
|
|
701 c = sqrt((1+c)/2);
|
|
|
702 for (k=0; k<d; k++)
|
|
|
703 { u = X[i*d+k]; v = X[j*d+k];
|
|
|
704 X[i*d+k] = u*c+v*s;
|
|
|
705 X[j*d+k] = v*c-u*s;
|
|
|
706 }
|
|
|
707 for (k=0; k<d; k++)
|
|
|
708 { u = X[k*d+i]; v = X[k*d+j];
|
|
|
709 X[k*d+i] = u*c+v*s;
|
|
|
710 X[k*d+j] = v*c-u*s;
|
|
|
711 }
|
|
|
712 X[i*d+j] = X[j*d+i] = 0.0;
|
|
|
713 for (k=0; k<d; k++)
|
|
|
714 { u = P[k*d+i]; v = P[k*d+j];
|
|
|
715 P[k*d+i] = u*c+v*s;
|
|
|
716 P[k*d+j] = v*c-u*s;
|
|
|
717 }
|
|
|
718 ms = 1;
|
|
|
719 }
|
|
|
720 if (ms==0) return;
|
|
|
721 }
|
|
|
722 mut_printf("eig_dec not converged\n");
|
|
|
723 }
|
|
|
724
|
|
|
725 int eig_solve(J,x)
|
|
|
726 jacobian *J;
|
|
|
727 double *x;
|
|
|
728 { int d, i, j, rank;
|
|
|
729 double *D, *P, *Q, *w;
|
|
|
730 double tol;
|
|
|
731
|
|
|
732 D = J->Z;
|
|
|
733 P = Q = J->Q;
|
|
|
734 d = J->p;
|
|
|
735 w = J->wk;
|
|
|
736
|
|
|
737 tol = e_tol(D,d);
|
|
|
738
|
|
|
739 rank = 0;
|
|
|
740 for (i=0; i<d; i++)
|
|
|
741 { w[i] = 0.0;
|
|
|
742 for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j];
|
|
|
743 }
|
|
|
744 for (i=0; i<d; i++)
|
|
|
745 if (D[i*d+i]>tol)
|
|
|
746 { w[i] /= D[i*(d+1)];
|
|
|
747 rank++;
|
|
|
748 }
|
|
|
749 for (i=0; i<d; i++)
|
|
|
750 { x[i] = 0.0;
|
|
|
751 for (j=0; j<d; j++) x[i] += Q[i*d+j]*w[j];
|
|
|
752 }
|
|
|
753 return(rank);
|
|
|
754 }
|
|
|
755
|
|
|
756 int eig_hsolve(J,v)
|
|
|
757 jacobian *J;
|
|
|
758 double *v;
|
|
|
759 { int i, j, p, rank;
|
|
|
760 double *D, *Q, *w;
|
|
|
761 double tol;
|
|
|
762
|
|
|
763 D = J->Z;
|
|
|
764 Q = J->Q;
|
|
|
765 p = J->p;
|
|
|
766 w = J->wk;
|
|
|
767
|
|
|
768 tol = e_tol(D,p);
|
|
|
769 rank = 0;
|
|
|
770
|
|
|
771 for (i=0; i<p; i++)
|
|
|
772 { w[i] = 0.0;
|
|
|
773 for (j=0; j<p; j++) w[i] += Q[j*p+i]*v[j];
|
|
|
774 }
|
|
|
775 for (i=0; i<p; i++)
|
|
|
776 { if (D[i*p+i]>tol)
|
|
|
777 { v[i] = w[i]/sqrt(D[i*(p+1)]);
|
|
|
778 rank++;
|
|
|
779 }
|
|
|
780 else v[i] = 0.0;
|
|
|
781 }
|
|
|
782 return(rank);
|
|
|
783 }
|
|
|
784
|
|
|
785 int eig_isolve(J,v)
|
|
|
786 jacobian *J;
|
|
|
787 double *v;
|
|
|
788 { int i, j, p, rank;
|
|
|
789 double *D, *Q, *w;
|
|
|
790 double tol;
|
|
|
791
|
|
|
792 D = J->Z;
|
|
|
793 Q = J->Q;
|
|
|
794 p = J->p;
|
|
|
795 w = J->wk;
|
|
|
796
|
|
|
797 tol = e_tol(D,p);
|
|
|
798 rank = 0;
|
|
|
799
|
|
|
800 for (i=0; i<p; i++)
|
|
|
801 { if (D[i*p+i]>tol)
|
|
|
802 { v[i] = w[i]/sqrt(D[i*(p+1)]);
|
|
|
803 rank++;
|
|
|
804 }
|
|
|
805 else v[i] = 0.0;
|
|
|
806 }
|
|
|
807
|
|
|
808 for (i=0; i<p; i++)
|
|
|
809 { w[i] = 0.0;
|
|
|
810 for (j=0; j<p; j++) w[i] += Q[i*p+j]*v[j];
|
|
|
811 }
|
|
|
812
|
|
|
813 return(rank);
|
|
|
814 }
|
|
|
815
|
|
|
816 double eig_qf(J,v)
|
|
|
817 jacobian *J;
|
|
|
818 double *v;
|
|
|
819 { int i, j, p;
|
|
|
820 double sum, tol;
|
|
|
821
|
|
|
822 p = J->p;
|
|
|
823 sum = 0.0;
|
|
|
824 tol = e_tol(J->Z,p);
|
|
|
825
|
|
|
826 for (i=0; i<p; i++)
|
|
|
827 if (J->Z[i*p+i]>tol)
|
|
|
828 { J->wk[i] = 0.0;
|
|
|
829 for (j=0; j<p; j++) J->wk[i] += J->Q[j*p+i]*v[j];
|
|
|
830 sum += J->wk[i]*J->wk[i]/J->Z[i*p+i];
|
|
|
831 }
|
|
|
832 return(sum);
|
|
|
833 }
|
|
|
834 /*
|
|
|
835 * Copyright 1996-2006 Catherine Loader.
|
|
|
836 */
|
|
|
837 /*
|
|
|
838 * Integrate a function f over a circle or disc.
|
|
|
839 */
|
|
|
840
|
|
|
841 #include "mut.h"
|
|
|
842
|
|
|
843 void setM(M,r,s,c,b)
|
|
|
844 double *M, r, s, c;
|
|
|
845 int b;
|
|
|
846 { M[0] =-r*s; M[1] = r*c;
|
|
|
847 M[2] = b*c; M[3] = b*s;
|
|
|
848 M[4] =-r*c; M[5] = -s;
|
|
|
849 M[6] = -s; M[7] = 0.0;
|
|
|
850 M[8] =-r*s; M[9] = c;
|
|
|
851 M[10]= c; M[11]= 0.0;
|
|
|
852 }
|
|
|
853
|
|
|
854 void integ_circ(f,r,orig,res,mint,b)
|
|
|
855 int (*f)(), mint, b;
|
|
|
856 double r, *orig, *res;
|
|
|
857 { double y, x[2], theta, tres[MXRESULT], M[12], c, s;
|
|
|
858 int i, j, nr;
|
|
|
859
|
|
|
860 y = 0;
|
|
|
861 for (i=0; i<mint; i++)
|
|
|
862 { theta = 2*PI*(double)i/(double)mint;
|
|
|
863 c = cos(theta); s = sin(theta);
|
|
|
864 x[0] = orig[0]+r*c;
|
|
|
865 x[1] = orig[1]+r*s;
|
|
|
866
|
|
|
867 if (b!=0)
|
|
|
868 { M[0] =-r*s; M[1] = r*c;
|
|
|
869 M[2] = b*c; M[3] = b*s;
|
|
|
870 M[4] =-r*c; M[5] = -s;
|
|
|
871 M[6] = -s; M[7] = 0.0;
|
|
|
872 M[8] =-r*s; M[9] = c;
|
|
|
873 M[10]= c; M[11]= 0.0;
|
|
|
874 }
|
|
|
875
|
|
|
876 nr = f(x,2,tres,M);
|
|
|
877 if (i==0) setzero(res,nr);
|
|
|
878 for (j=0; j<nr; j++) res[j] += tres[j];
|
|
|
879 }
|
|
|
880 y = 2 * PI * ((b==0)?r:1.0) / mint;
|
|
|
881 for (j=0; j<nr; j++) res[j] *= y;
|
|
|
882 }
|
|
|
883
|
|
|
884 void integ_disc(f,fb,fl,res,resb,mg)
|
|
|
885 int (*f)(), (*fb)(), *mg;
|
|
|
886 double *fl, *res, *resb;
|
|
|
887 { double x[2], y, r, tres[MXRESULT], *orig, rmin, rmax, theta, c, s, M[12];
|
|
|
888 int ct, ctb, i, j, k, nr, nrb, w;
|
|
|
889
|
|
|
890 orig = &fl[2];
|
|
|
891 rmax = fl[1];
|
|
|
892 rmin = fl[0];
|
|
|
893 y = 0.0;
|
|
|
894 ct = ctb = 0;
|
|
|
895
|
|
|
896 for (j=0; j<mg[1]; j++)
|
|
|
897 { theta = 2*PI*(double)j/(double)mg[1];
|
|
|
898 c = cos(theta); s = sin(theta);
|
|
|
899 for (i= (rmin>0) ? 0 : 1; i<=mg[0]; i++)
|
|
|
900 { r = rmin + (rmax-rmin)*i/mg[0];
|
|
|
901 w = (2+2*(i&1)-(i==0)-(i==mg[0]));
|
|
|
902 x[0] = orig[0] + r*c;
|
|
|
903 x[1] = orig[1] + r*s;
|
|
|
904 nr = f(x,2,tres,NULL);
|
|
|
905 if (ct==0) setzero(res,nr);
|
|
|
906 for (k=0; k<nr; k++) res[k] += w*r*tres[k];
|
|
|
907 ct++;
|
|
|
908 if (((i==0) | (i==mg[0])) && (fb!=NULL))
|
|
|
909 { setM(M,r,s,c,1-2*(i==0));
|
|
|
910 nrb = fb(x,2,tres,M);
|
|
|
911 if (ctb==0) setzero(resb,nrb);
|
|
|
912 ctb++;
|
|
|
913 for (k=0; k<nrb; k++) resb[k] += tres[k];
|
|
|
914 }
|
|
|
915 }
|
|
|
916 }
|
|
|
917
|
|
|
918
|
|
|
919 /* for (i= (rmin>0) ? 0 : 1; i<=mg[0]; i++)
|
|
|
920 {
|
|
|
921 r = rmin + (rmax-rmin)*i/mg[0];
|
|
|
922 w = (2+2*(i&1)-(i==0)-(i==mg[0]));
|
|
|
923
|
|
|
924 for (j=0; j<mg[1]; j++)
|
|
|
925 { theta = 2*PI*(double)j/(double)mg[1];
|
|
|
926 c = cos(theta); s = sin(theta);
|
|
|
927 x[0] = orig[0] + r*c;
|
|
|
928 x[1] = orig[1] + r*s;
|
|
|
929 nr = f(x,2,tres,NULL);
|
|
|
930 if (ct==0) setzero(res,nr);
|
|
|
931 ct++;
|
|
|
932 for (k=0; k<nr; k++) res[k] += w*r*tres[k];
|
|
|
933
|
|
|
934 if (((i==0) | (i==mg[0])) && (fb!=NULL))
|
|
|
935 { setM(M,r,s,c,1-2*(i==0));
|
|
|
936 nrb = fb(x,2,tres,M);
|
|
|
937 if (ctb==0) setzero(resb,nrb);
|
|
|
938 ctb++;
|
|
|
939 for (k=0; k<nrb; k++) resb[k] += tres[k];
|
|
|
940 }
|
|
|
941 }
|
|
|
942 } */
|
|
|
943 for (j=0; j<nr; j++) res[j] *= 2*PI*(rmax-rmin)/(3*mg[0]*mg[1]);
|
|
|
944 for (j=0; j<nrb; j++) resb[j] *= 2*PI/mg[1];
|
|
|
945 }
|
|
|
946 /*
|
|
|
947 * Copyright 1996-2006 Catherine Loader.
|
|
|
948 */
|
|
|
949 /*
|
|
|
950 * Multivariate integration of a vector-valued function
|
|
|
951 * using Monte-Carlo method.
|
|
|
952 *
|
|
|
953 * uses drand48() random number generator. Does not seed.
|
|
|
954 */
|
|
|
955
|
|
|
956 #include <stdlib.h>
|
|
|
957 #include "mut.h"
|
|
|
958 extern void setzero();
|
|
|
959
|
|
|
960 static double M[(1+MXIDIM)*MXIDIM*MXIDIM];
|
|
|
961
|
|
|
962 void monte(f,ll,ur,d,res,n)
|
|
|
963 int (*f)(), d, n;
|
|
|
964 double *ll, *ur, *res;
|
|
|
965 {
|
|
|
966 int i, j, nr;
|
|
|
967 #ifdef WINDOWS
|
|
|
968 mut_printf("Sorry, Monte-Carlo Integration not enabled.\n");
|
|
|
969 for (i=0; i<n; i++) res[i] = 0.0;
|
|
|
970 #else
|
|
|
971 double z, x[MXIDIM], tres[MXRESULT];
|
|
|
972
|
|
|
973 srand48(234L);
|
|
|
974
|
|
|
975 for (i=0; i<n; i++)
|
|
|
976 { for (j=0; j<d; j++) x[j] = ll[j] + (ur[j]-ll[j])*drand48();
|
|
|
977 nr = f(x,d,tres,NULL);
|
|
|
978 if (i==0) setzero(res,nr);
|
|
|
979 for (j=0; j<nr; j++) res[j] += tres[j];
|
|
|
980 }
|
|
|
981
|
|
|
982 z = 1;
|
|
|
983 for (i=0; i<d; i++) z *= (ur[i]-ll[i]);
|
|
|
984 for (i=0; i<nr; i++) res[i] *= z/n;
|
|
|
985 #endif
|
|
|
986 }
|
|
|
987 /*
|
|
|
988 * Copyright 1996-2006 Catherine Loader.
|
|
|
989 */
|
|
|
990 /*
|
|
|
991 * Multivariate integration of a vector-valued function
|
|
|
992 * using Simpson's rule.
|
|
|
993 */
|
|
|
994
|
|
|
995 #include <math.h>
|
|
|
996 #include "mut.h"
|
|
|
997 extern void setzero();
|
|
|
998
|
|
|
999 static double M[(1+MXIDIM)*MXIDIM*MXIDIM];
|
|
|
1000
|
|
|
1001 /* third order corners */
|
|
|
1002 void simp3(fd,x,d,resd,delta,wt,i0,i1,mg,ct,res2,index)
|
|
|
1003 int (*fd)(), d, wt, i0, i1, *mg, ct, *index;
|
|
|
1004 double *x, *resd, *delta, *res2;
|
|
|
1005 { int k, l, m, nrd;
|
|
|
1006 double zb;
|
|
|
1007
|
|
|
1008 for (k=i1+1; k<d; k++) if ((index[k]==0) | (index[k]==mg[k]))
|
|
|
1009 {
|
|
|
1010 setzero(M,d*d);
|
|
|
1011 m = 0; zb = 1.0;
|
|
|
1012 for (l=0; l<d; l++)
|
|
|
1013 if ((l!=i0) & (l!=i1) & (l!=k))
|
|
|
1014 { M[m*d+l] = 1.0;
|
|
|
1015 m++;
|
|
|
1016 zb *= delta[l];
|
|
|
1017 }
|
|
|
1018 M[(d-3)*d+i0] = (index[i0]==0) ? -1 : 1;
|
|
|
1019 M[(d-2)*d+i1] = (index[i1]==0) ? -1 : 1;
|
|
|
1020 M[(d-1)*d+k] = (index[k]==0) ? -1 : 1;
|
|
|
1021 nrd = fd(x,d,res2,M);
|
|
|
1022 if ((ct==0) & (i0==0) & (i1==1) & (k==2)) setzero(resd,nrd);
|
|
|
1023 for (l=0; l<nrd; l++)
|
|
|
1024 resd[l] += wt*zb*res2[l];
|
|
|
1025 }
|
|
|
1026 }
|
|
|
1027
|
|
|
1028 /* second order corners */
|
|
|
1029 void simp2(fc,fd,x,d,resc,resd,delta,wt,i0,mg,ct,res2,index)
|
|
|
1030 int (*fc)(), (*fd)(), d, wt, i0, *mg, ct, *index;
|
|
|
1031 double *x, *resc, *resd, *delta, *res2;
|
|
|
1032 { int j, k, l, nrc;
|
|
|
1033 double zb;
|
|
|
1034 for (j=i0+1; j<d; j++) if ((index[j]==0) | (index[j]==mg[j]))
|
|
|
1035 { setzero(M,d*d);
|
|
|
1036 l = 0; zb = 1;
|
|
|
1037 for (k=0; k<d; k++) if ((k!=i0) & (k!=j))
|
|
|
1038 { M[l*d+k] = 1.0;
|
|
|
1039 l++;
|
|
|
1040 zb *= delta[k];
|
|
|
1041 }
|
|
|
1042 M[(d-2)*d+i0] = (index[i0]==0) ? -1 : 1;
|
|
|
1043 M[(d-1)*d+j] = (index[j]==0) ? -1 : 1;
|
|
|
1044 nrc = fc(x,d,res2,M);
|
|
|
1045 if ((ct==0) & (i0==0) & (j==1)) setzero(resc,nrc);
|
|
|
1046 for (k=0; k<nrc; k++) resc[k] += wt*zb*res2[k];
|
|
|
1047
|
|
|
1048 if (fd!=NULL)
|
|
|
1049 simp3(fd,x,d,resd,delta,wt,i0,j,mg,ct,res2,index);
|
|
|
1050 }
|
|
|
1051 }
|
|
|
1052
|
|
|
1053 /* first order boundary */
|
|
|
1054 void simp1(fb,fc,fd,x,d,resb,resc,resd,delta,wt,mg,ct,res2,index)
|
|
|
1055 int (*fb)(), (*fc)(), (*fd)(), d, wt, *mg, ct, *index;
|
|
|
1056 double *x, *resb, *resc, *resd, *delta, *res2;
|
|
|
1057 { int i, j, k, nrb;
|
|
|
1058 double zb;
|
|
|
1059 for (i=0; i<d; i++) if ((index[i]==0) | (index[i]==mg[i]))
|
|
|
1060 { setzero(M,(1+d)*d*d);
|
|
|
1061 k = 0;
|
|
|
1062 for (j=0; j<d; j++) if (j!=i)
|
|
|
1063 { M[k*d+j] = 1;
|
|
|
1064 k++;
|
|
|
1065 }
|
|
|
1066 M[(d-1)*d+i] = (index[i]==0) ? -1 : 1;
|
|
|
1067 nrb = fb(x,d,res2,M);
|
|
|
1068 zb = 1;
|
|
|
1069 for (j=0; j<d; j++) if (i!=j) zb *= delta[j];
|
|
|
1070 if ((ct==0) && (i==0))
|
|
|
1071 for (j=0; j<nrb; j++) resb[j] = 0.0;
|
|
|
1072 for (j=0; j<nrb; j++) resb[j] += wt*zb*res2[j];
|
|
|
1073
|
|
|
1074 if (fc!=NULL)
|
|
|
1075 simp2(fc,fd,x,d,resc,resd,delta,wt,i,mg,ct,res2,index);
|
|
|
1076 }
|
|
|
1077 }
|
|
|
1078
|
|
|
1079 void simpson4(f,fb,fc,fd,ll,ur,d,res,resb,resc,resd,mg,res2)
|
|
|
1080 int (*f)(), (*fb)(), (*fc)(), (*fd)(), d, *mg;
|
|
|
1081 double *ll, *ur, *res, *resb, *resc, *resd, *res2;
|
|
|
1082 { int ct, i, j, nr, wt, index[MXIDIM];
|
|
|
1083 double x[MXIDIM], delta[MXIDIM], z;
|
|
|
1084
|
|
|
1085 for (i=0; i<d; i++)
|
|
|
1086 { index[i] = 0;
|
|
|
1087 x[i] = ll[i];
|
|
|
1088 if (mg[i]&1) mg[i]++;
|
|
|
1089 delta[i] = (ur[i]-ll[i])/(3*mg[i]);
|
|
|
1090 }
|
|
|
1091 ct = 0;
|
|
|
1092
|
|
|
1093 while(1)
|
|
|
1094 { wt = 1;
|
|
|
1095 for (i=0; i<d; i++)
|
|
|
1096 wt *= (4-2*(index[i]%2==0)-(index[i]==0)-(index[i]==mg[i]));
|
|
|
1097 nr = f(x,d,res2,NULL);
|
|
|
1098 if (ct==0) setzero(res,nr);
|
|
|
1099 for (i=0; i<nr; i++) res[i] += wt*res2[i];
|
|
|
1100
|
|
|
1101 if (fb!=NULL)
|
|
|
1102 simp1(fb,fc,fd,x,d,resb,resc,resd,delta,wt,mg,ct,res2,index);
|
|
|
1103
|
|
|
1104 /* compute next grid point */
|
|
|
1105 for (i=0; i<d; i++)
|
|
|
1106 { index[i]++;
|
|
|
1107 if (index[i]>mg[i])
|
|
|
1108 { index[i] = 0;
|
|
|
1109 x[i] = ll[i];
|
|
|
1110 if (i==d-1) /* done */
|
|
|
1111 { z = 1.0;
|
|
|
1112 for (j=0; j<d; j++) z *= delta[j];
|
|
|
1113 for (j=0; j<nr; j++) res[j] *= z;
|
|
|
1114 return;
|
|
|
1115 }
|
|
|
1116 }
|
|
|
1117 else
|
|
|
1118 { x[i] = ll[i] + 3*delta[i]*index[i];
|
|
|
1119 i = d;
|
|
|
1120 }
|
|
|
1121 }
|
|
|
1122 ct++;
|
|
|
1123 }
|
|
|
1124 }
|
|
|
1125
|
|
|
1126 void simpsonm(f,ll,ur,d,res,mg,res2)
|
|
|
1127 int (*f)(), d, *mg;
|
|
|
1128 double *ll, *ur, *res, *res2;
|
|
|
1129 { simpson4(f,NULL,NULL,NULL,ll,ur,d,res,NULL,NULL,NULL,mg,res2);
|
|
|
1130 }
|
|
|
1131
|
|
|
1132 double simpson(f,l0,l1,m)
|
|
|
1133 double (*f)(), l0, l1;
|
|
|
1134 int m;
|
|
|
1135 { double x, sum;
|
|
|
1136 int i;
|
|
|
1137 sum = 0;
|
|
|
1138 for (i=0; i<=m; i++)
|
|
|
1139 { x = ((m-i)*l0 + i*l1)/m;
|
|
|
1140 sum += (2+2*(i&1)-(i==0)-(i==m)) * f(x);
|
|
|
1141 }
|
|
|
1142 return( (l1-l0) * sum / (3*m) );
|
|
|
1143 }
|
|
|
1144 /*
|
|
|
1145 * Copyright 1996-2006 Catherine Loader.
|
|
|
1146 */
|
|
|
1147 #include "mut.h"
|
|
|
1148
|
|
|
1149 static double *res, *resb, *orig, rmin, rmax;
|
|
|
1150 static int ct0;
|
|
|
1151
|
|
|
1152 void sphM(M,r,u)
|
|
|
1153 double *M, r, *u;
|
|
|
1154 { double h, u1[3], u2[3];
|
|
|
1155
|
|
|
1156 /* set the orthogonal unit vectors. */
|
|
|
1157 h = sqrt(u[0]*u[0]+u[1]*u[1]);
|
|
|
1158 if (h<=0)
|
|
|
1159 { u1[0] = u2[1] = 1.0;
|
|
|
1160 u1[1] = u1[2] = u2[0] = u2[2] = 0.0;
|
|
|
1161 }
|
|
|
1162 else
|
|
|
1163 { u1[0] = u[1]/h; u1[1] = -u[0]/h; u1[2] = 0.0;
|
|
|
1164 u2[0] = u[2]*u[0]/h; u2[1] = u[2]*u[1]/h; u2[2] = -h;
|
|
|
1165 }
|
|
|
1166
|
|
|
1167 /* parameterize the sphere as r(cos(t)cos(v)u + sin(t)u1 + cos(t)sin(v)u2).
|
|
|
1168 * first layer of M is (dx/dt, dx/dv, dx/dr) at t=v=0.
|
|
|
1169 */
|
|
|
1170 M[0] = r*u1[0]; M[1] = r*u1[1]; M[2] = r*u1[2];
|
|
|
1171 M[3] = r*u2[0]; M[4] = r*u2[1]; M[5] = r*u2[2];
|
|
|
1172 M[6] = u[0]; M[7] = u[1]; M[8] = u[2];
|
|
|
1173
|
|
|
1174 /* next layers are second derivative matrix of components of x(r,t,v).
|
|
|
1175 * d^2x/dt^2 = d^2x/dv^2 = -ru; d^2x/dtdv = 0;
|
|
|
1176 * d^2x/drdt = u1; d^2x/drdv = u2; d^2x/dr^2 = 0.
|
|
|
1177 */
|
|
|
1178
|
|
|
1179 M[9] = M[13] = -r*u[0];
|
|
|
1180 M[11]= M[15] = u1[0];
|
|
|
1181 M[14]= M[16] = u2[0];
|
|
|
1182 M[10]= M[12] = M[17] = 0.0;
|
|
|
1183
|
|
|
1184 M[18]= M[22] = -r*u[1];
|
|
|
1185 M[20]= M[24] = u1[1];
|
|
|
1186 M[23]= M[25] = u2[1];
|
|
|
1187 M[19]= M[21] = M[26] = 0.0;
|
|
|
1188
|
|
|
1189 M[27]= M[31] = -r*u[1];
|
|
|
1190 M[29]= M[33] = u1[1];
|
|
|
1191 M[32]= M[34] = u2[1];
|
|
|
1192 M[28]= M[30] = M[35] = 0.0;
|
|
|
1193
|
|
|
1194 }
|
|
|
1195
|
|
|
1196 double ip3(a,b)
|
|
|
1197 double *a, *b;
|
|
|
1198 { return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2]);
|
|
|
1199 }
|
|
|
1200
|
|
|
1201 void rn3(a)
|
|
|
1202 double *a;
|
|
|
1203 { double s;
|
|
|
1204 s = sqrt(ip3(a,a));
|
|
|
1205 a[0] /= s; a[1] /= s; a[2] /= s;
|
|
|
1206 }
|
|
|
1207
|
|
|
1208 double sptarea(a,b,c)
|
|
|
1209 double *a, *b, *c;
|
|
|
1210 { double ea, eb, ec, yab, yac, ybc, sab, sac, sbc;
|
|
|
1211 double ab[3], ac[3], bc[3], x1[3], x2[3];
|
|
|
1212
|
|
|
1213 ab[0] = a[0]-b[0]; ab[1] = a[1]-b[1]; ab[2] = a[2]-b[2];
|
|
|
1214 ac[0] = a[0]-c[0]; ac[1] = a[1]-c[1]; ac[2] = a[2]-c[2];
|
|
|
1215 bc[0] = b[0]-c[0]; bc[1] = b[1]-c[1]; bc[2] = b[2]-c[2];
|
|
|
1216
|
|
|
1217 yab = ip3(ab,a); yac = ip3(ac,a); ybc = ip3(bc,b);
|
|
|
1218
|
|
|
1219 x1[0] = ab[0] - yab*a[0]; x2[0] = ac[0] - yac*a[0];
|
|
|
1220 x1[1] = ab[1] - yab*a[1]; x2[1] = ac[1] - yac*a[1];
|
|
|
1221 x1[2] = ab[2] - yab*a[2]; x2[2] = ac[2] - yac*a[2];
|
|
|
1222 sab = ip3(x1,x1); sac = ip3(x2,x2);
|
|
|
1223 ea = acos(ip3(x1,x2)/sqrt(sab*sac));
|
|
|
1224
|
|
|
1225 x1[0] = ab[0] + yab*b[0]; x2[0] = bc[0] - ybc*b[0];
|
|
|
1226 x1[1] = ab[1] + yab*b[1]; x2[1] = bc[1] - ybc*b[1];
|
|
|
1227 x1[2] = ab[2] + yab*b[2]; x2[2] = bc[2] - ybc*b[2];
|
|
|
1228 sbc = ip3(x2,x2);
|
|
|
1229 eb = acos(ip3(x1,x2)/sqrt(sab*sbc));
|
|
|
1230
|
|
|
1231 x1[0] = ac[0] + yac*c[0]; x2[0] = bc[0] + ybc*c[0];
|
|
|
1232 x1[1] = ac[1] + yac*c[1]; x2[1] = bc[1] + ybc*c[1];
|
|
|
1233 x1[2] = ac[2] + yac*c[2]; x2[2] = bc[2] + ybc*c[2];
|
|
|
1234 ec = acos(ip3(x1,x2)/sqrt(sac*sbc));
|
|
|
1235
|
|
|
1236 /*
|
|
|
1237 * Euler's formula is a+b+c-PI, except I've cheated...
|
|
|
1238 * a=ea, c=ec, b=PI-eb, which is more stable.
|
|
|
1239 */
|
|
|
1240 return(ea+ec-eb);
|
|
|
1241 }
|
|
|
1242
|
|
|
1243 void li(x,f,fb,mint,ar)
|
|
|
1244 double *x, ar;
|
|
|
1245 int (*f)(), (*fb)(), mint;
|
|
|
1246 { int i, j, nr, nrb, ct1, w;
|
|
|
1247 double u[3], r, M[36];
|
|
|
1248 double sres[MXRESULT], tres[MXRESULT];
|
|
|
1249
|
|
|
1250 /* divide mint by 2, and force to even (Simpson's rule...)
|
|
|
1251 * to make comparable with rectangular interpretation of mint
|
|
|
1252 */
|
|
|
1253 mint <<= 1;
|
|
|
1254 if (mint&1) mint++;
|
|
|
1255
|
|
|
1256 ct1 = 0;
|
|
|
1257 for (i= (rmin==0) ? 1 : 0; i<=mint; i++)
|
|
|
1258 {
|
|
|
1259 r = rmin + (rmax-rmin)*i/mint;
|
|
|
1260 w = 2+2*(i&1)-(i==0)-(i==mint);
|
|
|
1261 u[0] = orig[0]+x[0]*r;
|
|
|
1262 u[1] = orig[1]+x[1]*r;
|
|
|
1263 u[2] = orig[2]+x[2]*r;
|
|
|
1264 nr = f(u,3,tres,NULL);
|
|
|
1265 if (ct1==0) setzero(sres,nr);
|
|
|
1266 for (j=0; j<nr; j++)
|
|
|
1267 sres[j] += w*r*r*tres[j];
|
|
|
1268 ct1++;
|
|
|
1269
|
|
|
1270 if ((fb!=NULL) && (i==mint)) /* boundary */
|
|
|
1271 { sphM(M,rmax,x);
|
|
|
1272 nrb = fb(u,3,tres,M);
|
|
|
1273 if (ct0==0) for (j=0; j<nrb; j++) resb[j] = 0.0;
|
|
|
1274 for (j=0; j<nrb; j++)
|
|
|
1275 resb[j] += tres[j]*ar;
|
|
|
1276 }
|
|
|
1277 }
|
|
|
1278
|
|
|
1279 if (ct0==0) for (j=0; j<nr; j++) res[j] = 0.0;
|
|
|
1280 ct0++;
|
|
|
1281
|
|
|
1282 for (j=0; j<nr; j++)
|
|
|
1283 res[j] += sres[j] * ar * (rmax-rmin)/(3*mint);
|
|
|
1284 }
|
|
|
1285
|
|
|
1286 void sphint(f,fb,a,b,c,lev,mint,cent)
|
|
|
1287 double *a, *b, *c;
|
|
|
1288 int (*f)(), (*fb)(), lev, mint, cent;
|
|
|
1289 { double x[3], ab[3], ac[3], bc[3], ar;
|
|
|
1290 int i;
|
|
|
1291
|
|
|
1292 if (lev>1)
|
|
|
1293 { ab[0] = a[0]+b[0]; ab[1] = a[1]+b[1]; ab[2] = a[2]+b[2]; rn3(ab);
|
|
|
1294 ac[0] = a[0]+c[0]; ac[1] = a[1]+c[1]; ac[2] = a[2]+c[2]; rn3(ac);
|
|
|
1295 bc[0] = b[0]+c[0]; bc[1] = b[1]+c[1]; bc[2] = b[2]+c[2]; rn3(bc);
|
|
|
1296 lev >>= 1;
|
|
|
1297 if (cent==0)
|
|
|
1298 { sphint(f,fb,a,ab,ac,lev,mint,1);
|
|
|
1299 sphint(f,fb,ab,bc,ac,lev,mint,0);
|
|
|
1300 }
|
|
|
1301 else
|
|
|
1302 { sphint(f,fb,a,ab,ac,lev,mint,1);
|
|
|
1303 sphint(f,fb,b,ab,bc,lev,mint,1);
|
|
|
1304 sphint(f,fb,c,ac,bc,lev,mint,1);
|
|
|
1305 sphint(f,fb,ab,bc,ac,lev,mint,1);
|
|
|
1306 }
|
|
|
1307 return;
|
|
|
1308 }
|
|
|
1309
|
|
|
1310 x[0] = a[0]+b[0]+c[0];
|
|
|
1311 x[1] = a[1]+b[1]+c[1];
|
|
|
1312 x[2] = a[2]+b[2]+c[2];
|
|
|
1313 rn3(x);
|
|
|
1314 ar = sptarea(a,b,c);
|
|
|
1315
|
|
|
1316 for (i=0; i<8; i++)
|
|
|
1317 { if (i>0)
|
|
|
1318 { x[0] = -x[0];
|
|
|
1319 if (i%2 == 0) x[1] = -x[1];
|
|
|
1320 if (i==4) x[2] = -x[2];
|
|
|
1321 }
|
|
|
1322 switch(cent)
|
|
|
1323 { case 2: /* the reflection and its 120', 240' rotations */
|
|
|
1324 ab[0] = x[0]; ab[1] = x[2]; ab[2] = x[1]; li(ab,f,fb,mint,ar);
|
|
|
1325 ab[0] = x[2]; ab[1] = x[1]; ab[2] = x[0]; li(ab,f,fb,mint,ar);
|
|
|
1326 ab[0] = x[1]; ab[1] = x[0]; ab[2] = x[2]; li(ab,f,fb,mint,ar);
|
|
|
1327 case 1: /* and the 120' and 240' rotations */
|
|
|
1328 ab[0] = x[1]; ab[1] = x[2]; ab[2] = x[0]; li(ab,f,fb,mint,ar);
|
|
|
1329 ac[0] = x[2]; ac[1] = x[0]; ac[2] = x[1]; li(ac,f,fb,mint,ar);
|
|
|
1330 case 0: /* and the triangle itself. */
|
|
|
1331 li( x,f,fb,mint,ar);
|
|
|
1332 }
|
|
|
1333 }
|
|
|
1334 }
|
|
|
1335
|
|
|
1336 void integ_sphere(f,fb,fl,Res,Resb,mg)
|
|
|
1337 double *fl, *Res, *Resb;
|
|
|
1338 int (*f)(), (*fb)(), *mg;
|
|
|
1339 { double a[3], b[3], c[3];
|
|
|
1340
|
|
|
1341 a[0] = 1; a[1] = a[2] = 0;
|
|
|
1342 b[1] = 1; b[0] = b[2] = 0;
|
|
|
1343 c[2] = 1; c[0] = c[1] = 0;
|
|
|
1344
|
|
|
1345 res = Res;
|
|
|
1346 resb=Resb;
|
|
|
1347 orig = &fl[2];
|
|
|
1348 rmin = fl[0];
|
|
|
1349 rmax = fl[1];
|
|
|
1350
|
|
|
1351 ct0 = 0;
|
|
|
1352 sphint(f,fb,a,b,c,mg[1],mg[0],0);
|
|
|
1353 }
|
|
|
1354 /*
|
|
|
1355 * Copyright 1996-2006 Catherine Loader.
|
|
|
1356 */
|
|
|
1357 /*
|
|
|
1358 * solving symmetric equations using the jacobian structure. Currently, three
|
|
|
1359 * methods can be used: cholesky decomposition, eigenvalues, eigenvalues on
|
|
|
1360 * the correlation matrix.
|
|
|
1361 *
|
|
|
1362 * jacob_dec(J,meth) decompose the matrix, meth=JAC_CHOL, JAC_EIG, JAC_EIGD
|
|
|
1363 * jacob_solve(J,v) J^{-1}v
|
|
|
1364 * jacob_hsolve(J,v) (R')^{-1/2}v
|
|
|
1365 * jacob_isolve(J,v) (R)^{-1/2}v
|
|
|
1366 * jacob_qf(J,v) v' J^{-1} v
|
|
|
1367 * jacob_mult(J,v) (R'R) v (pres. CHOL only)
|
|
|
1368 * where for each decomposition, R'R=J, although the different decomp's will
|
|
|
1369 * produce different R's.
|
|
|
1370 *
|
|
|
1371 * To set up the J matrix:
|
|
|
1372 * first, allocate storage: jac_alloc(J,p,wk)
|
|
|
1373 * where p=dimension of matrix, wk is a numeric vector of length
|
|
|
1374 * jac_reqd(p) (or NULL, to allocate automatically).
|
|
|
1375 * now, copy the numeric values to J->Z (numeric vector with length p*p).
|
|
|
1376 * (or, just set J->Z to point to the data vector. But remember this
|
|
|
1377 * will be overwritten by the decomposition).
|
|
|
1378 * finally, set:
|
|
|
1379 * J->st=JAC_RAW;
|
|
|
1380 * J->p = p;
|
|
|
1381 *
|
|
|
1382 * now, call jac_dec(J,meth) (optional) and the solve functions as required.
|
|
|
1383 *
|
|
|
1384 */
|
|
|
1385
|
|
|
1386 #include "math.h"
|
|
|
1387 #include "mut.h"
|
|
|
1388
|
|
|
1389 #define DEF_METH JAC_EIGD
|
|
|
1390
|
|
|
1391 int jac_reqd(int p) { return(2*p*(p+1)); }
|
|
|
1392
|
|
|
1393 double *jac_alloc(J,p,wk)
|
|
|
1394 jacobian *J;
|
|
|
1395 int p;
|
|
|
1396 double *wk;
|
|
|
1397 { if (wk==NULL)
|
|
|
1398 wk = (double *)calloc(2*p*(p+1),sizeof(double));
|
|
|
1399 if ( wk == NULL ) {
|
|
|
1400 printf("Problem allocating memory for wk\n");fflush(stdout);
|
|
|
1401 }
|
|
|
1402 J->Z = wk; wk += p*p;
|
|
|
1403 J->Q = wk; wk += p*p;
|
|
|
1404 J->wk= wk; wk += p;
|
|
|
1405 J->dg= wk; wk += p;
|
|
|
1406 return(wk);
|
|
|
1407 }
|
|
|
1408
|
|
|
1409 void jacob_dec(J, meth)
|
|
|
1410 jacobian *J;
|
|
|
1411 int meth;
|
|
|
1412 { int i, j, p;
|
|
|
1413
|
|
|
1414 if (J->st != JAC_RAW) return;
|
|
|
1415
|
|
|
1416 J->sm = J->st = meth;
|
|
|
1417 switch(meth)
|
|
|
1418 { case JAC_EIG:
|
|
|
1419 eig_dec(J->Z,J->Q,J->p);
|
|
|
1420 return;
|
|
|
1421 case JAC_EIGD:
|
|
|
1422 p = J->p;
|
|
|
1423 for (i=0; i<p; i++)
|
|
|
1424 J->dg[i] = (J->Z[i*(p+1)]<=0) ? 0.0 : 1/sqrt(J->Z[i*(p+1)]);
|
|
|
1425 for (i=0; i<p; i++)
|
|
|
1426 for (j=0; j<p; j++)
|
|
|
1427 J->Z[i*p+j] *= J->dg[i]*J->dg[j];
|
|
|
1428 eig_dec(J->Z,J->Q,J->p);
|
|
|
1429 J->st = JAC_EIGD;
|
|
|
1430 return;
|
|
|
1431 case JAC_CHOL:
|
|
|
1432 chol_dec(J->Z,J->p,J->p);
|
|
|
1433 return;
|
|
|
1434 default: mut_printf("jacob_dec: unknown method %d",meth);
|
|
|
1435 }
|
|
|
1436 }
|
|
|
1437
|
|
|
1438 int jacob_solve(J,v) /* (X^T W X)^{-1} v */
|
|
|
1439 jacobian *J;
|
|
|
1440 double *v;
|
|
|
1441 { int i, rank;
|
|
|
1442
|
|
|
1443 if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);
|
|
|
1444
|
|
|
1445 switch(J->st)
|
|
|
1446 { case JAC_EIG:
|
|
|
1447 return(eig_solve(J,v));
|
|
|
1448 case JAC_EIGD:
|
|
|
1449 for (i=0; i<J->p; i++) v[i] *= J->dg[i];
|
|
|
1450 rank = eig_solve(J,v);
|
|
|
1451 for (i=0; i<J->p; i++) v[i] *= J->dg[i];
|
|
|
1452 return(rank);
|
|
|
1453 case JAC_CHOL:
|
|
|
1454 return(chol_solve(J->Z,v,J->p,J->p));
|
|
|
1455 }
|
|
|
1456 mut_printf("jacob_solve: unknown method %d",J->st);
|
|
|
1457 return(0);
|
|
|
1458 }
|
|
|
1459
|
|
|
1460 int jacob_hsolve(J,v) /* J^{-1/2} v */
|
|
|
1461 jacobian *J;
|
|
|
1462 double *v;
|
|
|
1463 { int i;
|
|
|
1464
|
|
|
1465 if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);
|
|
|
1466
|
|
|
1467 switch(J->st)
|
|
|
1468 { case JAC_EIG:
|
|
|
1469 return(eig_hsolve(J,v));
|
|
|
1470 case JAC_EIGD: /* eigenvalues on corr matrix */
|
|
|
1471 for (i=0; i<J->p; i++) v[i] *= J->dg[i];
|
|
|
1472 return(eig_hsolve(J,v));
|
|
|
1473 case JAC_CHOL:
|
|
|
1474 return(chol_hsolve(J->Z,v,J->p,J->p));
|
|
|
1475 }
|
|
|
1476 mut_printf("jacob_hsolve: unknown method %d\n",J->st);
|
|
|
1477 return(0);
|
|
|
1478 }
|
|
|
1479
|
|
|
1480 int jacob_isolve(J,v) /* J^{-1/2} v */
|
|
|
1481 jacobian *J;
|
|
|
1482 double *v;
|
|
|
1483 { int i, r;
|
|
|
1484
|
|
|
1485 if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);
|
|
|
1486
|
|
|
1487 switch(J->st)
|
|
|
1488 { case JAC_EIG:
|
|
|
1489 return(eig_isolve(J,v));
|
|
|
1490 case JAC_EIGD: /* eigenvalues on corr matrix */
|
|
|
1491 r = eig_isolve(J,v);
|
|
|
1492 for (i=0; i<J->p; i++) v[i] *= J->dg[i];
|
|
|
1493 return(r);
|
|
|
1494 case JAC_CHOL:
|
|
|
1495 return(chol_isolve(J->Z,v,J->p,J->p));
|
|
|
1496 }
|
|
|
1497 mut_printf("jacob_hsolve: unknown method %d\n",J->st);
|
|
|
1498 return(0);
|
|
|
1499 }
|
|
|
1500
|
|
|
1501 double jacob_qf(J,v) /* vT J^{-1} v */
|
|
|
1502 jacobian *J;
|
|
|
1503 double *v;
|
|
|
1504 { int i;
|
|
|
1505
|
|
|
1506 if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);
|
|
|
1507
|
|
|
1508 switch (J->st)
|
|
|
1509 { case JAC_EIG:
|
|
|
1510 return(eig_qf(J,v));
|
|
|
1511 case JAC_EIGD:
|
|
|
1512 for (i=0; i<J->p; i++) v[i] *= J->dg[i];
|
|
|
1513 return(eig_qf(J,v));
|
|
|
1514 case JAC_CHOL:
|
|
|
1515 return(chol_qf(J->Z,v,J->p,J->p));
|
|
|
1516 default:
|
|
|
1517 mut_printf("jacob_qf: invalid method\n");
|
|
|
1518 return(0.0);
|
|
|
1519 }
|
|
|
1520 }
|
|
|
1521
|
|
|
1522 int jacob_mult(J,v) /* J v */
|
|
|
1523 jacobian *J;
|
|
|
1524 double *v;
|
|
|
1525 {
|
|
|
1526 if (J->st == JAC_RAW) jacob_dec(J,DEF_METH);
|
|
|
1527 switch (J->st)
|
|
|
1528 { case JAC_CHOL:
|
|
|
1529 return(chol_mult(J->Z,v,J->p,J->p));
|
|
|
1530 default:
|
|
|
1531 mut_printf("jacob_mult: invalid method\n");
|
|
|
1532 return(0);
|
|
|
1533 }
|
|
|
1534 }
|
|
|
1535 /*
|
|
|
1536 * Copyright 1996-2006 Catherine Loader.
|
|
|
1537 */
|
|
|
1538 /*
|
|
|
1539 * Routines for maximization of a one dimensional function f()
|
|
|
1540 * over an interval [xlo,xhi]. In all cases. the flag argument
|
|
|
1541 * controls the return:
|
|
|
1542 * flag='x', the maximizer xmax is returned.
|
|
|
1543 * otherwise, maximum f(xmax) is returned.
|
|
|
1544 *
|
|
|
1545 * max_grid(f,xlo,xhi,n,flag)
|
|
|
1546 * grid maximization of f() over [xlo,xhi] with n intervals.
|
|
|
1547 *
|
|
|
1548 * max_golden(f,xlo,xhi,n,tol,err,flag)
|
|
|
1549 * golden section maximization.
|
|
|
1550 * If n>2, an initial grid search is performed with n intervals
|
|
|
1551 * (this helps deal with local maxima).
|
|
|
1552 * convergence criterion is |x-xmax| < tol.
|
|
|
1553 * err is an error flag.
|
|
|
1554 * if flag='x', return value is xmax.
|
|
|
1555 * otherwise, return value is f(xmax).
|
|
|
1556 *
|
|
|
1557 * max_quad(f,xlo,xhi,n,tol,err,flag)
|
|
|
1558 * quadratic maximization.
|
|
|
1559 *
|
|
|
1560 * max_nr()
|
|
|
1561 * newton-raphson, handles multivariate case.
|
|
|
1562 *
|
|
|
1563 * TODO: additional error checking, non-convergence stop.
|
|
|
1564 */
|
|
|
1565
|
|
|
1566 #include <math.h>
|
|
|
1567 #include "mut.h"
|
|
|
1568
|
|
|
1569 #define max_val(a,b) ((flag=='x') ? a : b)
|
|
|
1570
|
|
|
1571 double max_grid(f,xlo,xhi,n,flag)
|
|
|
1572 double (*f)(), xlo, xhi;
|
|
|
1573 int n;
|
|
|
1574 char flag;
|
|
|
1575 { int i, mi;
|
|
|
1576 double x, y, mx, my;
|
|
|
1577 for (i=0; i<=n; i++)
|
|
|
1578 { x = xlo + (xhi-xlo)*i/n;
|
|
|
1579 y = f(x);
|
|
|
1580 if ((i==0) || (y>my))
|
|
|
1581 { mx = x;
|
|
|
1582 my = y;
|
|
|
1583 mi = i;
|
|
|
1584 }
|
|
|
1585 }
|
|
|
1586 if (mi==0) return(max_val(xlo,my));
|
|
|
1587 if (mi==n) return(max_val(xhi,my));
|
|
|
1588 return(max_val(mx,my));
|
|
|
1589 }
|
|
|
1590
|
|
|
1591 double max_golden(f,xlo,xhi,n,tol,err,flag)
|
|
|
1592 double (*f)(), xhi, xlo, tol;
|
|
|
1593 int n, *err;
|
|
|
1594 char flag;
|
|
|
1595 { double dlt, x0, x1, x2, x3, y0, y1, y2, y3;
|
|
|
1596 *err = 0;
|
|
|
1597
|
|
|
1598 if (n>2)
|
|
|
1599 { dlt = (xhi-xlo)/n;
|
|
|
1600 x0 = max_grid(f,xlo,xhi,n,'x');
|
|
|
1601 if (xlo<x0) xlo = x0-dlt;
|
|
|
1602 if (xhi>x0) xhi = x0+dlt;
|
|
|
1603 }
|
|
|
1604
|
|
|
1605 x0 = xlo; y0 = f(xlo);
|
|
|
1606 x3 = xhi; y3 = f(xhi);
|
|
|
1607 x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1);
|
|
|
1608 x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2);
|
|
|
1609
|
|
|
1610 while (fabs(x3-x0)>tol)
|
|
|
1611 { if ((y1>=y0) && (y1>=y2))
|
|
|
1612 { x3 = x2; y3 = y2;
|
|
|
1613 x2 = x1; y2 = y1;
|
|
|
1614 x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1);
|
|
|
1615 }
|
|
|
1616 else if ((y2>=y3) && (y2>=y1))
|
|
|
1617 { x0 = x1; y0 = y1;
|
|
|
1618 x1 = x2; y1 = y2;
|
|
|
1619 x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2);
|
|
|
1620 }
|
|
|
1621 else
|
|
|
1622 { if (y3>y0) { x0 = x2; y0 = y2; }
|
|
|
1623 else { x3 = x1; y3 = y1; }
|
|
|
1624 x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1);
|
|
|
1625 x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2);
|
|
|
1626 }
|
|
|
1627 }
|
|
|
1628 if (y0>=y1) return(max_val(x0,y0));
|
|
|
1629 if (y3>=y2) return(max_val(x3,y3));
|
|
|
1630 return((y1>y2) ? max_val(x1,y1) : max_val(x2,y2));
|
|
|
1631 }
|
|
|
1632
|
|
|
1633 double max_quad(f,xlo,xhi,n,tol,err,flag)
|
|
|
1634 double (*f)(), xhi, xlo, tol;
|
|
|
1635 int n, *err;
|
|
|
1636 char flag;
|
|
|
1637 { double x0, x1, x2, xnew, y0, y1, y2, ynew, a, b;
|
|
|
1638 *err = 0;
|
|
|
1639
|
|
|
1640 if (n>2)
|
|
|
1641 { x0 = max_grid(f,xlo,xhi,n,'x');
|
|
|
1642 if (xlo<x0) xlo = x0-1.0/n;
|
|
|
1643 if (xhi>x0) xhi = x0+1.0/n;
|
|
|
1644 }
|
|
|
1645
|
|
|
1646 x0 = xlo; y0 = f(x0);
|
|
|
1647 x2 = xhi; y2 = f(x2);
|
|
|
1648 x1 = (x0+x2)/2; y1 = f(x1);
|
|
|
1649
|
|
|
1650 while (x2-x0>tol)
|
|
|
1651 {
|
|
|
1652 /* first, check (y0,y1,y2) is a peak. If not,
|
|
|
1653 * next interval is the halve with larger of (y0,y2).
|
|
|
1654 */
|
|
|
1655 if ((y0>y1) | (y2>y1))
|
|
|
1656 {
|
|
|
1657 if (y0>y2) { x2 = x1; y2 = y1; }
|
|
|
1658 else { x0 = x1; y0 = y1; }
|
|
|
1659 x1 = (x0+x2)/2;
|
|
|
1660 y1 = f(x1);
|
|
|
1661 }
|
|
|
1662 else /* peak */
|
|
|
1663 { a = (y1-y0)*(x2-x1) + (y1-y2)*(x1-x0);
|
|
|
1664 b = ((y1-y0)*(x2-x1)*(x2+x1) + (y1-y2)*(x1-x0)*(x1+x0))/2;
|
|
|
1665 /* quadratic maximizer is b/a. But first check if a's too
|
|
|
1666 * small, since we may be close to constant.
|
|
|
1667 */
|
|
|
1668 if ((a<=0) | (b<x0*a) | (b>x2*a))
|
|
|
1669 { /* split the larger halve */
|
|
|
1670 xnew = ((x2-x1) > (x1-x0)) ? (x1+x2)/2 : (x0+x1)/2;
|
|
|
1671 }
|
|
|
1672 else
|
|
|
1673 { xnew = b/a;
|
|
|
1674 if (10*xnew < (9*x0+x1)) xnew = (9*x0+x1)/10;
|
|
|
1675 if (10*xnew > (9*x2+x1)) xnew = (9*x2+x1)/10;
|
|
|
1676 if (fabs(xnew-x1) < 0.001*(x2-x0))
|
|
|
1677 {
|
|
|
1678 if ((x2-x1) > (x1-x0))
|
|
|
1679 xnew = (99*x1+x2)/100;
|
|
|
1680 else
|
|
|
1681 xnew = (99*x1+x0)/100;
|
|
|
1682 }
|
|
|
1683 }
|
|
|
1684 ynew = f(xnew);
|
|
|
1685 if (xnew>x1)
|
|
|
1686 { if (ynew >= y1) { x0 = x1; y0 = y1; x1 = xnew; y1 = ynew; }
|
|
|
1687 else { x2 = xnew; y2 = ynew; }
|
|
|
1688 }
|
|
|
1689 else
|
|
|
1690 { if (ynew >= y1) { x2 = x1; y2 = y1; x1 = xnew; y1 = ynew; }
|
|
|
1691 else { x0 = xnew; y0 = ynew; }
|
|
|
1692 }
|
|
|
1693 }
|
|
|
1694 }
|
|
|
1695 return(max_val(x1,y1));
|
|
|
1696 }
|
|
|
1697
|
|
|
1698 double max_nr(F, coef, old_coef, f1, delta, J, p, maxit, tol, err)
|
|
|
1699 double *coef, *old_coef, *f1, *delta, tol;
|
|
|
1700 int (*F)(), p, maxit, *err;
|
|
|
1701 jacobian *J;
|
|
|
1702 { double old_f, f, lambda;
|
|
|
1703 int i, j, fr;
|
|
|
1704 double nc, nd, cut;
|
|
|
1705 int rank;
|
|
|
1706
|
|
|
1707 *err = NR_OK;
|
|
|
1708 J->p = p;
|
|
|
1709 fr = F(coef, &f, f1, J->Z); J->st = JAC_RAW;
|
|
|
1710
|
|
|
1711 for (i=0; i<maxit; i++)
|
|
|
1712 { memcpy(old_coef,coef,p*sizeof(double));
|
|
|
1713 old_f = f;
|
|
|
1714 rank = jacob_solve(J,f1);
|
|
|
1715 memcpy(delta,f1,p*sizeof(double));
|
|
|
1716
|
|
|
1717 if (rank==0) /* NR won't move! */
|
|
|
1718 delta[0] = -f/f1[0];
|
|
|
1719
|
|
|
1720 lambda = 1.0;
|
|
|
1721
|
|
|
1722 nc = innerprod(old_coef,old_coef,p);
|
|
|
1723 nd = innerprod(delta, delta, p);
|
|
|
1724 cut = sqrt(nc/nd);
|
|
|
1725 if (cut>1.0) cut = 1.0;
|
|
|
1726 cut *= 0.0001;
|
|
|
1727 do
|
|
|
1728 { for (j=0; j<p; j++) coef[j] = old_coef[j] + lambda*delta[j];
|
|
|
1729 f = old_f - 1.0;
|
|
|
1730 fr = F(coef, &f, f1, J->Z); J->st = JAC_RAW;
|
|
|
1731 if (fr==NR_BREAK) return(old_f);
|
|
|
1732
|
|
|
1733 lambda = (fr==NR_REDUCE) ? lambda/2 : lambda/10.0;
|
|
|
1734 } while ((lambda>cut) & (f <= old_f - 1.0e-3));
|
|
|
1735
|
|
|
1736 if (f < old_f - 1.0e-3)
|
|
|
1737 { *err = NR_NDIV;
|
|
|
1738 return(f);
|
|
|
1739 }
|
|
|
1740 if (fr==NR_REDUCE) return(f);
|
|
|
1741
|
|
|
1742 if (fabs(f-old_f) < tol) return(f);
|
|
|
1743
|
|
|
1744 }
|
|
|
1745 *err = NR_NCON;
|
|
|
1746 return(f);
|
|
|
1747 }
|
|
|
1748 /*
|
|
|
1749 * Copyright 1996-2006 Catherine Loader.
|
|
|
1750 */
|
|
|
1751 #include <math.h>
|
|
|
1752 #include "mut.h"
|
|
|
1753
|
|
|
1754 /* qr decomposition of X (n*p organized by column).
|
|
|
1755 * Take w for the ride, if not NULL.
|
|
|
1756 */
|
|
|
1757 void qr(X,n,p,w)
|
|
|
1758 double *X, *w;
|
|
|
1759 int n, p;
|
|
|
1760 { int i, j, k, mi;
|
|
|
1761 double c, s, mx, nx, t;
|
|
|
1762
|
|
|
1763 for (j=0; j<p; j++)
|
|
|
1764 { mi = j;
|
|
|
1765 mx = fabs(X[(n+1)*j]);
|
|
|
1766 nx = mx*mx;
|
|
|
1767
|
|
|
1768 /* find the largest remaining element in j'th column, row mi.
|
|
|
1769 * flip that row with row j.
|
|
|
1770 */
|
|
|
1771 for (i=j+1; i<n; i++)
|
|
|
1772 { nx += X[j*n+i]*X[j*n+i];
|
|
|
1773 if (fabs(X[j*n+i])>mx)
|
|
|
1774 { mi = i;
|
|
|
1775 mx = fabs(X[j*n+i]);
|
|
|
1776 }
|
|
|
1777 }
|
|
|
1778 for (i=j; i<p; i++)
|
|
|
1779 { t = X[i*n+j];
|
|
|
1780 X[i*n+j] = X[i*n+mi];
|
|
|
1781 X[i*n+mi] = t;
|
|
|
1782 }
|
|
|
1783 if (w!=NULL) { t = w[j]; w[j] = w[mi]; w[mi] = t; }
|
|
|
1784
|
|
|
1785 /* want the diag. element -ve, so we do the `good' Householder reflect.
|
|
|
1786 */
|
|
|
1787 if (X[(n+1)*j]>0)
|
|
|
1788 { for (i=j; i<p; i++) X[i*n+j] = -X[i*n+j];
|
|
|
1789 if (w!=NULL) w[j] = -w[j];
|
|
|
1790 }
|
|
|
1791
|
|
|
1792 nx = sqrt(nx);
|
|
|
1793 c = nx*(nx-X[(n+1)*j]);
|
|
|
1794 if (c!=0)
|
|
|
1795 { for (i=j+1; i<p; i++)
|
|
|
1796 { s = 0;
|
|
|
1797 for (k=j; k<n; k++)
|
|
|
1798 s += X[i*n+k]*X[j*n+k];
|
|
|
1799 s = (s-nx*X[i*n+j])/c;
|
|
|
1800 for (k=j; k<n; k++)
|
|
|
1801 X[i*n+k] -= s*X[j*n+k];
|
|
|
1802 X[i*n+j] += s*nx;
|
|
|
1803 }
|
|
|
1804 if (w != NULL)
|
|
|
1805 { s = 0;
|
|
|
1806 for (k=j; k<n; k++)
|
|
|
1807 s += w[k]*X[n*j+k];
|
|
|
1808 s = (s-nx*w[j])/c;
|
|
|
1809 for (k=j; k<n; k++)
|
|
|
1810 w[k] -= s*X[n*j+k];
|
|
|
1811 w[j] += s*nx;
|
|
|
1812 }
|
|
|
1813 X[j*n+j] = nx;
|
|
|
1814 }
|
|
|
1815 }
|
|
|
1816 }
|
|
|
1817
|
|
|
1818 void qrinvx(R,x,n,p)
|
|
|
1819 double *R, *x;
|
|
|
1820 int n, p;
|
|
|
1821 { int i, j;
|
|
|
1822 for (i=p-1; i>=0; i--)
|
|
|
1823 { for (j=i+1; j<p; j++) x[i] -= R[j*n+i]*x[j];
|
|
|
1824 x[i] /= R[i*n+i];
|
|
|
1825 }
|
|
|
1826 }
|
|
|
1827
|
|
|
1828 void qrtinvx(R,x,n,p)
|
|
|
1829 double *R, *x;
|
|
|
1830 int n, p;
|
|
|
1831 { int i, j;
|
|
|
1832 for (i=0; i<p; i++)
|
|
|
1833 { for (j=0; j<i; j++) x[i] -= R[i*n+j]*x[j];
|
|
|
1834 x[i] /= R[i*n+i];
|
|
|
1835 }
|
|
|
1836 }
|
|
|
1837
|
|
|
1838 void qrsolv(R,x,n,p)
|
|
|
1839 double *R, *x;
|
|
|
1840 int n, p;
|
|
|
1841 { qrtinvx(R,x,n,p);
|
|
|
1842 qrinvx(R,x,n,p);
|
|
|
1843 }
|
|
|
1844 /*
|
|
|
1845 * Copyright 1996-2006 Catherine Loader.
|
|
|
1846 */
|
|
|
1847 /*
|
|
|
1848 * solve f(x)=c by various methods, with varying stability etc...
|
|
|
1849 * xlo and xhi should be initial bounds for the solution.
|
|
|
1850 * convergence criterion is |f(x)-c| < tol.
|
|
|
1851 *
|
|
|
1852 * double solve_bisect(f,c,xmin,xmax,tol,bd_flag,err)
|
|
|
1853 * double solve_secant(f,c,xmin,xmax,tol,bd_flag,err)
|
|
|
1854 * Bisection and secant methods for solving of f(x)=c.
|
|
|
1855 * xmin and xmax are starting values and bound for solution.
|
|
|
1856 * tol = convergence criterion, |f(x)-c| < tol.
|
|
|
1857 * bd_flag = if (xmin,xmax) doesn't bound a solution, what action to take?
|
|
|
1858 * BDF_NONE returns error.
|
|
|
1859 * BDF_EXPRIGHT increases xmax.
|
|
|
1860 * BDF_EXPLEFT decreases xmin.
|
|
|
1861 * err = error flag.
|
|
|
1862 * The (xmin,xmax) bound is not formally necessary for the secant method.
|
|
|
1863 * But having such a bound vastly improves stability; the code performs
|
|
|
1864 * a bisection step whenever the iterations run outside the bounds.
|
|
|
1865 *
|
|
|
1866 * double solve_nr(f,f1,c,x0,tol,err)
|
|
|
1867 * Newton-Raphson solution of f(x)=c.
|
|
|
1868 * f1 = f'(x).
|
|
|
1869 * x0 = starting value.
|
|
|
1870 * tol = convergence criteria, |f(x)-c| < tol.
|
|
|
1871 * err = error flag.
|
|
|
1872 * No stability checks at present.
|
|
|
1873 *
|
|
|
1874 * double solve_fp(f,x0,tol)
|
|
|
1875 * fixed-point iteration to solve f(x)=x.
|
|
|
1876 * x0 = starting value.
|
|
|
1877 * tol = convergence criteria, stops when |f(x)-x| < tol.
|
|
|
1878 * Convergence requires |f'(x)|<1 in neighborhood of true solution;
|
|
|
1879 * f'(x) \approx 0 gives the fastest convergence.
|
|
|
1880 * No stability checks at present.
|
|
|
1881 *
|
|
|
1882 * TODO: additional error checking, non-convergence stop.
|
|
|
1883 */
|
|
|
1884
|
|
|
1885 #include <math.h>
|
|
|
1886 #include "mut.h"
|
|
|
1887
|
|
|
1888 typedef struct {
|
|
|
1889 double xmin, xmax, x0, x1;
|
|
|
1890 double ymin, ymax, y0, y1;
|
|
|
1891 } solvest;
|
|
|
1892
|
|
|
1893 int step_expand(f,c,sv,bd_flag)
|
|
|
1894 double (*f)(), c;
|
|
|
1895 solvest *sv;
|
|
|
1896 int bd_flag;
|
|
|
1897 { double x, y;
|
|
|
1898 if (sv->ymin*sv->ymax <= 0.0) return(0);
|
|
|
1899 if (bd_flag == BDF_NONE)
|
|
|
1900 { mut_printf("invalid bracket\n");
|
|
|
1901 return(1); /* error */
|
|
|
1902 }
|
|
|
1903 if (bd_flag == BDF_EXPRIGHT)
|
|
|
1904 { while (sv->ymin*sv->ymax > 0)
|
|
|
1905 { x = sv->xmax + 2*(sv->xmax-sv->xmin);
|
|
|
1906 y = f(x) - c;
|
|
|
1907 sv->xmin = sv->xmax; sv->xmax = x;
|
|
|
1908 sv->ymin = sv->ymax; sv->ymax = y;
|
|
|
1909 }
|
|
|
1910 return(0);
|
|
|
1911 }
|
|
|
1912 if (bd_flag == BDF_EXPLEFT)
|
|
|
1913 { while (sv->ymin*sv->ymax > 0)
|
|
|
1914 { x = sv->xmin - 2*(sv->xmax-sv->xmin);
|
|
|
1915 y = f(x) - c;
|
|
|
1916 sv->xmax = sv->xmin; sv->xmin = x;
|
|
|
1917 sv->ymax = sv->ymin; sv->ymin = y;
|
|
|
1918 }
|
|
|
1919 return(0);
|
|
|
1920 }
|
|
|
1921 mut_printf("step_expand: unknown bd_flag %d.\n",bd_flag);
|
|
|
1922 return(1);
|
|
|
1923 }
|
|
|
1924
|
|
|
1925 int step_addin(sv,x,y)
|
|
|
1926 solvest *sv;
|
|
|
1927 double x, y;
|
|
|
1928 { sv->x1 = sv->x0; sv->x0 = x;
|
|
|
1929 sv->y1 = sv->y0; sv->y0 = y;
|
|
|
1930 if (y*sv->ymin > 0)
|
|
|
1931 { sv->xmin = x;
|
|
|
1932 sv->ymin = y;
|
|
|
1933 return(0);
|
|
|
1934 }
|
|
|
1935 if (y*sv->ymax > 0)
|
|
|
1936 { sv->xmax = x;
|
|
|
1937 sv->ymax = y;
|
|
|
1938 return(0);
|
|
|
1939 }
|
|
|
1940 if (y==0)
|
|
|
1941 { sv->xmin = sv->xmax = x;
|
|
|
1942 sv->ymin = sv->ymax = 0;
|
|
|
1943 return(0);
|
|
|
1944 }
|
|
|
1945 return(1);
|
|
|
1946 }
|
|
|
1947
|
|
|
1948 int step_bisect(f,c,sv)
|
|
|
1949 double (*f)(), c;
|
|
|
1950 solvest *sv;
|
|
|
1951 { double x, y;
|
|
|
1952 x = sv->x0 = (sv->xmin + sv->xmax)/2;
|
|
|
1953 y = sv->y0 = f(x)-c;
|
|
|
1954 return(step_addin(sv,x,y));
|
|
|
1955 }
|
|
|
1956
|
|
|
1957 double solve_bisect(f,c,xmin,xmax,tol,bd_flag,err)
|
|
|
1958 double (*f)(), c, xmin, xmax, tol;
|
|
|
1959 int bd_flag, *err;
|
|
|
1960 { solvest sv;
|
|
|
1961 int z;
|
|
|
1962 *err = 0;
|
|
|
1963 sv.xmin = xmin; sv.ymin = f(xmin)-c;
|
|
|
1964 sv.xmax = xmax; sv.ymax = f(xmax)-c;
|
|
|
1965 *err = step_expand(f,c,&sv,bd_flag);
|
|
|
1966 if (*err>0) return(sv.xmin);
|
|
|
1967 while(1) /* infinite loop if f is discontinuous */
|
|
|
1968 { z = step_bisect(f,c,&sv);
|
|
|
1969 if (z)
|
|
|
1970 { *err = 1;
|
|
|
1971 return(sv.x0);
|
|
|
1972 }
|
|
|
1973 if (fabs(sv.y0)<tol) return(sv.x0);
|
|
|
1974 }
|
|
|
1975 }
|
|
|
1976
|
|
|
1977 int step_secant(f,c,sv)
|
|
|
1978 double (*f)(), c;
|
|
|
1979 solvest *sv;
|
|
|
1980 { double x, y;
|
|
|
1981 if (sv->y0==sv->y1) return(step_bisect(f,c,sv));
|
|
|
1982 x = sv->x0 + (sv->x1-sv->x0)*sv->y0/(sv->y0-sv->y1);
|
|
|
1983 if ((x<=sv->xmin) | (x>=sv->xmax)) return(step_bisect(f,c,sv));
|
|
|
1984 y = f(x)-c;
|
|
|
1985 return(step_addin(sv,x,y));
|
|
|
1986 }
|
|
|
1987
|
|
|
1988 double solve_secant(f,c,xmin,xmax,tol,bd_flag,err)
|
|
|
1989 double (*f)(), c, xmin, xmax, tol;
|
|
|
1990 int bd_flag, *err;
|
|
|
1991 { solvest sv;
|
|
|
1992 int z;
|
|
|
1993 *err = 0;
|
|
|
1994 sv.xmin = xmin; sv.ymin = f(xmin)-c;
|
|
|
1995 sv.xmax = xmax; sv.ymax = f(xmax)-c;
|
|
|
1996 *err = step_expand(f,c,&sv,bd_flag);
|
|
|
1997 if (*err>0) return(sv.xmin);
|
|
|
1998 sv.x0 = sv.xmin; sv.y0 = sv.ymin;
|
|
|
1999 sv.x1 = sv.xmax; sv.y1 = sv.ymax;
|
|
|
2000 while(1) /* infinite loop if f is discontinuous */
|
|
|
2001 { z = step_secant(f,c,&sv);
|
|
|
2002 if (z)
|
|
|
2003 { *err = 1;
|
|
|
2004 return(sv.x0);
|
|
|
2005 }
|
|
|
2006 if (fabs(sv.y0)<tol) return(sv.x0);
|
|
|
2007 }
|
|
|
2008 }
|
|
|
2009
|
|
|
2010 double solve_nr(f,f1,c,x0,tol,err)
|
|
|
2011 double (*f)(), (*f1)(), c, x0, tol;
|
|
|
2012 int *err;
|
|
|
2013 { double y;
|
|
|
2014 do
|
|
|
2015 { y = f(x0)-c;
|
|
|
2016 x0 -= y/f1(x0);
|
|
|
2017 } while (fabs(y)>tol);
|
|
|
2018 return(x0);
|
|
|
2019 }
|
|
|
2020
|
|
|
2021 double solve_fp(f,x0,tol,maxit)
|
|
|
2022 double (*f)(), x0, tol;
|
|
|
2023 int maxit;
|
|
|
2024 { double x1;
|
|
|
2025 int i;
|
|
|
2026 for (i=0; i<maxit; i++)
|
|
|
2027 { x1 = f(x0);
|
|
|
2028 if (fabs(x1-x0)<tol) return(x1);
|
|
|
2029 x0 = x1;
|
|
|
2030 }
|
|
|
2031 return(x1); /* although it hasn't converged */
|
|
|
2032 }
|
|
|
2033 /*
|
|
|
2034 * Copyright 1996-2006 Catherine Loader.
|
|
|
2035 */
|
|
|
2036 #include "mut.h"
|
|
|
2037
|
|
|
2038 void svd(x,p,q,d,mxit) /* svd of square matrix */
|
|
|
2039 double *x, *p, *q;
|
|
|
2040 int d, mxit;
|
|
|
2041 { int i, j, k, iter, ms, zer;
|
|
|
2042 double r, u, v, cp, cm, sp, sm, c1, c2, s1, s2, mx;
|
|
|
2043 for (i=0; i<d; i++)
|
|
|
2044 for (j=0; j<d; j++) p[i*d+j] = q[i*d+j] = (i==j);
|
|
|
2045 for (iter=0; iter<mxit; iter++)
|
|
|
2046 { ms = 0;
|
|
|
2047 for (i=0; i<d; i++)
|
|
|
2048 for (j=i+1; j<d; j++)
|
|
|
2049 { s1 = fabs(x[i*d+j]);
|
|
|
2050 s2 = fabs(x[j*d+i]);
|
|
|
2051 mx = (s1>s2) ? s1 : s2;
|
|
|
2052 zer = 1;
|
|
|
2053 if (mx*mx>1.0e-15*fabs(x[i*d+i]*x[j*d+j]))
|
|
|
2054 { if (fabs(x[i*(d+1)])<fabs(x[j*(d+1)]))
|
|
|
2055 { for (k=0; k<d; k++)
|
|
|
2056 { u = x[i*d+k]; x[i*d+k] = x[j*d+k]; x[j*d+k] = u;
|
|
|
2057 u = p[k*d+i]; p[k*d+i] = p[k*d+j]; p[k*d+j] = u;
|
|
|
2058 }
|
|
|
2059 for (k=0; k<d; k++)
|
|
|
2060 { u = x[k*d+i]; x[k*d+i] = x[k*d+j]; x[k*d+j] = u;
|
|
|
2061 u = q[k*d+i]; q[k*d+i] = q[k*d+j]; q[k*d+j] = u;
|
|
|
2062 }
|
|
|
2063 }
|
|
|
2064 cp = x[i*(d+1)]+x[j*(d+1)];
|
|
|
2065 sp = x[j*d+i]-x[i*d+j];
|
|
|
2066 r = sqrt(cp*cp+sp*sp);
|
|
|
2067 if (r>0) { cp /= r; sp /= r; }
|
|
|
2068 else { cp = 1.0; zer = 0;}
|
|
|
2069 cm = x[i*(d+1)]-x[j*(d+1)];
|
|
|
2070 sm = x[i*d+j]+x[j*d+i];
|
|
|
2071 r = sqrt(cm*cm+sm*sm);
|
|
|
2072 if (r>0) { cm /= r; sm /= r; }
|
|
|
2073 else { cm = 1.0; zer = 0;}
|
|
|
2074 c1 = cm+cp;
|
|
|
2075 s1 = sm+sp;
|
|
|
2076 r = sqrt(c1*c1+s1*s1);
|
|
|
2077 if (r>0) { c1 /= r; s1 /= r; }
|
|
|
2078 else { c1 = 1.0; zer = 0;}
|
|
|
2079 if (fabs(s1)>ms) ms = fabs(s1);
|
|
|
2080 c2 = cm+cp;
|
|
|
2081 s2 = sp-sm;
|
|
|
2082 r = sqrt(c2*c2+s2*s2);
|
|
|
2083 if (r>0) { c2 /= r; s2 /= r; }
|
|
|
2084 else { c2 = 1.0; zer = 0;}
|
|
|
2085 for (k=0; k<d; k++)
|
|
|
2086 { u = x[i*d+k]; v = x[j*d+k];
|
|
|
2087 x[i*d+k] = c1*u+s1*v;
|
|
|
2088 x[j*d+k] = c1*v-s1*u;
|
|
|
2089 u = p[k*d+i]; v = p[k*d+j];
|
|
|
2090 p[k*d+i] = c1*u+s1*v;
|
|
|
2091 p[k*d+j] = c1*v-s1*u;
|
|
|
2092 }
|
|
|
2093 for (k=0; k<d; k++)
|
|
|
2094 { u = x[k*d+i]; v = x[k*d+j];
|
|
|
2095 x[k*d+i] = c2*u-s2*v;
|
|
|
2096 x[k*d+j] = s2*u+c2*v;
|
|
|
2097 u = q[k*d+i]; v = q[k*d+j];
|
|
|
2098 q[k*d+i] = c2*u-s2*v;
|
|
|
2099 q[k*d+j] = s2*u+c2*v;
|
|
|
2100 }
|
|
|
2101 if (zer) x[i*d+j] = x[j*d+i] = 0.0;
|
|
|
2102 ms = 1;
|
|
|
2103 }
|
|
|
2104 }
|
|
|
2105 if (ms==0) iter=mxit+10;
|
|
|
2106 }
|
|
|
2107 if (iter==mxit) mut_printf("Warning: svd not converged.\n");
|
|
|
2108 for (i=0; i<d; i++)
|
|
|
2109 if (x[i*d+i]<0)
|
|
|
2110 { x[i*d+i] = -x[i*d+i];
|
|
|
2111 for (j=0; j<d; j++) p[j*d+i] = -p[j*d+i];
|
|
|
2112 }
|
|
|
2113 }
|
|
|
2114
|
|
|
2115 int svdsolve(x,w,P,D,Q,d,tol) /* original X = PDQ^T; comp. QD^{-1}P^T x */
|
|
|
2116 double *x, *w, *P, *D, *Q, tol;
|
|
|
2117 int d;
|
|
|
2118 { int i, j, rank;
|
|
|
2119 double mx;
|
|
|
2120 if (tol>0)
|
|
|
2121 { mx = D[0];
|
|
|
2122 for (i=1; i<d; i++) if (D[i*(d+1)]>mx) mx = D[i*(d+1)];
|
|
|
2123 tol *= mx;
|
|
|
2124 }
|
|
|
2125 rank = 0;
|
|
|
2126 for (i=0; i<d; i++)
|
|
|
2127 { w[i] = 0.0;
|
|
|
2128 for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j];
|
|
|
2129 }
|
|
|
2130 for (i=0; i<d; i++)
|
|
|
2131 if (D[i*d+i]>tol)
|
|
|
2132 { w[i] /= D[i*(d+1)];
|
|
|
2133 rank++;
|
|
|
2134 }
|
|
|
2135 for (i=0; i<d; i++)
|
|
|
2136 { x[i] = 0.0;
|
|
|
2137 for (j=0; j<d; j++) x[i] += Q[i*d+j]*w[j];
|
|
|
2138 }
|
|
|
2139 return(rank);
|
|
|
2140 }
|
|
|
2141
|
|
|
2142 void hsvdsolve(x,w,P,D,Q,d,tol) /* original X = PDQ^T; comp. D^{-1/2}P^T x */
|
|
|
2143 double *x, *w, *P, *D, *Q, tol;
|
|
|
2144 int d;
|
|
|
2145 { int i, j;
|
|
|
2146 double mx;
|
|
|
2147 if (tol>0)
|
|
|
2148 { mx = D[0];
|
|
|
2149 for (i=1; i<d; i++) if (D[i*(d+1)]>mx) mx = D[i*(d+1)];
|
|
|
2150 tol *= mx;
|
|
|
2151 }
|
|
|
2152 for (i=0; i<d; i++)
|
|
|
2153 { w[i] = 0.0;
|
|
|
2154 for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j];
|
|
|
2155 }
|
|
|
2156 for (i=0; i<d; i++) if (D[i*d+i]>tol) w[i] /= sqrt(D[i*(d+1)]);
|
|
|
2157 for (i=0; i<d; i++) x[i] = w[i];
|
|
|
2158 }
|
|
|
2159 /*
|
|
|
2160 * Copyright 1996-2006 Catherine Loader.
|
|
|
2161 */
|
|
|
2162 /*
|
|
|
2163 * Includes some miscellaneous vector functions:
|
|
|
2164 * setzero(v,p) sets all elements of v to 0.
|
|
|
2165 * unitvec(x,k,p) sets x to k'th unit vector e_k.
|
|
|
2166 * innerprod(v1,v2,p) inner product.
|
|
|
2167 * addouter(A,v1,v2,p,c) A <- A + c * v_1 v2^T
|
|
|
2168 * multmatscal(A,z,n) A <- A*z
|
|
|
2169 * matrixmultiply(A,B,C,m,n,p) C(m*p) <- A(m*n) * B(n*p)
|
|
|
2170 * transpose(x,m,n) inline transpose
|
|
|
2171 * m_trace(x,n) trace
|
|
|
2172 * vecsum(x,n) sum elements.
|
|
|
2173 */
|
|
|
2174
|
|
|
2175 #include "mut.h"
|
|
|
2176
|
|
|
2177 void setzero(v,p)
|
|
|
2178 double *v;
|
|
|
2179 int p;
|
|
|
2180 { int i;
|
|
|
2181 for (i=0; i<p; i++) v[i] = 0.0;
|
|
|
2182 }
|
|
|
2183
|
|
|
2184 void unitvec(x,k,p)
|
|
|
2185 double *x;
|
|
|
2186 int k, p;
|
|
|
2187 { setzero(x,p);
|
|
|
2188 x[k] = 1.0;
|
|
|
2189 }
|
|
|
2190
|
|
|
2191 double innerprod(v1,v2,p)
|
|
|
2192 double *v1, *v2;
|
|
|
2193 int p;
|
|
|
2194 { int i;
|
|
|
2195 double s;
|
|
|
2196 s = 0;
|
|
|
2197 for (i=0; i<p; i++) s += v1[i]*v2[i];
|
|
|
2198 return(s);
|
|
|
2199 }
|
|
|
2200
|
|
|
2201 void addouter(A,v1,v2,p,c)
|
|
|
2202 double *A, *v1, *v2, c;
|
|
|
2203 int p;
|
|
|
2204 { int i, j;
|
|
|
2205 for (i=0; i<p; i++)
|
|
|
2206 for (j=0; j<p; j++)
|
|
|
2207 A[i*p+j] += c*v1[i]*v2[j];
|
|
|
2208 }
|
|
|
2209
|
|
|
2210 void multmatscal(A,z,n)
|
|
|
2211 double *A, z;
|
|
|
2212 int n;
|
|
|
2213 { int i;
|
|
|
2214 for (i=0; i<n; i++) A[i] *= z;
|
|
|
2215 }
|
|
|
2216
|
|
|
2217 /* matrix multiply A (m*n) times B (n*p).
|
|
|
2218 * store in C (m*p).
|
|
|
2219 * all matrices stored by column.
|
|
|
2220 */
|
|
|
2221 void matrixmultiply(A,B,C,m,n,p)
|
|
|
2222 double *A, *B, *C;
|
|
|
2223 int m, n, p;
|
|
|
2224 { int i, j, k, ij;
|
|
|
2225 for (i=0; i<m; i++)
|
|
|
2226 for (j=0; j<p; j++)
|
|
|
2227 { ij = j*m+i;
|
|
|
2228 C[ij] = 0.0;
|
|
|
2229 for (k=0; k<n; k++)
|
|
|
2230 C[ij] += A[k*m+i] * B[j*n+k];
|
|
|
2231 }
|
|
|
2232 }
|
|
|
2233
|
|
|
2234 /*
|
|
|
2235 * transpose() transposes an m*n matrix in place.
|
|
|
2236 * At input, the matrix has n rows, m columns and
|
|
|
2237 * x[0..n-1] is the is the first column.
|
|
|
2238 * At output, the matrix has m rows, n columns and
|
|
|
2239 * x[0..m-1] is the first column.
|
|
|
2240 */
|
|
|
2241 void transpose(x,m,n)
|
|
|
2242 double *x;
|
|
|
2243 int m, n;
|
|
|
2244 { int t0, t, ti, tj;
|
|
|
2245 double z;
|
|
|
2246 for (t0=1; t0<m*n-2; t0++)
|
|
|
2247 { ti = t0%m; tj = t0/m;
|
|
|
2248 do
|
|
|
2249 { t = ti*n+tj;
|
|
|
2250 ti= t%m;
|
|
|
2251 tj= t/m;
|
|
|
2252 } while (t<t0);
|
|
|
2253 z = x[t];
|
|
|
2254 x[t] = x[t0];
|
|
|
2255 x[t0] = z;
|
|
|
2256 }
|
|
|
2257 }
|
|
|
2258
|
|
|
2259 /* trace of an n*n square matrix. */
|
|
|
2260 double m_trace(x,n)
|
|
|
2261 double *x;
|
|
|
2262 int n;
|
|
|
2263 { int i;
|
|
|
2264 double sum;
|
|
|
2265 sum = 0;
|
|
|
2266 for (i=0; i<n; i++)
|
|
|
2267 sum += x[i*(n+1)];
|
|
|
2268 return(sum);
|
|
|
2269 }
|
|
|
2270
|
|
|
2271 double vecsum(v,n)
|
|
|
2272 double *v;
|
|
|
2273 int n;
|
|
|
2274 { int i;
|
|
|
2275 double sum;
|
|
|
2276 sum = 0.0;
|
|
|
2277 for (i=0; i<n; i++) sum += v[i];
|
|
|
2278 return(sum);
|
|
|
2279 }
|
|
|
2280 /*
|
|
|
2281 * Copyright 1996-2006 Catherine Loader.
|
|
|
2282 */
|
|
|
2283 /*
|
|
|
2284 miscellaneous functions that may not be defined in the math
|
|
|
2285 libraries. The implementations are crude.
|
|
|
2286 mut_daws(x) -- dawson's function
|
|
|
2287 mut_exp(x) -- exp(x), but it won't overflow.
|
|
|
2288
|
|
|
2289 where required, these must be #define'd in header files.
|
|
|
2290
|
|
|
2291 also includes
|
|
|
2292 ptail(x) -- exp(x*x/2)*int_{-\infty}^x exp(-u^2/2)du for x < -6.
|
|
|
2293 logit(x) -- logistic function.
|
|
|
2294 expit(x) -- inverse of logit.
|
|
|
2295 factorial(n)-- factorial
|
|
|
2296 */
|
|
|
2297
|
|
|
2298 #include "mut.h"
|
|
|
2299
|
|
|
2300 double mut_exp(x)
|
|
|
2301 double x;
|
|
|
2302 { if (x>700.0) return(1.014232054735004e+304);
|
|
|
2303 return(exp(x));
|
|
|
2304 }
|
|
|
2305
|
|
|
2306 double mut_daws(x)
|
|
|
2307 double x;
|
|
|
2308 { static double val[] = {
|
|
|
2309 0, 0.24485619356002, 0.46034428261948, 0.62399959848185, 0.72477845900708,
|
|
|
2310 0.76388186132749, 0.75213621001998, 0.70541701910853, 0.63998807456541,
|
|
|
2311 0.56917098836654, 0.50187821196415, 0.44274283060424, 0.39316687916687,
|
|
|
2312 0.35260646480842, 0.31964847250685, 0.29271122077502, 0.27039629581340,
|
|
|
2313 0.25160207761769, 0.23551176224443, 0.22153505358518, 0.20924575719548,
|
|
|
2314 0.19833146819662, 0.18855782729305, 0.17974461154688, 0.17175005072385 };
|
|
|
2315 double h, f0, f1, f2, y, z, xx;
|
|
|
2316 int j, m;
|
|
|
2317 if (x<0) return(-mut_daws(-x));
|
|
|
2318 if (x>6)
|
|
|
2319 { /* Tail series: 1/x + 1/x^3 + 1.3/x^5 + 1.3.5/x^7 + ... */
|
|
|
2320 y = z = 1/x;
|
|
|
2321 j = 0;
|
|
|
2322 while (((f0=(2*j+1)/(x*x))<1) && (y>1.0e-10*z))
|
|
|
2323 { y *= f0;
|
|
|
2324 z += y;
|
|
|
2325 j++;
|
|
|
2326 }
|
|
|
2327 return(z);
|
|
|
2328 }
|
|
|
2329 m = (int) (4*x);
|
|
|
2330 h = x-0.25*m;
|
|
|
2331 if (h>0.125)
|
|
|
2332 { m++;
|
|
|
2333 h = h-0.25;
|
|
|
2334 }
|
|
|
2335 xx = 0.25*m;
|
|
|
2336 f0 = val[m];
|
|
|
2337 f1 = 1-xx*f0;
|
|
|
2338 z = f0+h*f1;
|
|
|
2339 y = h;
|
|
|
2340 j = 2;
|
|
|
2341 while (fabs(y)>z*1.0e-10)
|
|
|
2342 { f2 = -(j-1)*f0-xx*f1;
|
|
|
2343 y *= h/j;
|
|
|
2344 z += y*f2;
|
|
|
2345 f0 = f1; f1 = f2;
|
|
|
2346 j++;
|
|
|
2347 }
|
|
|
2348 return(z);
|
|
|
2349 }
|
|
|
2350
|
|
|
2351 double ptail(x) /* exp(x*x/2)*int_{-\infty}^x exp(-u^2/2)du for x < -6 */
|
|
|
2352 double x;
|
|
|
2353 { double y, z, f0;
|
|
|
2354 int j;
|
|
|
2355 y = z = -1.0/x;
|
|
|
2356 j = 0;
|
|
|
2357 while ((fabs(f0= -(2*j+1)/(x*x))<1) && (fabs(y)>1.0e-10*z))
|
|
|
2358 { y *= f0;
|
|
|
2359 z += y;
|
|
|
2360 j++;
|
|
|
2361 }
|
|
|
2362 return(z);
|
|
|
2363 }
|
|
|
2364
|
|
|
2365 double logit(x)
|
|
|
2366 double x;
|
|
|
2367 { return(log(x/(1-x)));
|
|
|
2368 }
|
|
|
2369
|
|
|
2370 double expit(x)
|
|
|
2371 double x;
|
|
|
2372 { double u;
|
|
|
2373 if (x<0)
|
|
|
2374 { u = exp(x);
|
|
|
2375 return(u/(1+u));
|
|
|
2376 }
|
|
|
2377 return(1/(1+exp(-x)));
|
|
|
2378 }
|
|
|
2379
|
|
|
2380 int factorial(n)
|
|
|
2381 int n;
|
|
|
2382 { if (n<=1) return(1.0);
|
|
|
2383 return(n*factorial(n-1));
|
|
|
2384 }
|
|
|
2385 /*
|
|
|
2386 * Copyright 1996-2006 Catherine Loader.
|
|
|
2387 */
|
|
|
2388 /*
|
|
|
2389 * Constrained maximization of a bivariate function.
|
|
|
2390 * maxbvgrid(f,x,ll,ur,m0,m1)
|
|
|
2391 * maximizes over a grid of m0*m1 points. Returns the maximum,
|
|
|
2392 * and the maximizer through the array x. Usually this is a starter,
|
|
|
2393 * to choose between local maxima, followed by other routines to refine.
|
|
|
2394 *
|
|
|
2395 * maxbvstep(f,x,ymax,h,ll,ur,err)
|
|
|
2396 * essentially multivariate bisection. A 3x3 grid of points is
|
|
|
2397 * built around the starting value (x,ymax). This grid is moved
|
|
|
2398 * around (step size h[0] and h[1] in the two dimensions) until
|
|
|
2399 * the maximum is in the middle. Then, the step size is halved.
|
|
|
2400 * Usually, this will be called in a loop.
|
|
|
2401 * The error flag is set if the maximum can't be centered in a
|
|
|
2402 * reasonable number of steps.
|
|
|
2403 *
|
|
|
2404 * maxbv(f,x,h,ll,ur,m0,m1,tol)
|
|
|
2405 * combines the two previous functions. It begins with a grid search
|
|
|
2406 * (if m0>0 and m1>0), followed by refinement. Refines until both h
|
|
|
2407 * components are < tol.
|
|
|
2408 */
|
|
|
2409 #include "mut.h"
|
|
|
2410
|
|
|
2411 #define max(a,b) ((a)>(b) ? (a) : (b))
|
|
|
2412 #define min(a,b) ((a)<(b) ? (a) : (b))
|
|
|
2413
|
|
|
2414 double maxbvgrid(f,x,ll,ur,m0,m1,con)
|
|
|
2415 double (*f)(), *x, *ll, *ur;
|
|
|
2416 int m0, m1, *con;
|
|
|
2417 { int i, j, im, jm;
|
|
|
2418 double y, ymax;
|
|
|
2419
|
|
|
2420 im = -1;
|
|
|
2421 for (i=0; i<=m0; i++)
|
|
|
2422 { x[0] = ((m0-i)*ll[0] + i*ur[0])/m0;
|
|
|
2423 for (j=0; j<=m1; j++)
|
|
|
2424 { x[1] = ((m1-j)*ll[1] + j*ur[1])/m1;
|
|
|
2425 y = f(x);
|
|
|
2426 if ((im==-1) || (y>ymax))
|
|
|
2427 { im = i; jm = j;
|
|
|
2428 ymax = y;
|
|
|
2429 }
|
|
|
2430 }
|
|
|
2431 }
|
|
|
2432
|
|
|
2433 x[0] = ((m0-im)*ll[0] + im*ur[0])/m0;
|
|
|
2434 x[1] = ((m1-jm)*ll[1] + jm*ur[1])/m1;
|
|
|
2435 con[0] = (im==m0)-(im==0);
|
|
|
2436 con[1] = (jm==m1)-(jm==0);
|
|
|
2437 return(ymax);
|
|
|
2438 }
|
|
|
2439
|
|
|
2440 double maxbvstep(f,x,ymax,h,ll,ur,err,con)
|
|
|
2441 double (*f)(), *x, ymax, *h, *ll, *ur;
|
|
|
2442 int *err, *con;
|
|
|
2443 { int i, j, ij, imax, steps, cts[2];
|
|
|
2444 double newx, X[9][2], y[9];
|
|
|
2445
|
|
|
2446 imax =4; y[4] = ymax;
|
|
|
2447
|
|
|
2448 for (i=(con[0]==-1)-1; i<2-(con[0]==1); i++)
|
|
|
2449 for (j=(con[1]==-1)-1; j<2-(con[1]==1); j++)
|
|
|
2450 { ij = 3*i+j+4;
|
|
|
2451 X[ij][0] = x[0]+i*h[0];
|
|
|
2452 if (X[ij][0] < ll[0]+0.001*h[0]) X[ij][0] = ll[0];
|
|
|
2453 if (X[ij][0] > ur[0]-0.001*h[0]) X[ij][0] = ur[0];
|
|
|
2454 X[ij][1] = x[1]+j*h[1];
|
|
|
2455 if (X[ij][1] < ll[1]+0.001*h[1]) X[ij][1] = ll[1];
|
|
|
2456 if (X[ij][1] > ur[1]-0.001*h[1]) X[ij][1] = ur[1];
|
|
|
2457 if (ij != 4)
|
|
|
2458 { y[ij] = f(X[ij]);
|
|
|
2459 if (y[ij]>ymax) { imax = ij; ymax = y[ij]; }
|
|
|
2460 }
|
|
|
2461 }
|
|
|
2462
|
|
|
2463 steps = 0;
|
|
|
2464 cts[0] = cts[1] = 0;
|
|
|
2465 while ((steps<20) && (imax != 4))
|
|
|
2466 { steps++;
|
|
|
2467 if ((con[0]>-1) && ((imax/3)==0)) /* shift right */
|
|
|
2468 {
|
|
|
2469 cts[0]--;
|
|
|
2470 for (i=8; i>2; i--)
|
|
|
2471 { X[i][0] = X[i-3][0]; y[i] = y[i-3];
|
|
|
2472 }
|
|
|
2473 imax = imax+3;
|
|
|
2474 if (X[imax][0]==ll[0])
|
|
|
2475 con[0] = -1;
|
|
|
2476 else
|
|
|
2477 { newx = X[imax][0]-h[0];
|
|
|
2478 if (newx < ll[0]+0.001*h[0]) newx = ll[0];
|
|
|
2479 for (i=(con[1]==-1); i<3-(con[1]==1); i++)
|
|
|
2480 { X[i][0] = newx;
|
|
|
2481 y[i] = f(X[i]);
|
|
|
2482 if (y[i]>ymax) { ymax = y[i]; imax = i; }
|
|
|
2483 }
|
|
|
2484 con[0] = 0;
|
|
|
2485 }
|
|
|
2486 }
|
|
|
2487
|
|
|
2488 if ((con[0]<1) && ((imax/3)==2)) /* shift left */
|
|
|
2489 {
|
|
|
2490 cts[0]++;
|
|
|
2491 for (i=0; i<6; i++)
|
|
|
2492 { X[i][0] = X[i+3][0]; y[i] = y[i+3];
|
|
|
2493 }
|
|
|
2494 imax = imax-3;
|
|
|
2495 if (X[imax][0]==ur[0])
|
|
|
2496 con[0] = 1;
|
|
|
2497 else
|
|
|
2498 { newx = X[imax][0]+h[0];
|
|
|
2499 if (newx > ur[0]-0.001*h[0]) newx = ur[0];
|
|
|
2500 for (i=6+(con[1]==-1); i<9-(con[1]==1); i++)
|
|
|
2501 { X[i][0] = newx;
|
|
|
2502 y[i] = f(X[i]);
|
|
|
2503 if (y[i]>ymax) { ymax = y[i]; imax = i; }
|
|
|
2504 }
|
|
|
2505 con[0] = 0;
|
|
|
2506 }
|
|
|
2507 }
|
|
|
2508
|
|
|
2509 if ((con[1]>-1) && ((imax%3)==0)) /* shift up */
|
|
|
2510 {
|
|
|
2511 cts[1]--;
|
|
|
2512 for (i=9; i>0; i--) if (i%3 > 0)
|
|
|
2513 { X[i][1] = X[i-1][1]; y[i] = y[i-1];
|
|
|
2514 }
|
|
|
2515 imax = imax+1;
|
|
|
2516 if (X[imax][1]==ll[1])
|
|
|
2517 con[1] = -1;
|
|
|
2518 else
|
|
|
2519 { newx = X[imax][1]-h[1];
|
|
|
2520 if (newx < ll[1]+0.001*h[1]) newx = ll[1];
|
|
|
2521 for (i=3*(con[0]==-1); i<7-(con[0]==1); i=i+3)
|
|
|
2522 { X[i][1] = newx;
|
|
|
2523 y[i] = f(X[i]);
|
|
|
2524 if (y[i]>ymax) { ymax = y[i]; imax = i; }
|
|
|
2525 }
|
|
|
2526 con[1] = 0;
|
|
|
2527 }
|
|
|
2528 }
|
|
|
2529
|
|
|
2530 if ((con[1]<1) && ((imax%3)==2)) /* shift down */
|
|
|
2531 {
|
|
|
2532 cts[1]++;
|
|
|
2533 for (i=0; i<9; i++) if (i%3 < 2)
|
|
|
2534 { X[i][1] = X[i+1][1]; y[i] = y[i+1];
|
|
|
2535 }
|
|
|
2536 imax = imax-1;
|
|
|
2537 if (X[imax][1]==ur[1])
|
|
|
2538 con[1] = 1;
|
|
|
2539 else
|
|
|
2540 { newx = X[imax][1]+h[1];
|
|
|
2541 if (newx > ur[1]-0.001*h[1]) newx = ur[1];
|
|
|
2542 for (i=2+3*(con[0]==-1); i<9-(con[0]==1); i=i+3)
|
|
|
2543 { X[i][1] = newx;
|
|
|
2544 y[i] = f(X[i]);
|
|
|
2545 if (y[i]>ymax) { ymax = y[i]; imax = i; }
|
|
|
2546 }
|
|
|
2547 con[1] = 0;
|
|
|
2548 }
|
|
|
2549 }
|
|
|
2550 /* if we've taken 3 steps in one direction, try increasing the
|
|
|
2551 * corresponding h.
|
|
|
2552 */
|
|
|
2553 if ((cts[0]==-2) | (cts[0]==2))
|
|
|
2554 { h[0] = 2*h[0]; cts[0] = 0; }
|
|
|
2555 if ((cts[1]==-2) | (cts[1]==2))
|
|
|
2556 { h[1] = 2*h[1]; cts[1] = 0; }
|
|
|
2557 }
|
|
|
2558
|
|
|
2559 if (steps==40)
|
|
|
2560 *err = 1;
|
|
|
2561 else
|
|
|
2562 {
|
|
|
2563 h[0] /= 2.0; h[1] /= 2.0;
|
|
|
2564 *err = 0;
|
|
|
2565 }
|
|
|
2566
|
|
|
2567 x[0] = X[imax][0];
|
|
|
2568 x[1] = X[imax][1];
|
|
|
2569 return(y[imax]);
|
|
|
2570 }
|
|
|
2571
|
|
|
2572 #define BQMmaxp 5
|
|
|
2573
|
|
|
2574 int boxquadmin(J,b0,p,x0,ll,ur)
|
|
|
2575 jacobian *J;
|
|
|
2576 double *b0, *x0, *ll, *ur;
|
|
|
2577 int p;
|
|
|
2578 { double b[BQMmaxp], x[BQMmaxp], L[BQMmaxp*BQMmaxp], C[BQMmaxp*BQMmaxp], d[BQMmaxp];
|
|
|
2579 double f, fmin;
|
|
|
2580 int i, imin, m, con[BQMmaxp], rlx;
|
|
|
2581
|
|
|
2582 if (p>BQMmaxp) mut_printf("boxquadmin: maxp is 5.\n");
|
|
|
2583 if (J->st != JAC_RAW) mut_printf("boxquadmin: must start with JAC_RAW.\n");
|
|
|
2584
|
|
|
2585 m = 0;
|
|
|
2586 setzero(L,p*p);
|
|
|
2587 setzero(x,p);
|
|
|
2588 memcpy(C,J->Z,p*p*sizeof(double));
|
|
|
2589 for (i=0; i<p; i++) con[i] = 0;
|
|
|
2590
|
|
|
2591 do
|
|
|
2592 {
|
|
|
2593 /* first, keep minimizing and add constraints, one at a time.
|
|
|
2594 */
|
|
|
2595 do
|
|
|
2596 {
|
|
|
2597 matrixmultiply(C,x,b,p,p,1);
|
|
|
2598 for (i=0; i<p; i++) b[i] += b0[i];
|
|
|
2599 conquadmin(J,b,p,L,d,m);
|
|
|
2600 /* if C matrix is not pd, don't even bother.
|
|
|
2601 * this relies on having used cholesky dec.
|
|
|
2602 */
|
|
|
2603 if ((J->Z[0]==0.0) | (J->Z[3]==0.0)) return(1);
|
|
|
2604 fmin = 1.0;
|
|
|
2605 for (i=0; i<p; i++) if (con[i]==0)
|
|
|
2606 { f = 1.0;
|
|
|
2607 if (x0[i]+x[i]+b[i] < ll[i]) f = (ll[i]-x[i]-x0[i])/b[i];
|
|
|
2608 if (x0[i]+x[i]+b[i] > ur[i]) f = (ur[i]-x[i]-x0[i])/b[i];
|
|
|
2609 if (f<fmin) fmin = f;
|
|
|
2610 imin = i;
|
|
|
2611 }
|
|
|
2612 for (i=0; i<p; i++) x[i] += fmin*b[i];
|
|
|
2613 if (fmin<1.0)
|
|
|
2614 { L[m*p+imin] = 1;
|
|
|
2615 m++;
|
|
|
2616 con[imin] = (b[imin]<0) ? -1 : 1;
|
|
|
2617 }
|
|
|
2618 } while ((fmin < 1.0) & (m<p));
|
|
|
2619
|
|
|
2620 /* now, can I relax any constraints?
|
|
|
2621 * compute slopes at current point. Can relax if:
|
|
|
2622 * slope is -ve on a lower boundary.
|
|
|
2623 * slope is +ve on an upper boundary.
|
|
|
2624 */
|
|
|
2625 rlx = 0;
|
|
|
2626 if (m>0)
|
|
|
2627 { matrixmultiply(C,x,b,p,p,1);
|
|
|
2628 for (i=0; i<p; i++) b[i] += b0[i];
|
|
|
2629 for (i=0; i<p; i++)
|
|
|
2630 { if ((con[i]==-1)&& (b[i]<0)) { con[i] = 0; rlx = 1; }
|
|
|
2631 if ((con[i]==1) && (b[i]>0)) { con[i] = 0; rlx = 1; }
|
|
|
2632 }
|
|
|
2633
|
|
|
2634 if (rlx) /* reconstruct the constraint matrix */
|
|
|
2635 { setzero(L,p*p); m = 0;
|
|
|
2636 for (i=0; i<p; i++) if (con[i] != 0)
|
|
|
2637 { L[m*p+i] = 1;
|
|
|
2638 m++;
|
|
|
2639 }
|
|
|
2640 }
|
|
|
2641 }
|
|
|
2642 } while (rlx);
|
|
|
2643
|
|
|
2644 memcpy(b0,x,p*sizeof(double)); /* this is how far we should move from x0 */
|
|
|
2645 return(0);
|
|
|
2646 }
|
|
|
2647
|
|
|
2648 double maxquadstep(f,x,ymax,h,ll,ur,err,con)
|
|
|
2649 double (*f)(), *x, ymax, *h, *ll, *ur;
|
|
|
2650 int *err, *con;
|
|
|
2651 { jacobian J;
|
|
|
2652 double b[2], c[2], d, jwork[12];
|
|
|
2653 double x0, x1, y0, y1, ym, h0, xl[2], xu[2], xi[2];
|
|
|
2654 int i, m;
|
|
|
2655
|
|
|
2656 *err = 0;
|
|
|
2657
|
|
|
2658 /* first, can we relax any of the initial constraints?
|
|
|
2659 * if so, just do one step away from the boundary, and
|
|
|
2660 * return for restart.
|
|
|
2661 */
|
|
|
2662 for (i=0; i<2; i++)
|
|
|
2663 if (con[i] != 0)
|
|
|
2664 { xi[0] = x[0]; xi[1] = x[1];
|
|
|
2665 xi[i] = x[i]-con[i]*h[i];
|
|
|
2666 y0 = f(xi);
|
|
|
2667 if (y0>ymax)
|
|
|
2668 { memcpy(x,xi,2*sizeof(double));
|
|
|
2669 con[i] = 0;
|
|
|
2670 return(y0);
|
|
|
2671 }
|
|
|
2672 }
|
|
|
2673
|
|
|
2674 /* now, all initial constraints remain active.
|
|
|
2675 */
|
|
|
2676
|
|
|
2677 m = 9;
|
|
|
2678 for (i=0; i<2; i++) if (con[i]==0)
|
|
|
2679 { m /= 3;
|
|
|
2680 xl[0] = x[0]; xl[1] = x[1];
|
|
|
2681 xl[i] = max(x[i]-h[i],ll[i]); y0 = f(xl);
|
|
|
2682 x0 = xl[i]-x[i]; y0 -= ymax;
|
|
|
2683 xu[0] = x[0]; xu[1] = x[1];
|
|
|
2684 xu[i] = min(x[i]+h[i],ur[i]); y1 = f(xu);
|
|
|
2685 x1 = xu[i]-x[i]; y1 -= ymax;
|
|
|
2686 if (x0*x1*(x1-x0)==0) { *err = 1; return(0.0); }
|
|
|
2687 b[i] = (x0*x0*y1-x1*x1*y0)/(x0*x1*(x0-x1));
|
|
|
2688 c[i] = 2*(x0*y1-x1*y0)/(x0*x1*(x1-x0));
|
|
|
2689 if (c[i] >= 0.0) { *err = 1; return(0.0); }
|
|
|
2690 xi[i] = (b[i]<0) ? xl[i] : xu[i];
|
|
|
2691 }
|
|
|
2692 else { c[i] = -1.0; b[i] = 0.0; } /* enforce initial constraints */
|
|
|
2693
|
|
|
2694 if ((con[0]==0) && (con[1]==0))
|
|
|
2695 { x0 = xi[0]-x[0];
|
|
|
2696 x1 = xi[1]-x[1];
|
|
|
2697 ym = f(xi) - ymax - b[0]*x0 - c[0]*x0*x0/2 - b[1]*x1 - c[1]*x1*x1/2;
|
|
|
2698 d = ym/(x0*x1);
|
|
|
2699 }
|
|
|
2700 else d = 0.0;
|
|
|
2701
|
|
|
2702 /* now, maximize the quadratic.
|
|
|
2703 * y[4] + b0*x0 + b1*x1 + 0.5(c0*x0*x0 + c1*x1*x1 + 2*d*x0*x1)
|
|
|
2704 * -ve everything, to call quadmin.
|
|
|
2705 */
|
|
|
2706 jac_alloc(&J,2,jwork);
|
|
|
2707 J.Z[0] = -c[0];
|
|
|
2708 J.Z[1] = J.Z[2] = -d;
|
|
|
2709 J.Z[3] = -c[1];
|
|
|
2710 J.st = JAC_RAW;
|
|
|
2711 J.p = 2;
|
|
|
2712 b[0] = -b[0]; b[1] = -b[1];
|
|
|
2713 *err = boxquadmin(&J,b,2,x,ll,ur);
|
|
|
2714 if (*err) return(ymax);
|
|
|
2715
|
|
|
2716 /* test to see if this step successfully increases...
|
|
|
2717 */
|
|
|
2718 for (i=0; i<2; i++)
|
|
|
2719 { xi[i] = x[i]+b[i];
|
|
|
2720 if (xi[i]<ll[i]+1e-8*h[i]) xi[i] = ll[i];
|
|
|
2721 if (xi[i]>ur[i]-1e-8*h[i]) xi[i] = ur[i];
|
|
|
2722 }
|
|
|
2723 y1 = f(xi);
|
|
|
2724 if (y1 < ymax) /* no increase */
|
|
|
2725 { *err = 1;
|
|
|
2726 return(ymax);
|
|
|
2727 }
|
|
|
2728
|
|
|
2729 /* wonderful. update x, h, with the restriction that h can't decrease
|
|
|
2730 * by a factor over 10, or increase by over 2.
|
|
|
2731 */
|
|
|
2732 for (i=0; i<2; i++)
|
|
|
2733 { x[i] = xi[i];
|
|
|
2734 if (x[i]==ll[i]) con[i] = -1;
|
|
|
2735 if (x[i]==ur[i]) con[i] = 1;
|
|
|
2736 h0 = fabs(b[i]);
|
|
|
2737 h0 = min(h0,2*h[i]);
|
|
|
2738 h0 = max(h0,h[i]/10);
|
|
|
2739 h[i] = min(h0,(ur[i]-ll[i])/2.0);
|
|
|
2740 }
|
|
|
2741 return(y1);
|
|
|
2742 }
|
|
|
2743
|
|
|
2744 double maxbv(f,x,h,ll,ur,m0,m1,tol)
|
|
|
2745 double (*f)(), *x, *h, *ll, *ur, tol;
|
|
|
2746 int m0, m1;
|
|
|
2747 { double ymax;
|
|
|
2748 int err, con[2];
|
|
|
2749
|
|
|
2750 con[0] = con[1] = 0;
|
|
|
2751 if ((m0>0) & (m1>0))
|
|
|
2752 {
|
|
|
2753 ymax = maxbvgrid(f,x,ll,ur,m0,m1,con);
|
|
|
2754 h[0] = (ur[0]-ll[0])/(2*m0);
|
|
|
2755 h[1] = (ur[1]-ll[1])/(2*m1);
|
|
|
2756 }
|
|
|
2757 else
|
|
|
2758 { x[0] = (ll[0]+ur[0])/2;
|
|
|
2759 x[1] = (ll[1]+ur[1])/2;
|
|
|
2760 h[0] = (ur[0]-ll[0])/2;
|
|
|
2761 h[1] = (ur[1]-ll[1])/2;
|
|
|
2762 ymax = f(x);
|
|
|
2763 }
|
|
|
2764
|
|
|
2765 while ((h[0]>tol) | (h[1]>tol))
|
|
|
2766 { ymax = maxbvstep(f,x,ymax,h,ll,ur,&err,con);
|
|
|
2767 if (err) mut_printf("maxbvstep failure\n");
|
|
|
2768 }
|
|
|
2769
|
|
|
2770 return(ymax);
|
|
|
2771 }
|
|
|
2772
|
|
|
2773 double maxbvq(f,x,h,ll,ur,m0,m1,tol)
|
|
|
2774 double (*f)(), *x, *h, *ll, *ur, tol;
|
|
|
2775 int m0, m1;
|
|
|
2776 { double ymax;
|
|
|
2777 int err, con[2];
|
|
|
2778
|
|
|
2779 con[0] = con[1] = 0;
|
|
|
2780 if ((m0>0) & (m1>0))
|
|
|
2781 {
|
|
|
2782 ymax = maxbvgrid(f,x,ll,ur,m0,m1,con);
|
|
|
2783 h[0] = (ur[0]-ll[0])/(2*m0);
|
|
|
2784 h[1] = (ur[1]-ll[1])/(2*m1);
|
|
|
2785 }
|
|
|
2786 else
|
|
|
2787 { x[0] = (ll[0]+ur[0])/2;
|
|
|
2788 x[1] = (ll[1]+ur[1])/2;
|
|
|
2789 h[0] = (ur[0]-ll[0])/2;
|
|
|
2790 h[1] = (ur[1]-ll[1])/2;
|
|
|
2791 ymax = f(x);
|
|
|
2792 }
|
|
|
2793
|
|
|
2794 while ((h[0]>tol) | (h[1]>tol))
|
|
|
2795 { /* first, try a quadratric step */
|
|
|
2796 ymax = maxquadstep(f,x,ymax,h,ll,ur,&err,con);
|
|
|
2797 /* if the quadratic step fails, move the grid around */
|
|
|
2798 if (err)
|
|
|
2799 {
|
|
|
2800 ymax = maxbvstep(f,x,ymax,h,ll,ur,&err,con);
|
|
|
2801 if (err)
|
|
|
2802 { mut_printf("maxbvstep failure\n");
|
|
|
2803 return(ymax);
|
|
|
2804 }
|
|
|
2805 }
|
|
|
2806 }
|
|
|
2807
|
|
|
2808 return(ymax);
|
|
|
2809 }
|
|
|
2810 /*
|
|
|
2811 * Copyright 1996-2006 Catherine Loader.
|
|
|
2812 */
|
|
|
2813 #include "mut.h"
|
|
|
2814
|
|
|
2815 prf mut_printf = (prf)printf;
|
|
|
2816
|
|
|
2817 void mut_redirect(newprf)
|
|
|
2818 prf newprf;
|
|
|
2819 { mut_printf = newprf;
|
|
|
2820 }
|
|
|
2821 /*
|
|
|
2822 * Copyright 1996-2006 Catherine Loader.
|
|
|
2823 */
|
|
|
2824 /*
|
|
|
2825 * function to find order of observations in an array.
|
|
|
2826 *
|
|
|
2827 * mut_order(x,ind,i0,i1)
|
|
|
2828 * x array to find order of.
|
|
|
2829 * ind integer array of indexes.
|
|
|
2830 * i0,i1 (integers) range to order.
|
|
|
2831 *
|
|
|
2832 * at output, ind[i0...i1] are permuted so that
|
|
|
2833 * x[ind[i0]] <= x[ind[i0+1]] <= ... <= x[ind[i1]].
|
|
|
2834 * (with ties, output order of corresponding indices is arbitrary).
|
|
|
2835 * The array x is unchanged.
|
|
|
2836 *
|
|
|
2837 * Typically, if x has length n, then i0=0, i1=n-1 and
|
|
|
2838 * ind is (any permutation of) 0...n-1.
|
|
|
2839 */
|
|
|
2840
|
|
|
2841 #include "mut.h"
|
|
|
2842
|
|
|
2843 double med3(x0,x1,x2)
|
|
|
2844 double x0, x1, x2;
|
|
|
2845 { if (x0<x1)
|
|
|
2846 { if (x2<x0) return(x0);
|
|
|
2847 if (x1<x2) return(x1);
|
|
|
2848 return(x2);
|
|
|
2849 }
|
|
|
2850 /* x1 < x0 */
|
|
|
2851 if (x2<x1) return(x1);
|
|
|
2852 if (x0<x2) return(x0);
|
|
|
2853 return(x2);
|
|
|
2854 }
|
|
|
2855
|
|
|
2856 void mut_order(x,ind,i0,i1)
|
|
|
2857 double *x;
|
|
|
2858 int *ind, i0, i1;
|
|
|
2859 { double piv;
|
|
|
2860 int i, l, r, z;
|
|
|
2861
|
|
|
2862 if (i1<=i0) return;
|
|
|
2863 piv = med3(x[ind[i0]],x[ind[i1]],x[ind[(i0+i1)/2]]);
|
|
|
2864 l = i0; r = i0-1;
|
|
|
2865
|
|
|
2866 /* at each stage,
|
|
|
2867 * x[i0..l-1] < piv
|
|
|
2868 * x[l..r] = piv
|
|
|
2869 * x[r+1..i-1] > piv
|
|
|
2870 * then, decide where to put x[i].
|
|
|
2871 */
|
|
|
2872 for (i=i0; i<=i1; i++)
|
|
|
2873 { if (x[ind[i]]==piv)
|
|
|
2874 { r++;
|
|
|
2875 z = ind[i]; ind[i] = ind[r]; ind[r] = z;
|
|
|
2876 }
|
|
|
2877 else if (x[ind[i]]<piv)
|
|
|
2878 { r++;
|
|
|
2879 z = ind[i]; ind[i] = ind[r]; ind[r] = ind[l]; ind[l] = z;
|
|
|
2880 l++;
|
|
|
2881 }
|
|
|
2882 }
|
|
|
2883
|
|
|
2884 if (l>i0) mut_order(x,ind,i0,l-1);
|
|
|
2885 if (r<i1) mut_order(x,ind,r+1,i1);
|
|
|
2886 }
|
|
|
2887 /*
|
|
|
2888 * Copyright 1996-2006 Catherine Loader.
|
|
|
2889 */
|
|
|
2890 #include "mut.h"
|
|
|
2891
|
|
|
2892 #define LOG_2 0.6931471805599453094172321214581765680755
|
|
|
2893 #define IBETA_LARGE 1.0e30
|
|
|
2894 #define IBETA_SMALL 1.0e-30
|
|
|
2895 #define IGAMMA_LARGE 1.0e30
|
|
|
2896 #define DOUBLE_EP 2.2204460492503131E-16
|
|
|
2897
|
|
|
2898 double ibeta(x, a, b)
|
|
|
2899 double x, a, b;
|
|
|
2900 { int flipped = 0, i, k, count;
|
|
|
2901 double I = 0, temp, pn[6], ak, bk, next, prev, factor, val;
|
|
|
2902 if (x <= 0) return(0);
|
|
|
2903 if (x >= 1) return(1);
|
|
|
2904 /* use ibeta(x,a,b) = 1-ibeta(1-x,b,z) */
|
|
|
2905 if ((a+b+1)*x > (a+1))
|
|
|
2906 { flipped = 1;
|
|
|
2907 temp = a;
|
|
|
2908 a = b;
|
|
|
2909 b = temp;
|
|
|
2910 x = 1 - x;
|
|
|
2911 }
|
|
|
2912 pn[0] = 0.0;
|
|
|
2913 pn[2] = pn[3] = pn[1] = 1.0;
|
|
|
2914 count = 1;
|
|
|
2915 val = x/(1.0-x);
|
|
|
2916 bk = 1.0;
|
|
|
2917 next = 1.0;
|
|
|
2918 do
|
|
|
2919 { count++;
|
|
|
2920 k = count/2;
|
|
|
2921 prev = next;
|
|
|
2922 if (count%2 == 0)
|
|
|
2923 ak = -((a+k-1.0)*(b-k)*val)/((a+2.0*k-2.0)*(a+2.0*k-1.0));
|
|
|
2924 else
|
|
|
2925 ak = ((a+b+k-1.0)*k*val)/((a+2.0*k)*(a+2.0*k-1.0));
|
|
|
2926 pn[4] = bk*pn[2] + ak*pn[0];
|
|
|
2927 pn[5] = bk*pn[3] + ak*pn[1];
|
|
|
2928 next = pn[4] / pn[5];
|
|
|
2929 for (i=0; i<=3; i++)
|
|
|
2930 pn[i] = pn[i+2];
|
|
|
2931 if (fabs(pn[4]) >= IBETA_LARGE)
|
|
|
2932 for (i=0; i<=3; i++)
|
|
|
2933 pn[i] /= IBETA_LARGE;
|
|
|
2934 if (fabs(pn[4]) <= IBETA_SMALL)
|
|
|
2935 for (i=0; i<=3; i++)
|
|
|
2936 pn[i] /= IBETA_SMALL;
|
|
|
2937 } while (fabs(next-prev) > DOUBLE_EP*prev);
|
|
|
2938 /* factor = a*log(x) + (b-1)*log(1-x);
|
|
|
2939 factor -= mut_lgamma(a+1) + mut_lgamma(b) - mut_lgamma(a+b); */
|
|
|
2940 factor = dbeta(x,a,b,1) + log(x/a);
|
|
|
2941 I = exp(factor) * next;
|
|
|
2942 return(flipped ? 1-I : I);
|
|
|
2943 }
|
|
|
2944
|
|
|
2945 /*
|
|
|
2946 * Incomplete gamma function.
|
|
|
2947 * int_0^x u^{df-1} e^{-u} du / Gamma(df).
|
|
|
2948 */
|
|
|
2949 double igamma(x, df)
|
|
|
2950 double x, df;
|
|
|
2951 { double factor, term, gintegral, pn[6], rn, ak, bk;
|
|
|
2952 int i, count, k;
|
|
|
2953 if (x <= 0.0) return(0.0);
|
|
|
2954
|
|
|
2955 if (df < 1.0)
|
|
|
2956 return( dgamma(x,df+1.0,1.0,0) + igamma(x,df+1.0) );
|
|
|
2957
|
|
|
2958 factor = x * dgamma(x,df,1.0,0);
|
|
|
2959 /* factor = exp(df*log(x) - x - lgamma(df)); */
|
|
|
2960
|
|
|
2961 if (x > 1.0 && x >= df)
|
|
|
2962 {
|
|
|
2963 pn[0] = 0.0;
|
|
|
2964 pn[2] = pn[1] = 1.0;
|
|
|
2965 pn[3] = x;
|
|
|
2966 count = 1;
|
|
|
2967 rn = 1.0 / x;
|
|
|
2968 do
|
|
|
2969 { count++;
|
|
|
2970 k = count / 2;
|
|
|
2971 gintegral = rn;
|
|
|
2972 if (count%2 == 0)
|
|
|
2973 { bk = 1.0;
|
|
|
2974 ak = (double)k - df;
|
|
|
2975 } else
|
|
|
2976 { bk = x;
|
|
|
2977 ak = (double)k;
|
|
|
2978 }
|
|
|
2979 pn[4] = bk*pn[2] + ak*pn[0];
|
|
|
2980 pn[5] = bk*pn[3] + ak*pn[1];
|
|
|
2981 rn = pn[4] / pn[5];
|
|
|
2982 for (i=0; i<4; i++)
|
|
|
2983 pn[i] = pn[i+2];
|
|
|
2984 if (pn[4] > IGAMMA_LARGE)
|
|
|
2985 for (i=0; i<4; i++)
|
|
|
2986 pn[i] /= IGAMMA_LARGE;
|
|
|
2987 } while (fabs(gintegral-rn) > DOUBLE_EP*rn);
|
|
|
2988 gintegral = 1.0 - factor*rn;
|
|
|
2989 }
|
|
|
2990 else
|
|
|
2991 { /* For x<df, use the series
|
|
|
2992 * dpois(df,x)*( 1 + x/(df+1) + x^2/((df+1)(df+2)) + ... )
|
|
|
2993 * This could be slow if df large and x/df is close to 1.
|
|
|
2994 */
|
|
|
2995 gintegral = term = 1.0;
|
|
|
2996 rn = df;
|
|
|
2997 do
|
|
|
2998 { rn += 1.0;
|
|
|
2999 term *= x/rn;
|
|
|
3000 gintegral += term;
|
|
|
3001 } while (term > DOUBLE_EP*gintegral);
|
|
|
3002 gintegral *= factor/df;
|
|
|
3003 }
|
|
|
3004 return(gintegral);
|
|
|
3005 }
|
|
|
3006
|
|
|
3007 double pf(q, df1, df2)
|
|
|
3008 double q, df1, df2;
|
|
|
3009 { return(ibeta(q*df1/(df2+q*df1), df1/2, df2/2));
|
|
|
3010 }
|
|
|
3011 /*
|
|
|
3012 * Copyright 1996-2006 Catherine Loader.
|
|
|
3013 */
|
|
|
3014 #include "mut.h"
|
|
|
3015 #include <string.h>
|
|
|
3016
|
|
|
3017 /* quadmin: minimize the quadratic,
|
|
|
3018 * 2<x,b> + x^T A x.
|
|
|
3019 * x = -A^{-1} b.
|
|
|
3020 *
|
|
|
3021 * conquadmin: min. subject to L'x = d (m constraints)
|
|
|
3022 * x = -A^{-1}(b+Ly) (y = Lagrange multiplier)
|
|
|
3023 * y = -(L'A^{-1}L)^{-1} (L'A^{-1}b)
|
|
|
3024 * x = -A^{-1}b + A^{-1}L (L'A^{-1}L)^{-1} [(L'A^{-1})b + d]
|
|
|
3025 * (non-zero d to be added!!)
|
|
|
3026 *
|
|
|
3027 * qprogmin: min. subject to L'x >= 0.
|
|
|
3028 */
|
|
|
3029
|
|
|
3030 void quadmin(J,b,p)
|
|
|
3031 jacobian *J;
|
|
|
3032 double *b;
|
|
|
3033 int p;
|
|
|
3034 { int i;
|
|
|
3035 jacob_dec(J,JAC_CHOL);
|
|
|
3036 i = jacob_solve(J,b);
|
|
|
3037 if (i<p) mut_printf("quadmin singular %2d %2d\n",i,p);
|
|
|
3038 for (i=0; i<p; i++) b[i] = -b[i];
|
|
|
3039 }
|
|
|
3040
|
|
|
3041 /* project vector a (length n) onto
|
|
|
3042 * columns of X (n rows, m columns, organized by column).
|
|
|
3043 * store result in y.
|
|
|
3044 */
|
|
|
3045 #define pmaxm 10
|
|
|
3046 #define pmaxn 100
|
|
|
3047 void project(a,X,y,n,m)
|
|
|
3048 double *a, *X, *y;
|
|
|
3049 int n, m;
|
|
|
3050 { double xta[pmaxm], R[pmaxn*pmaxm];
|
|
|
3051 int i;
|
|
|
3052
|
|
|
3053 if (n>pmaxn) mut_printf("project: n too large\n");
|
|
|
3054 if (m>pmaxm) mut_printf("project: m too large\n");
|
|
|
3055
|
|
|
3056 for (i=0; i<m; i++) xta[i] = innerprod(a,&X[i*n],n);
|
|
|
3057 memcpy(R,X,m*n*sizeof(double));
|
|
|
3058 qr(R,n,m,NULL);
|
|
|
3059 qrsolv(R,xta,n,m);
|
|
|
3060
|
|
|
3061 matrixmultiply(X,xta,y,n,m,1);
|
|
|
3062 }
|
|
|
3063
|
|
|
3064 void resproj(a,X,y,n,m)
|
|
|
3065 double *a, *X, *y;
|
|
|
3066 int n, m;
|
|
|
3067 { int i;
|
|
|
3068 project(a,X,y,n,m);
|
|
|
3069 for (i=0; i<n; i++) y[i] = a[i]-y[i];
|
|
|
3070 }
|
|
|
3071
|
|
|
3072 /* x = -A^{-1}b + A^{-1}L (L'A^{-1}L)^{-1} [(L'A^{-1})b + d] */
|
|
|
3073 void conquadmin(J,b,n,L,d,m)
|
|
|
3074 jacobian *J;
|
|
|
3075 double *b, *L, *d;
|
|
|
3076 int m, n;
|
|
|
3077 { double bp[10], L0[100];
|
|
|
3078 int i, j;
|
|
|
3079
|
|
|
3080 if (n>10) mut_printf("conquadmin: max. n is 10.\n");
|
|
|
3081 memcpy(L0,L,n*m*sizeof(double));
|
|
|
3082 jacob_dec(J,JAC_CHOL);
|
|
|
3083 for (i=0; i<m; i++) jacob_hsolve(J,&L[i*n]);
|
|
|
3084 jacob_hsolve(J,b);
|
|
|
3085
|
|
|
3086 resproj(b,L,bp,n,m);
|
|
|
3087
|
|
|
3088 jacob_isolve(J,bp);
|
|
|
3089 for (i=0; i<n; i++) b[i] = -bp[i];
|
|
|
3090
|
|
|
3091 qr(L,n,m,NULL);
|
|
|
3092 qrsolv(L,d,n,m);
|
|
|
3093 for (i=0; i<n; i++)
|
|
|
3094 { bp[i] = 0;
|
|
|
3095 for (j=0; j<m; j++) bp[i] += L0[j*n+i]*d[j];
|
|
|
3096 }
|
|
|
3097 jacob_solve(J,bp);
|
|
|
3098 for (i=0; i<n; i++) b[i] += bp[i];
|
|
|
3099 }
|
|
|
3100
|
|
|
3101 void qactivemin(J,b,n,L,d,m,ac)
|
|
|
3102 jacobian *J;
|
|
|
3103 double *b, *L, *d;
|
|
|
3104 int m, n, *ac;
|
|
|
3105 { int i, nac;
|
|
|
3106 double M[100], dd[10];
|
|
|
3107 nac = 0;
|
|
|
3108 for (i=0; i<m; i++) if (ac[i]>0)
|
|
|
3109 { memcpy(&M[nac*n],&L[i*n],n*sizeof(double));
|
|
|
3110 dd[nac] = d[i];
|
|
|
3111 nac++;
|
|
|
3112 }
|
|
|
3113 conquadmin(J,b,n,M,dd,nac);
|
|
|
3114 }
|
|
|
3115
|
|
|
3116 /* return 1 for full step; 0 if new constraint imposed. */
|
|
|
3117 int movefrom(x0,x,n,L,d,m,ac)
|
|
|
3118 double *x0, *x, *L, *d;
|
|
|
3119 int n, m, *ac;
|
|
|
3120 { int i, imin;
|
|
|
3121 double c0, c1, lb, lmin;
|
|
|
3122 lmin = 1.0;
|
|
|
3123 for (i=0; i<m; i++) if (ac[i]==0)
|
|
|
3124 { c1 = innerprod(&L[i*n],x,n)-d[i];
|
|
|
3125 if (c1<0.0)
|
|
|
3126 { c0 = innerprod(&L[i*n],x0,n)-d[i];
|
|
|
3127 if (c0<0.0)
|
|
|
3128 { if (c1<c0) { lmin = 0.0; imin = 1; }
|
|
|
3129 }
|
|
|
3130 else
|
|
|
3131 { lb = c0/(c0-c1);
|
|
|
3132 if (lb<lmin) { lmin = lb; imin = i; }
|
|
|
3133 }
|
|
|
3134 }
|
|
|
3135 }
|
|
|
3136 for (i=0; i<n; i++)
|
|
|
3137 x0[i] = lmin*x[i]+(1-lmin)*x0[i];
|
|
|
3138 if (lmin==1.0) return(1);
|
|
|
3139 ac[imin] = 1;
|
|
|
3140 return(0);
|
|
|
3141 }
|
|
|
3142
|
|
|
3143 int qstep(J,b,x0,n,L,d,m,ac,deac)
|
|
|
3144 jacobian *J;
|
|
|
3145 double *b, *x0, *L, *d;
|
|
|
3146 int m, n, *ac, deac;
|
|
|
3147 { double x[10];
|
|
|
3148 int i;
|
|
|
3149
|
|
|
3150 if (m>10) mut_printf("qstep: too many constraints.\n");
|
|
|
3151 if (deac)
|
|
|
3152 { for (i=0; i<m; i++) if (ac[i]==1)
|
|
|
3153 { ac[i] = 0;
|
|
|
3154 memcpy(x,b,n*sizeof(double));
|
|
|
3155 qactivemin(J,x,n,L,d,m,ac);
|
|
|
3156 if (innerprod(&L[i*n],x,n)>d[i]) /* deactivate this constraint; should rem. */
|
|
|
3157 i = m+10;
|
|
|
3158 else
|
|
|
3159 ac[i] = 1;
|
|
|
3160 }
|
|
|
3161 if (i==m) return(0); /* no deactivation possible */
|
|
|
3162 }
|
|
|
3163
|
|
|
3164 do
|
|
|
3165 { if (!deac)
|
|
|
3166 { memcpy(x,b,n*sizeof(double));
|
|
|
3167 qactivemin(J,x,n,L,d,m,ac);
|
|
|
3168 }
|
|
|
3169 i = movefrom(x0,x,n,L,d,m,ac);
|
|
|
3170
|
|
|
3171 deac = 0;
|
|
|
3172 } while (i==0);
|
|
|
3173 return(1);
|
|
|
3174 }
|
|
|
3175
|
|
|
3176 /*
|
|
|
3177 * x0 is starting value; should satisfy constraints.
|
|
|
3178 * L is n*m constraint matrix.
|
|
|
3179 * ac is active constraint vector:
|
|
|
3180 * ac[i]=0, inactive.
|
|
|
3181 * ac[i]=1, active, but can be deactivated.
|
|
|
3182 * ac[i]=2, active, cannot be deactivated.
|
|
|
3183 */
|
|
|
3184
|
|
|
3185 void qprogmin(J,b,x0,n,L,d,m,ac)
|
|
|
3186 jacobian *J;
|
|
|
3187 double *b, *x0, *L, *d;
|
|
|
3188 int m, n, *ac;
|
|
|
3189 { int i;
|
|
|
3190 for (i=0; i<m; i++) if (ac[i]==0)
|
|
|
3191 { if (innerprod(&L[i*n],x0,n) < d[i]) ac[i] = 1; }
|
|
|
3192 jacob_dec(J,JAC_CHOL);
|
|
|
3193 qstep(J,b,x0,n,L,d,m,ac,0);
|
|
|
3194 while (qstep(J,b,x0,n,L,d,m,ac,1));
|
|
|
3195 }
|
|
|
3196
|
|
|
3197 void qpm(A,b,x0,n,L,d,m,ac)
|
|
|
3198 double *A, *b, *x0, *L, *d;
|
|
|
3199 int *n, *m, *ac;
|
|
|
3200 { jacobian J;
|
|
|
3201 double wk[1000];
|
|
|
3202 jac_alloc(&J,*n,wk);
|
|
|
3203 memcpy(J.Z,A,(*n)*(*n)*sizeof(double));
|
|
|
3204 J.p = *n;
|
|
|
3205 J.st = JAC_RAW;
|
|
|
3206 qprogmin(&J,b,x0,*n,L,d,*m,ac);
|
|
|
3207 }
|
