Mercurial > repos > vipints > rdiff
diff rDiff/src/locfit/Source/libmut.c @ 0:0f80a5141704
version 0.3 uploaded
| author | vipints |
|---|---|
| date | Thu, 14 Feb 2013 23:38:36 -0500 |
| parents | |
| children |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rDiff/src/locfit/Source/libmut.c Thu Feb 14 23:38:36 2013 -0500 @@ -0,0 +1,3207 @@ +/* + * Copyright 1996-2006 Catherine Loader. + */ + +#include "mex.h" +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include <math.h> +#include "mut.h" + +/* stirlerr(n) = log(n!) - log( sqrt(2*pi*n)*(n/e)^n ) */ + +#define S0 0.083333333333333333333 /* 1/12 */ +#define S1 0.00277777777777777777778 /* 1/360 */ +#define S2 0.00079365079365079365079365 /* 1/1260 */ +#define S3 0.000595238095238095238095238 /* 1/1680 */ +#define S4 0.0008417508417508417508417508 /* 1/1188 */ + +/* + error for 0, 0.5, 1.0, 1.5, ..., 14.5, 15.0. +*/ +static double sferr_halves[31] = { +0.0, /* n=0 - wrong, place holder only */ +0.1534264097200273452913848, /* 0.5 */ +0.0810614667953272582196702, /* 1.0 */ +0.0548141210519176538961390, /* 1.5 */ +0.0413406959554092940938221, /* 2.0 */ +0.03316287351993628748511048, /* 2.5 */ +0.02767792568499833914878929, /* 3.0 */ +0.02374616365629749597132920, /* 3.5 */ +0.02079067210376509311152277, /* 4.0 */ +0.01848845053267318523077934, /* 4.5 */ +0.01664469118982119216319487, /* 5.0 */ +0.01513497322191737887351255, /* 5.5 */ +0.01387612882307074799874573, /* 6.0 */ +0.01281046524292022692424986, /* 6.5 */ +0.01189670994589177009505572, /* 7.0 */ +0.01110455975820691732662991, /* 7.5 */ +0.010411265261972096497478567, /* 8.0 */ +0.009799416126158803298389475, /* 8.5 */ +0.009255462182712732917728637, /* 9.0 */ +0.008768700134139385462952823, /* 9.5 */ +0.008330563433362871256469318, /* 10.0 */ +0.007934114564314020547248100, /* 10.5 */ +0.007573675487951840794972024, /* 11.0 */ +0.007244554301320383179543912, /* 11.5 */ +0.006942840107209529865664152, /* 12.0 */ +0.006665247032707682442354394, /* 12.5 */ +0.006408994188004207068439631, /* 13.0 */ +0.006171712263039457647532867, /* 13.5 */ +0.005951370112758847735624416, /* 14.0 */ +0.005746216513010115682023589, /* 14.5 */ +0.005554733551962801371038690 /* 15.0 */ +}; + +double stirlerr(n) +double n; +{ double nn; + + if (n<15.0) + { nn = 2.0*n; + if (nn==(int)nn) return(sferr_halves[(int)nn]); + return(mut_lgamma(n+1.0) - (n+0.5)*log((double)n)+n - HF_LG_PIx2); + } + + nn = (double)n; + nn = nn*nn; + if (n>500) return((S0-S1/nn)/n); + if (n>80) return((S0-(S1-S2/nn)/nn)/n); + if (n>35) return((S0-(S1-(S2-S3/nn)/nn)/nn)/n); + return((S0-(S1-(S2-(S3-S4/nn)/nn)/nn)/nn)/n); +} + +double bd0(x,np) +double x, np; +{ double ej, s, s1, v; + int j; + if (fabs(x-np)<0.1*(x+np)) + { + s = (x-np)*(x-np)/(x+np); + v = (x-np)/(x+np); + ej = 2*x*v; v = v*v; + for (j=1; ;++j) + { ej *= v; + s1 = s+ej/((j<<1)+1); + if (s1==s) return(s1); + s = s1; + } + } + return(x*log(x/np)+np-x); +} + +/* + Raw binomial probability calculation. + (1) This has both p and q arguments, when one may be represented + more accurately than the other (in particular, in df()). + (2) This should NOT check that inputs x and n are integers. This + should be done in the calling function, where necessary. + (3) Does not check for 0<=p<=1 and 0<=q<=1 or NaN's. Do this in + the calling function. +*/ +double dbinom_raw(x,n,p,q,give_log) +double x, n, p, q; +int give_log; +{ double f, lc; + + if (p==0.0) return((x==0) ? D_1 : D_0); + if (q==0.0) return((x==n) ? D_1 : D_0); + + if (x==0) + { lc = (p<0.1) ? -bd0(n,n*q) - n*p : n*log(q); + return( DEXP(lc) ); + } + + if (x==n) + { lc = (q<0.1) ? -bd0(n,n*p) - n*q : n*log(p); + return( DEXP(lc) ); + } + + if ((x<0) | (x>n)) return( D_0 ); + + lc = stirlerr(n) - stirlerr(x) - stirlerr(n-x) + - bd0(x,n*p) - bd0(n-x,n*q); + f = (PIx2*x*(n-x))/n; + + return( FEXP(f,lc) ); +} + +double dbinom(x,n,p,give_log) +int x, n; +double p; +int give_log; +{ + if ((p<0) | (p>1) | (n<0)) return(INVALID_PARAMS); + if (x<0) return( D_0 ); + + return( dbinom_raw((double)x,(double)n,p,1-p,give_log) ); +} + +/* + Poisson probability lb^x exp(-lb) / x!. + I don't check that x is an integer, since other functions + that call dpois_raw() (i.e. dgamma) may use a fractional + x argument. +*/ +double dpois_raw(x,lambda,give_log) +int give_log; +double x, lambda; +{ + if (lambda==0) return( (x==0) ? D_1 : D_0 ); + if (x==0) return( DEXP(-lambda) ); + if (x<0) return( D_0 ); + + return(FEXP( PIx2*x, -stirlerr(x)-bd0(x,lambda) )); +} + +double dpois(x,lambda,give_log) +int x, give_log; +double lambda; +{ + if (lambda<0) return(INVALID_PARAMS); + if (x<0) return( D_0 ); + + return( dpois_raw((double)x,lambda,give_log) ); +} + +double dbeta(x,a,b,give_log) +double x, a, b; +int give_log; +{ double f, p; + + if ((a<=0) | (b<=0)) return(INVALID_PARAMS); + if ((x<=0) | (x>=1)) return(D_0); + + if (a<1) + { if (b<1) /* a<1, b<1 */ + { f = a*b/((a+b)*x*(1-x)); + p = dbinom_raw(a,a+b,x,1-x,give_log); + } + else /* a<1, b>=1 */ + { f = a/x; + p = dbinom_raw(a,a+b-1,x,1-x,give_log); + } + } + else + { if (b<1) /* a>=1, b<1 */ + { f = b/(1-x); + p = dbinom_raw(a-1,a+b-1,x,1-x,give_log); + } + else /* a>=1, b>=1 */ + { f = a+b-1; + p = dbinom_raw(a-1,(a-1)+(b-1),x,1-x,give_log); + } + } + + return( (give_log) ? p + log(f) : p*f ); +} + +/* + * To evaluate the F density, write it as a Binomial probability + * with p = x*m/(n+x*m). For m>=2, use the simplest conversion. + * For m<2, (m-2)/2<0 so the conversion will not work, and we must use + * a second conversion. Note the division by p; this seems unavoidable + * for m < 2, since the F density has a singularity as x (or p) -> 0. + */ +double df(x,m,n,give_log) +double x, m, n; +int give_log; +{ double p, q, f, dens; + + if ((m<=0) | (n<=0)) return(INVALID_PARAMS); + if (x <= 0.0) return(D_0); + + f = 1.0/(n+x*m); + q = n*f; + p = x*m*f; + + if (m>=2) + { f = m*q/2; + dens = dbinom_raw((m-2)/2.0, (m+n-2)/2.0, p, q, give_log); + } + else + { f = m*m*q / (2*p*(m+n)); + dens = dbinom_raw(m/2.0, (m+n)/2.0, p, q, give_log); + } + + return((give_log) ? log(f)+dens : f*dens); +} + +/* + * Gamma density, + * lb^r x^{r-1} exp(-lb*x) + * p(x;r,lb) = ----------------------- + * (r-1)! + * + * If USE_SCALE is defined below, the lb argument will be interpreted + * as a scale parameter (i.e. replace lb by 1/lb above). Otherwise, + * it is interpreted as a rate parameter, as above. + */ + +/* #define USE_SCALE */ + +double dgamma(x,r,lambda,give_log) +int give_log; +double x, r, lambda; +{ double pr; + + if ((r<=0) | (lambda<0)) return(INVALID_PARAMS); + if (x<=0.0) return( D_0 ); + +#ifdef USE_SCALE + lambda = 1.0/lambda; +#endif + + if (r<1) + { pr = dpois_raw(r,lambda*x,give_log); + return( (give_log) ? pr + log(r/x) : pr*r/x ); + } + + pr = dpois_raw(r-1.0,lambda*x,give_log); + return( (give_log) ? pr + log(lambda) : lambda*pr); +} + +double dchisq(x, df, give_log) +double x, df; +int give_log; +{ + return(dgamma(x, df/2.0, + 0.5 + ,give_log)); +/* +#ifdef USE_SCALE + 2.0 +#else + 0.5 +#endif + ,give_log)); +*/ +} + +/* + * Given a sequence of r successes and b failures, we sample n (\le b+r) + * items without replacement. The hypergeometric probability is the + * probability of x successes: + * + * dbinom(x,r,p) * dbinom(n-x,b,p) + * p(x;r,b,n) = --------------------------------- + * dbinom(n,r+b,p) + * + * for any p. For numerical stability, we take p=n/(r+b); with this choice, + * the denominator is not exponentially small. + */ +double dhyper(x,r,b,n,give_log) +int x, r, b, n, give_log; +{ double p, q, p1, p2, p3; + + if ((r<0) | (b<0) | (n<0) | (n>r+b)) + return( INVALID_PARAMS ); + + if (x<0) return(D_0); + + if (n==0) return((x==0) ? D_1 : D_0); + + p = ((double)n)/((double)(r+b)); + q = ((double)(r+b-n))/((double)(r+b)); + + p1 = dbinom_raw((double)x,(double)r,p,q,give_log); + p2 = dbinom_raw((double)(n-x),(double)b,p,q,give_log); + p3 = dbinom_raw((double)n,(double)(r+b),p,q,give_log); + + return( (give_log) ? p1 + p2 - p3 : p1*p2/p3 ); +} + +/* + probability of x failures before the nth success. +*/ +double dnbinom(x,n,p,give_log) +double n, p; +int x, give_log; +{ double prob, f; + + if ((p<0) | (p>1) | (n<=0)) return(INVALID_PARAMS); + + if (x<0) return( D_0 ); + + prob = dbinom_raw(n,x+n,p,1-p,give_log); + f = n/(n+x); + + return((give_log) ? log(f) + prob : f*prob); +} + +double dt(x, df, give_log) +double x, df; +int give_log; +{ double t, u, f; + + if (df<=0.0) return(INVALID_PARAMS); + + /* + exp(t) = Gamma((df+1)/2) /{ sqrt(df/2) * Gamma(df/2) } + = sqrt(df/2) / ((df+1)/2) * Gamma((df+3)/2) / Gamma((df+2)/2). + This form leads to a computation that should be stable for all + values of df, including df -> 0 and df -> infinity. + */ + t = -bd0(df/2.0,(df+1)/2.0) + stirlerr((df+1)/2.0) - stirlerr(df/2.0); + + if (x*x>df) + u = log( 1+ x*x/df ) * df/2; + else + u = -bd0(df/2.0,(df+x*x)/2.0) + x*x/2.0; + + f = PIx2*(1+x*x/df); + + return( FEXP(f,t-u) ); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Provides mut_erf() and mut_erfc() functions. Also mut_pnorm(). + * Had too many problems with erf()'s built into math libraries + * (when they existed). Hence the need to write my own... + * + * Algorithm from Walter Kr\"{a}mer, Frithjof Blomquist. + * "Algorithms with Guaranteed Error Bounds for the Error Function + * and Complementary Error Function" + * Preprint 2000/2, Bergische Universt\"{a}t GH Wuppertal + * http://www.math.uni-wuppertal.de/wrswt/preprints/prep_00_2.pdf + * + * Coded by Catherine Loader, September 2006. + */ + +#include "mut.h" + +double erf1(double x) /* erf; 0 < x < 0.65) */ +{ double p[5] = {1.12837916709551256e0, /* 2/sqrt(pi) */ + 1.35894887627277916e-1, + 4.03259488531795274e-2, + 1.20339380863079457e-3, + 6.49254556481904354e-5}; + double q[5] = {1.00000000000000000e0, + 4.53767041780002545e-1, + 8.69936222615385890e-2, + 8.49717371168693357e-3, + 3.64915280629351082e-4}; + double x2, p4, q4; + x2 = x*x; + p4 = p[0] + p[1]*x2 + p[2]*x2*x2 + p[3]*x2*x2*x2 + p[4]*x2*x2*x2*x2; + q4 = q[0] + q[1]*x2 + q[2]*x2*x2 + q[3]*x2*x2*x2 + q[4]*x2*x2*x2*x2; + return(x*p4/q4); +} + +double erf2(double x) /* erfc; 0.65 <= x < 2.2 */ +{ double p[6] = {9.99999992049799098e-1, + 1.33154163936765307e0, + 8.78115804155881782e-1, + 3.31899559578213215e-1, + 7.14193832506776067e-2, + 7.06940843763253131e-3}; + double q[7] = {1.00000000000000000e0, + 2.45992070144245533e0, + 2.65383972869775752e0, + 1.61876655543871376e0, + 5.94651311286481502e-1, + 1.26579413030177940e-1, + 1.25304936549413393e-2}; + double x2, p5, q6; + x2 = x*x; + p5 = p[0] + p[1]*x + p[2]*x2 + p[3]*x2*x + p[4]*x2*x2 + p[5]*x2*x2*x; + 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; + return( exp(-x2)*p5/q6 ); +} + +double erf3(double x) /* erfc; 2.2 < x <= 6 */ +{ double p[6] = {9.99921140009714409e-1, + 1.62356584489366647e0, + 1.26739901455873222e0, + 5.81528574177741135e-1, + 1.57289620742838702e-1, + 2.25716982919217555e-2}; + double q[7] = {1.00000000000000000e0, + 2.75143870676376208e0, + 3.37367334657284535e0, + 2.38574194785344389e0, + 1.05074004614827206e0, + 2.78788439273628983e-1, + 4.00072964526861362e-2}; + double x2, p5, q6; + x2 = x*x; + p5 = p[0] + p[1]*x + p[2]*x2 + p[3]*x2*x + p[4]*x2*x2 + p[5]*x2*x2*x; + 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; + return( exp(-x2)*p5/q6 ); +} + +double erf4(double x) /* erfc; x > 6.0 */ +{ double p[5] = {5.64189583547756078e-1, + 8.80253746105525775e0, + 3.84683103716117320e1, + 4.77209965874436377e1, + 8.08040729052301677e0}; + double q[5] = {1.00000000000000000e0, + 1.61020914205869003e1, + 7.54843505665954743e1, + 1.12123870801026015e2, + 3.73997570145040850e1}; + double x2, p4, q4; + if (x>26.5432) return(0.0); + x2 = 1.0/(x*x); + p4 = p[0] + p[1]*x2 + p[2]*x2*x2 + p[3]*x2*x2*x2 + p[4]*x2*x2*x2*x2; + q4 = q[0] + q[1]*x2 + q[2]*x2*x2 + q[3]*x2*x2*x2 + q[4]*x2*x2*x2*x2; + return(exp(-x*x)*p4/(x*q4)); +} + +double mut_erfc(double x) +{ if (x<0.0) return(2.0-mut_erfc(-x)); + if (x==0.0) return(1.0); + if (x<0.65) return(1.0-erf1(x)); + if (x<2.2) return(erf2(x)); + if (x<6.0) return(erf3(x)); + return(erf4(x)); +} + +double mut_erf(double x) +{ + if (x<0.0) return(-mut_erf(-x)); + if (x==0.0) return(0.0); + if (x<0.65) return(erf1(x)); + if (x<2.2) return(1.0-erf2(x)); + if (x<6.0) return(1.0-erf3(x)); + return(1.0-erf4(x)); +} + +double mut_pnorm(double x) +{ if (x<0.0) return(mut_erfc(-x/SQRT2)/2); + return((1.0+mut_erf(x/SQRT2))/2); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include "mut.h" + +static double lookup_gamma[21] = { + 0.0, /* place filler */ + 0.572364942924699971, /* log(G(0.5)) = log(sqrt(pi)) */ + 0.000000000000000000, /* log(G(1)) = log(0!) */ + -0.120782237635245301, /* log(G(3/2)) = log(sqrt(pi)/2)) */ + 0.000000000000000000, /* log(G(2)) = log(1!) */ + 0.284682870472919181, /* log(G(5/2)) = log(3sqrt(pi)/4) */ + 0.693147180559945286, /* log(G(3)) = log(2!) */ + 1.200973602347074287, /* etc */ + 1.791759469228054957, + 2.453736570842442344, + 3.178053830347945752, + 3.957813967618716511, + 4.787491742782045812, + 5.662562059857141783, + 6.579251212010101213, + 7.534364236758732680, + 8.525161361065414667, + 9.549267257300996903, + 10.604602902745250859, + 11.689333420797268559, + 12.801827480081469091 }; + +/* + * coefs are B(2n)/(2n(2n-1)) 2n(2n-1) = + * 2n B(2n) 2n(2n-1) coef + * 2 1/6 2 1/12 + * 4 -1/30 12 -1/360 + * 6 1/42 30 1/1260 + * 8 -1/30 56 -1/1680 + * 10 5/66 90 1/1188 + * 12 -691/2730 132 691/360360 + */ + +double mut_lgamma(double x) +{ double f, z, x2, se; + int ix; + +/* lookup table for common values. + */ + ix = (int)(2*x); + if (((ix>=1) & (ix<=20)) && (ix==2*x)) return(lookup_gamma[ix]); + + f = 1.0; + while (x <= 15) + { f *= x; + x += 1.0; + } + + x2 = 1.0/(x*x); + z = (x-0.5)*log(x) - x + HF_LG_PIx2; + se = (13860 - x2*(462 - x2*(132 - x2*(99 - 140*x2))))/(166320*x); + + return(z + se - log(f)); +} + +double mut_lgammai(int i) /* log(Gamma(i/2)) for integer i */ +{ if (i>20) return(mut_lgamma(i/2.0)); + return(lookup_gamma[i]); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * A is a n*p matrix, find the cholesky decomposition + * of the first p rows. In most applications, will want n=p. + * + * chol_dec(A,n,p) computes the decomoposition R'R=A. + * (note that R is stored in the input A). + * chol_solve(A,v,n,p) computes (R'R)^{-1}v + * chol_hsolve(A,v,n,p) computes (R')^{-1}v + * chol_isolve(A,v,n,p) computes (R)^{-1}v + * chol_qf(A,v,n,p) computes ||(R')^{-1}v||^2. + * chol_mult(A,v,n,p) computes (R'R)v + * + * The solve functions assume that A is already decomposed. + * chol_solve(A,v,n,p) is equivalent to applying chol_hsolve() + * and chol_isolve() in sequence. + */ + +#include <math.h> +#include "mut.h" + +void chol_dec(A,n,p) +double *A; +int n, p; +{ int i, j, k; + + for (j=0; j<p; j++) + { k = n*j+j; + for (i=0; i<j; i++) A[k] -= A[n*j+i]*A[n*j+i]; + if (A[k]<=0) + { for (i=j; i<p; i++) A[n*i+j] = 0.0; } + else + { A[k] = sqrt(A[k]); + for (i=j+1; i<p; i++) + { for (k=0; k<j; k++) + A[n*i+j] -= A[n*i+k]*A[n*j+k]; + A[n*i+j] /= A[n*j+j]; + } + } + } + for (j=0; j<p; j++) + for (i=j+1; i<p; i++) A[n*j+i] = 0.0; +} + +int chol_solve(A,v,n,p) +double *A, *v; +int n, p; +{ int i, j; + + for (i=0; i<p; i++) + { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j]; + v[i] /= A[i*n+i]; + } + for (i=p-1; i>=0; i--) + { for (j=i+1; j<p; j++) v[i] -= A[j*n+i]*v[j]; + v[i] /= A[i*n+i]; + } + return(p); +} + +int chol_hsolve(A,v,n,p) +double *A, *v; +int n, p; +{ int i, j; + + for (i=0; i<p; i++) + { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j]; + v[i] /= A[i*n+i]; + } + return(p); +} + +int chol_isolve(A,v,n,p) +double *A, *v; +int n, p; +{ int i, j; + + for (i=p-1; i>=0; i--) + { for (j=i+1; j<p; j++) v[i] -= A[j*n+i]*v[j]; + v[i] /= A[i*n+i]; + } + return(p); +} + +double chol_qf(A,v,n,p) +double *A, *v; +int n, p; +{ int i, j; + double sum; + + sum = 0.0; + for (i=0; i<p; i++) + { for (j=0; j<i; j++) v[i] -= A[i*n+j]*v[j]; + v[i] /= A[i*n+i]; + sum += v[i]*v[i]; + } + return(sum); +} + +int chol_mult(A,v,n,p) +double *A, *v; +int n, p; +{ int i, j; + double sum; + for (i=0; i<p; i++) + { sum = 0; + for (j=i; j<p; j++) sum += A[j*n+i]*v[j]; + v[i] = sum; + } + for (i=p-1; i>=0; i--) + { sum = 0; + for (j=0; j<=i; j++) sum += A[i*n+j]*v[j]; + v[i] = sum; + } + return(1); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include <stdio.h> +#include <math.h> +#include "mut.h" +#define E_MAXIT 20 +#define E_TOL 1.0e-10 +#define SQR(x) ((x)*(x)) + +double e_tol(D,p) +double *D; +int p; +{ double mx; + int i; + if (E_TOL <= 0.0) return(0.0); + mx = D[0]; + for (i=1; i<p; i++) if (D[i*(p+1)]>mx) mx = D[i*(p+1)]; + return(E_TOL*mx); +} + +void eig_dec(X,P,d) +double *X, *P; +int d; +{ int i, j, k, iter, ms; + double c, s, r, u, v; + + for (i=0; i<d; i++) + for (j=0; j<d; j++) P[i*d+j] = (i==j); + + for (iter=0; iter<E_MAXIT; iter++) + { ms = 0; + for (i=0; i<d; i++) + for (j=i+1; j<d; j++) + if (SQR(X[i*d+j]) > 1.0e-15*fabs(X[i*d+i]*X[j*d+j])) + { c = (X[j*d+j]-X[i*d+i])/2; + s = -X[i*d+j]; + r = sqrt(c*c+s*s); + c /= r; + s = sqrt((1-c)/2)*(2*(s>0)-1); + c = sqrt((1+c)/2); + for (k=0; k<d; k++) + { u = X[i*d+k]; v = X[j*d+k]; + X[i*d+k] = u*c+v*s; + X[j*d+k] = v*c-u*s; + } + for (k=0; k<d; k++) + { u = X[k*d+i]; v = X[k*d+j]; + X[k*d+i] = u*c+v*s; + X[k*d+j] = v*c-u*s; + } + X[i*d+j] = X[j*d+i] = 0.0; + for (k=0; k<d; k++) + { u = P[k*d+i]; v = P[k*d+j]; + P[k*d+i] = u*c+v*s; + P[k*d+j] = v*c-u*s; + } + ms = 1; + } + if (ms==0) return; + } + mut_printf("eig_dec not converged\n"); +} + +int eig_solve(J,x) +jacobian *J; +double *x; +{ int d, i, j, rank; + double *D, *P, *Q, *w; + double tol; + + D = J->Z; + P = Q = J->Q; + d = J->p; + w = J->wk; + + tol = e_tol(D,d); + + rank = 0; + for (i=0; i<d; i++) + { w[i] = 0.0; + for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j]; + } + for (i=0; i<d; i++) + if (D[i*d+i]>tol) + { w[i] /= D[i*(d+1)]; + rank++; + } + for (i=0; i<d; i++) + { x[i] = 0.0; + for (j=0; j<d; j++) x[i] += Q[i*d+j]*w[j]; + } + return(rank); +} + +int eig_hsolve(J,v) +jacobian *J; +double *v; +{ int i, j, p, rank; + double *D, *Q, *w; + double tol; + + D = J->Z; + Q = J->Q; + p = J->p; + w = J->wk; + + tol = e_tol(D,p); + rank = 0; + + for (i=0; i<p; i++) + { w[i] = 0.0; + for (j=0; j<p; j++) w[i] += Q[j*p+i]*v[j]; + } + for (i=0; i<p; i++) + { if (D[i*p+i]>tol) + { v[i] = w[i]/sqrt(D[i*(p+1)]); + rank++; + } + else v[i] = 0.0; + } + return(rank); +} + +int eig_isolve(J,v) +jacobian *J; +double *v; +{ int i, j, p, rank; + double *D, *Q, *w; + double tol; + + D = J->Z; + Q = J->Q; + p = J->p; + w = J->wk; + + tol = e_tol(D,p); + rank = 0; + + for (i=0; i<p; i++) + { if (D[i*p+i]>tol) + { v[i] = w[i]/sqrt(D[i*(p+1)]); + rank++; + } + else v[i] = 0.0; + } + + for (i=0; i<p; i++) + { w[i] = 0.0; + for (j=0; j<p; j++) w[i] += Q[i*p+j]*v[j]; + } + + return(rank); +} + +double eig_qf(J,v) +jacobian *J; +double *v; +{ int i, j, p; + double sum, tol; + + p = J->p; + sum = 0.0; + tol = e_tol(J->Z,p); + + for (i=0; i<p; i++) + if (J->Z[i*p+i]>tol) + { J->wk[i] = 0.0; + for (j=0; j<p; j++) J->wk[i] += J->Q[j*p+i]*v[j]; + sum += J->wk[i]*J->wk[i]/J->Z[i*p+i]; + } + return(sum); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Integrate a function f over a circle or disc. + */ + +#include "mut.h" + +void setM(M,r,s,c,b) +double *M, r, s, c; +int b; +{ M[0] =-r*s; M[1] = r*c; + M[2] = b*c; M[3] = b*s; + M[4] =-r*c; M[5] = -s; + M[6] = -s; M[7] = 0.0; + M[8] =-r*s; M[9] = c; + M[10]= c; M[11]= 0.0; +} + +void integ_circ(f,r,orig,res,mint,b) +int (*f)(), mint, b; +double r, *orig, *res; +{ double y, x[2], theta, tres[MXRESULT], M[12], c, s; + int i, j, nr; + + y = 0; + for (i=0; i<mint; i++) + { theta = 2*PI*(double)i/(double)mint; + c = cos(theta); s = sin(theta); + x[0] = orig[0]+r*c; + x[1] = orig[1]+r*s; + + if (b!=0) + { M[0] =-r*s; M[1] = r*c; + M[2] = b*c; M[3] = b*s; + M[4] =-r*c; M[5] = -s; + M[6] = -s; M[7] = 0.0; + M[8] =-r*s; M[9] = c; + M[10]= c; M[11]= 0.0; + } + + nr = f(x,2,tres,M); + if (i==0) setzero(res,nr); + for (j=0; j<nr; j++) res[j] += tres[j]; + } + y = 2 * PI * ((b==0)?r:1.0) / mint; + for (j=0; j<nr; j++) res[j] *= y; +} + +void integ_disc(f,fb,fl,res,resb,mg) +int (*f)(), (*fb)(), *mg; +double *fl, *res, *resb; +{ double x[2], y, r, tres[MXRESULT], *orig, rmin, rmax, theta, c, s, M[12]; + int ct, ctb, i, j, k, nr, nrb, w; + + orig = &fl[2]; + rmax = fl[1]; + rmin = fl[0]; + y = 0.0; + ct = ctb = 0; + + for (j=0; j<mg[1]; j++) + { theta = 2*PI*(double)j/(double)mg[1]; + c = cos(theta); s = sin(theta); + for (i= (rmin>0) ? 0 : 1; i<=mg[0]; i++) + { r = rmin + (rmax-rmin)*i/mg[0]; + w = (2+2*(i&1)-(i==0)-(i==mg[0])); + x[0] = orig[0] + r*c; + x[1] = orig[1] + r*s; + nr = f(x,2,tres,NULL); + if (ct==0) setzero(res,nr); + for (k=0; k<nr; k++) res[k] += w*r*tres[k]; + ct++; + if (((i==0) | (i==mg[0])) && (fb!=NULL)) + { setM(M,r,s,c,1-2*(i==0)); + nrb = fb(x,2,tres,M); + if (ctb==0) setzero(resb,nrb); + ctb++; + for (k=0; k<nrb; k++) resb[k] += tres[k]; + } + } + } + + +/* for (i= (rmin>0) ? 0 : 1; i<=mg[0]; i++) + { + r = rmin + (rmax-rmin)*i/mg[0]; + w = (2+2*(i&1)-(i==0)-(i==mg[0])); + + for (j=0; j<mg[1]; j++) + { theta = 2*PI*(double)j/(double)mg[1]; + c = cos(theta); s = sin(theta); + x[0] = orig[0] + r*c; + x[1] = orig[1] + r*s; + nr = f(x,2,tres,NULL); + if (ct==0) setzero(res,nr); + ct++; + for (k=0; k<nr; k++) res[k] += w*r*tres[k]; + + if (((i==0) | (i==mg[0])) && (fb!=NULL)) + { setM(M,r,s,c,1-2*(i==0)); + nrb = fb(x,2,tres,M); + if (ctb==0) setzero(resb,nrb); + ctb++; + for (k=0; k<nrb; k++) resb[k] += tres[k]; + } + } + } */ + for (j=0; j<nr; j++) res[j] *= 2*PI*(rmax-rmin)/(3*mg[0]*mg[1]); + for (j=0; j<nrb; j++) resb[j] *= 2*PI/mg[1]; +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Multivariate integration of a vector-valued function + * using Monte-Carlo method. + * + * uses drand48() random number generator. Does not seed. + */ + +#include <stdlib.h> +#include "mut.h" +extern void setzero(); + +static double M[(1+MXIDIM)*MXIDIM*MXIDIM]; + +void monte(f,ll,ur,d,res,n) +int (*f)(), d, n; +double *ll, *ur, *res; +{ + int i, j, nr; +#ifdef WINDOWS + mut_printf("Sorry, Monte-Carlo Integration not enabled.\n"); + for (i=0; i<n; i++) res[i] = 0.0; +#else + double z, x[MXIDIM], tres[MXRESULT]; + +srand48(234L); + + for (i=0; i<n; i++) + { for (j=0; j<d; j++) x[j] = ll[j] + (ur[j]-ll[j])*drand48(); + nr = f(x,d,tres,NULL); + if (i==0) setzero(res,nr); + for (j=0; j<nr; j++) res[j] += tres[j]; + } + + z = 1; + for (i=0; i<d; i++) z *= (ur[i]-ll[i]); + for (i=0; i<nr; i++) res[i] *= z/n; +#endif +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Multivariate integration of a vector-valued function + * using Simpson's rule. + */ + +#include <math.h> +#include "mut.h" +extern void setzero(); + +static double M[(1+MXIDIM)*MXIDIM*MXIDIM]; + +/* third order corners */ +void simp3(fd,x,d,resd,delta,wt,i0,i1,mg,ct,res2,index) +int (*fd)(), d, wt, i0, i1, *mg, ct, *index; +double *x, *resd, *delta, *res2; +{ int k, l, m, nrd; + double zb; + + for (k=i1+1; k<d; k++) if ((index[k]==0) | (index[k]==mg[k])) + { + setzero(M,d*d); + m = 0; zb = 1.0; + for (l=0; l<d; l++) + if ((l!=i0) & (l!=i1) & (l!=k)) + { M[m*d+l] = 1.0; + m++; + zb *= delta[l]; + } + M[(d-3)*d+i0] = (index[i0]==0) ? -1 : 1; + M[(d-2)*d+i1] = (index[i1]==0) ? -1 : 1; + M[(d-1)*d+k] = (index[k]==0) ? -1 : 1; + nrd = fd(x,d,res2,M); + if ((ct==0) & (i0==0) & (i1==1) & (k==2)) setzero(resd,nrd); + for (l=0; l<nrd; l++) + resd[l] += wt*zb*res2[l]; + } +} + +/* second order corners */ +void simp2(fc,fd,x,d,resc,resd,delta,wt,i0,mg,ct,res2,index) +int (*fc)(), (*fd)(), d, wt, i0, *mg, ct, *index; +double *x, *resc, *resd, *delta, *res2; +{ int j, k, l, nrc; + double zb; + for (j=i0+1; j<d; j++) if ((index[j]==0) | (index[j]==mg[j])) + { setzero(M,d*d); + l = 0; zb = 1; + for (k=0; k<d; k++) if ((k!=i0) & (k!=j)) + { M[l*d+k] = 1.0; + l++; + zb *= delta[k]; + } + M[(d-2)*d+i0] = (index[i0]==0) ? -1 : 1; + M[(d-1)*d+j] = (index[j]==0) ? -1 : 1; + nrc = fc(x,d,res2,M); + if ((ct==0) & (i0==0) & (j==1)) setzero(resc,nrc); + for (k=0; k<nrc; k++) resc[k] += wt*zb*res2[k]; + + if (fd!=NULL) + simp3(fd,x,d,resd,delta,wt,i0,j,mg,ct,res2,index); + } +} + +/* first order boundary */ +void simp1(fb,fc,fd,x,d,resb,resc,resd,delta,wt,mg,ct,res2,index) +int (*fb)(), (*fc)(), (*fd)(), d, wt, *mg, ct, *index; +double *x, *resb, *resc, *resd, *delta, *res2; +{ int i, j, k, nrb; + double zb; + for (i=0; i<d; i++) if ((index[i]==0) | (index[i]==mg[i])) + { setzero(M,(1+d)*d*d); + k = 0; + for (j=0; j<d; j++) if (j!=i) + { M[k*d+j] = 1; + k++; + } + M[(d-1)*d+i] = (index[i]==0) ? -1 : 1; + nrb = fb(x,d,res2,M); + zb = 1; + for (j=0; j<d; j++) if (i!=j) zb *= delta[j]; + if ((ct==0) && (i==0)) + for (j=0; j<nrb; j++) resb[j] = 0.0; + for (j=0; j<nrb; j++) resb[j] += wt*zb*res2[j]; + + if (fc!=NULL) + simp2(fc,fd,x,d,resc,resd,delta,wt,i,mg,ct,res2,index); + } +} + +void simpson4(f,fb,fc,fd,ll,ur,d,res,resb,resc,resd,mg,res2) +int (*f)(), (*fb)(), (*fc)(), (*fd)(), d, *mg; +double *ll, *ur, *res, *resb, *resc, *resd, *res2; +{ int ct, i, j, nr, wt, index[MXIDIM]; + double x[MXIDIM], delta[MXIDIM], z; + + for (i=0; i<d; i++) + { index[i] = 0; + x[i] = ll[i]; + if (mg[i]&1) mg[i]++; + delta[i] = (ur[i]-ll[i])/(3*mg[i]); + } + ct = 0; + + while(1) + { wt = 1; + for (i=0; i<d; i++) + wt *= (4-2*(index[i]%2==0)-(index[i]==0)-(index[i]==mg[i])); + nr = f(x,d,res2,NULL); + if (ct==0) setzero(res,nr); + for (i=0; i<nr; i++) res[i] += wt*res2[i]; + + if (fb!=NULL) + simp1(fb,fc,fd,x,d,resb,resc,resd,delta,wt,mg,ct,res2,index); + + /* compute next grid point */ + for (i=0; i<d; i++) + { index[i]++; + if (index[i]>mg[i]) + { index[i] = 0; + x[i] = ll[i]; + if (i==d-1) /* done */ + { z = 1.0; + for (j=0; j<d; j++) z *= delta[j]; + for (j=0; j<nr; j++) res[j] *= z; + return; + } + } + else + { x[i] = ll[i] + 3*delta[i]*index[i]; + i = d; + } + } + ct++; + } +} + +void simpsonm(f,ll,ur,d,res,mg,res2) +int (*f)(), d, *mg; +double *ll, *ur, *res, *res2; +{ simpson4(f,NULL,NULL,NULL,ll,ur,d,res,NULL,NULL,NULL,mg,res2); +} + +double simpson(f,l0,l1,m) +double (*f)(), l0, l1; +int m; +{ double x, sum; + int i; + sum = 0; + for (i=0; i<=m; i++) + { x = ((m-i)*l0 + i*l1)/m; + sum += (2+2*(i&1)-(i==0)-(i==m)) * f(x); + } + return( (l1-l0) * sum / (3*m) ); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include "mut.h" + +static double *res, *resb, *orig, rmin, rmax; +static int ct0; + +void sphM(M,r,u) +double *M, r, *u; +{ double h, u1[3], u2[3]; + + /* set the orthogonal unit vectors. */ + h = sqrt(u[0]*u[0]+u[1]*u[1]); + if (h<=0) + { u1[0] = u2[1] = 1.0; + u1[1] = u1[2] = u2[0] = u2[2] = 0.0; + } + else + { u1[0] = u[1]/h; u1[1] = -u[0]/h; u1[2] = 0.0; + u2[0] = u[2]*u[0]/h; u2[1] = u[2]*u[1]/h; u2[2] = -h; + } + + /* parameterize the sphere as r(cos(t)cos(v)u + sin(t)u1 + cos(t)sin(v)u2). + * first layer of M is (dx/dt, dx/dv, dx/dr) at t=v=0. + */ + M[0] = r*u1[0]; M[1] = r*u1[1]; M[2] = r*u1[2]; + M[3] = r*u2[0]; M[4] = r*u2[1]; M[5] = r*u2[2]; + M[6] = u[0]; M[7] = u[1]; M[8] = u[2]; + + /* next layers are second derivative matrix of components of x(r,t,v). + * d^2x/dt^2 = d^2x/dv^2 = -ru; d^2x/dtdv = 0; + * d^2x/drdt = u1; d^2x/drdv = u2; d^2x/dr^2 = 0. + */ + + M[9] = M[13] = -r*u[0]; + M[11]= M[15] = u1[0]; + M[14]= M[16] = u2[0]; + M[10]= M[12] = M[17] = 0.0; + + M[18]= M[22] = -r*u[1]; + M[20]= M[24] = u1[1]; + M[23]= M[25] = u2[1]; + M[19]= M[21] = M[26] = 0.0; + + M[27]= M[31] = -r*u[1]; + M[29]= M[33] = u1[1]; + M[32]= M[34] = u2[1]; + M[28]= M[30] = M[35] = 0.0; + +} + +double ip3(a,b) +double *a, *b; +{ return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2]); +} + +void rn3(a) +double *a; +{ double s; + s = sqrt(ip3(a,a)); + a[0] /= s; a[1] /= s; a[2] /= s; +} + +double sptarea(a,b,c) +double *a, *b, *c; +{ double ea, eb, ec, yab, yac, ybc, sab, sac, sbc; + double ab[3], ac[3], bc[3], x1[3], x2[3]; + + ab[0] = a[0]-b[0]; ab[1] = a[1]-b[1]; ab[2] = a[2]-b[2]; + ac[0] = a[0]-c[0]; ac[1] = a[1]-c[1]; ac[2] = a[2]-c[2]; + bc[0] = b[0]-c[0]; bc[1] = b[1]-c[1]; bc[2] = b[2]-c[2]; + + yab = ip3(ab,a); yac = ip3(ac,a); ybc = ip3(bc,b); + + x1[0] = ab[0] - yab*a[0]; x2[0] = ac[0] - yac*a[0]; + x1[1] = ab[1] - yab*a[1]; x2[1] = ac[1] - yac*a[1]; + x1[2] = ab[2] - yab*a[2]; x2[2] = ac[2] - yac*a[2]; + sab = ip3(x1,x1); sac = ip3(x2,x2); + ea = acos(ip3(x1,x2)/sqrt(sab*sac)); + + x1[0] = ab[0] + yab*b[0]; x2[0] = bc[0] - ybc*b[0]; + x1[1] = ab[1] + yab*b[1]; x2[1] = bc[1] - ybc*b[1]; + x1[2] = ab[2] + yab*b[2]; x2[2] = bc[2] - ybc*b[2]; + sbc = ip3(x2,x2); + eb = acos(ip3(x1,x2)/sqrt(sab*sbc)); + + x1[0] = ac[0] + yac*c[0]; x2[0] = bc[0] + ybc*c[0]; + x1[1] = ac[1] + yac*c[1]; x2[1] = bc[1] + ybc*c[1]; + x1[2] = ac[2] + yac*c[2]; x2[2] = bc[2] + ybc*c[2]; + ec = acos(ip3(x1,x2)/sqrt(sac*sbc)); + +/* + * Euler's formula is a+b+c-PI, except I've cheated... + * a=ea, c=ec, b=PI-eb, which is more stable. + */ + return(ea+ec-eb); +} + +void li(x,f,fb,mint,ar) +double *x, ar; +int (*f)(), (*fb)(), mint; +{ int i, j, nr, nrb, ct1, w; + double u[3], r, M[36]; + double sres[MXRESULT], tres[MXRESULT]; + +/* divide mint by 2, and force to even (Simpson's rule...) + * to make comparable with rectangular interpretation of mint + */ + mint <<= 1; + if (mint&1) mint++; + + ct1 = 0; + for (i= (rmin==0) ? 1 : 0; i<=mint; i++) + { + r = rmin + (rmax-rmin)*i/mint; + w = 2+2*(i&1)-(i==0)-(i==mint); + u[0] = orig[0]+x[0]*r; + u[1] = orig[1]+x[1]*r; + u[2] = orig[2]+x[2]*r; + nr = f(u,3,tres,NULL); + if (ct1==0) setzero(sres,nr); + for (j=0; j<nr; j++) + sres[j] += w*r*r*tres[j]; + ct1++; + + if ((fb!=NULL) && (i==mint)) /* boundary */ + { sphM(M,rmax,x); + nrb = fb(u,3,tres,M); + if (ct0==0) for (j=0; j<nrb; j++) resb[j] = 0.0; + for (j=0; j<nrb; j++) + resb[j] += tres[j]*ar; + } + } + + if (ct0==0) for (j=0; j<nr; j++) res[j] = 0.0; + ct0++; + + for (j=0; j<nr; j++) + res[j] += sres[j] * ar * (rmax-rmin)/(3*mint); +} + +void sphint(f,fb,a,b,c,lev,mint,cent) +double *a, *b, *c; +int (*f)(), (*fb)(), lev, mint, cent; +{ double x[3], ab[3], ac[3], bc[3], ar; + int i; + + if (lev>1) + { ab[0] = a[0]+b[0]; ab[1] = a[1]+b[1]; ab[2] = a[2]+b[2]; rn3(ab); + ac[0] = a[0]+c[0]; ac[1] = a[1]+c[1]; ac[2] = a[2]+c[2]; rn3(ac); + bc[0] = b[0]+c[0]; bc[1] = b[1]+c[1]; bc[2] = b[2]+c[2]; rn3(bc); + lev >>= 1; + if (cent==0) + { sphint(f,fb,a,ab,ac,lev,mint,1); + sphint(f,fb,ab,bc,ac,lev,mint,0); + } + else + { sphint(f,fb,a,ab,ac,lev,mint,1); + sphint(f,fb,b,ab,bc,lev,mint,1); + sphint(f,fb,c,ac,bc,lev,mint,1); + sphint(f,fb,ab,bc,ac,lev,mint,1); + } + return; + } + + x[0] = a[0]+b[0]+c[0]; + x[1] = a[1]+b[1]+c[1]; + x[2] = a[2]+b[2]+c[2]; + rn3(x); + ar = sptarea(a,b,c); + + for (i=0; i<8; i++) + { if (i>0) + { x[0] = -x[0]; + if (i%2 == 0) x[1] = -x[1]; + if (i==4) x[2] = -x[2]; + } + switch(cent) + { case 2: /* the reflection and its 120', 240' rotations */ + ab[0] = x[0]; ab[1] = x[2]; ab[2] = x[1]; li(ab,f,fb,mint,ar); + ab[0] = x[2]; ab[1] = x[1]; ab[2] = x[0]; li(ab,f,fb,mint,ar); + ab[0] = x[1]; ab[1] = x[0]; ab[2] = x[2]; li(ab,f,fb,mint,ar); + case 1: /* and the 120' and 240' rotations */ + ab[0] = x[1]; ab[1] = x[2]; ab[2] = x[0]; li(ab,f,fb,mint,ar); + ac[0] = x[2]; ac[1] = x[0]; ac[2] = x[1]; li(ac,f,fb,mint,ar); + case 0: /* and the triangle itself. */ + li( x,f,fb,mint,ar); + } + } +} + +void integ_sphere(f,fb,fl,Res,Resb,mg) +double *fl, *Res, *Resb; +int (*f)(), (*fb)(), *mg; +{ double a[3], b[3], c[3]; + + a[0] = 1; a[1] = a[2] = 0; + b[1] = 1; b[0] = b[2] = 0; + c[2] = 1; c[0] = c[1] = 0; + + res = Res; + resb=Resb; + orig = &fl[2]; + rmin = fl[0]; + rmax = fl[1]; + + ct0 = 0; + sphint(f,fb,a,b,c,mg[1],mg[0],0); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * solving symmetric equations using the jacobian structure. Currently, three + * methods can be used: cholesky decomposition, eigenvalues, eigenvalues on + * the correlation matrix. + * + * jacob_dec(J,meth) decompose the matrix, meth=JAC_CHOL, JAC_EIG, JAC_EIGD + * jacob_solve(J,v) J^{-1}v + * jacob_hsolve(J,v) (R')^{-1/2}v + * jacob_isolve(J,v) (R)^{-1/2}v + * jacob_qf(J,v) v' J^{-1} v + * jacob_mult(J,v) (R'R) v (pres. CHOL only) + * where for each decomposition, R'R=J, although the different decomp's will + * produce different R's. + * + * To set up the J matrix: + * first, allocate storage: jac_alloc(J,p,wk) + * where p=dimension of matrix, wk is a numeric vector of length + * jac_reqd(p) (or NULL, to allocate automatically). + * now, copy the numeric values to J->Z (numeric vector with length p*p). + * (or, just set J->Z to point to the data vector. But remember this + * will be overwritten by the decomposition). + * finally, set: + * J->st=JAC_RAW; + * J->p = p; + * + * now, call jac_dec(J,meth) (optional) and the solve functions as required. + * + */ + +#include "math.h" +#include "mut.h" + +#define DEF_METH JAC_EIGD + +int jac_reqd(int p) { return(2*p*(p+1)); } + +double *jac_alloc(J,p,wk) +jacobian *J; +int p; +double *wk; +{ if (wk==NULL) + wk = (double *)calloc(2*p*(p+1),sizeof(double)); + if ( wk == NULL ) { + printf("Problem allocating memory for wk\n");fflush(stdout); + } + J->Z = wk; wk += p*p; + J->Q = wk; wk += p*p; + J->wk= wk; wk += p; + J->dg= wk; wk += p; + return(wk); +} + +void jacob_dec(J, meth) +jacobian *J; +int meth; +{ int i, j, p; + + if (J->st != JAC_RAW) return; + + J->sm = J->st = meth; + switch(meth) + { case JAC_EIG: + eig_dec(J->Z,J->Q,J->p); + return; + case JAC_EIGD: + p = J->p; + for (i=0; i<p; i++) + J->dg[i] = (J->Z[i*(p+1)]<=0) ? 0.0 : 1/sqrt(J->Z[i*(p+1)]); + for (i=0; i<p; i++) + for (j=0; j<p; j++) + J->Z[i*p+j] *= J->dg[i]*J->dg[j]; + eig_dec(J->Z,J->Q,J->p); + J->st = JAC_EIGD; + return; + case JAC_CHOL: + chol_dec(J->Z,J->p,J->p); + return; + default: mut_printf("jacob_dec: unknown method %d",meth); + } +} + +int jacob_solve(J,v) /* (X^T W X)^{-1} v */ +jacobian *J; +double *v; +{ int i, rank; + + if (J->st == JAC_RAW) jacob_dec(J,DEF_METH); + + switch(J->st) + { case JAC_EIG: + return(eig_solve(J,v)); + case JAC_EIGD: + for (i=0; i<J->p; i++) v[i] *= J->dg[i]; + rank = eig_solve(J,v); + for (i=0; i<J->p; i++) v[i] *= J->dg[i]; + return(rank); + case JAC_CHOL: + return(chol_solve(J->Z,v,J->p,J->p)); + } + mut_printf("jacob_solve: unknown method %d",J->st); + return(0); +} + +int jacob_hsolve(J,v) /* J^{-1/2} v */ +jacobian *J; +double *v; +{ int i; + + if (J->st == JAC_RAW) jacob_dec(J,DEF_METH); + + switch(J->st) + { case JAC_EIG: + return(eig_hsolve(J,v)); + case JAC_EIGD: /* eigenvalues on corr matrix */ + for (i=0; i<J->p; i++) v[i] *= J->dg[i]; + return(eig_hsolve(J,v)); + case JAC_CHOL: + return(chol_hsolve(J->Z,v,J->p,J->p)); + } + mut_printf("jacob_hsolve: unknown method %d\n",J->st); + return(0); +} + +int jacob_isolve(J,v) /* J^{-1/2} v */ +jacobian *J; +double *v; +{ int i, r; + + if (J->st == JAC_RAW) jacob_dec(J,DEF_METH); + + switch(J->st) + { case JAC_EIG: + return(eig_isolve(J,v)); + case JAC_EIGD: /* eigenvalues on corr matrix */ + r = eig_isolve(J,v); + for (i=0; i<J->p; i++) v[i] *= J->dg[i]; + return(r); + case JAC_CHOL: + return(chol_isolve(J->Z,v,J->p,J->p)); + } + mut_printf("jacob_hsolve: unknown method %d\n",J->st); + return(0); +} + +double jacob_qf(J,v) /* vT J^{-1} v */ +jacobian *J; +double *v; +{ int i; + + if (J->st == JAC_RAW) jacob_dec(J,DEF_METH); + + switch (J->st) + { case JAC_EIG: + return(eig_qf(J,v)); + case JAC_EIGD: + for (i=0; i<J->p; i++) v[i] *= J->dg[i]; + return(eig_qf(J,v)); + case JAC_CHOL: + return(chol_qf(J->Z,v,J->p,J->p)); + default: + mut_printf("jacob_qf: invalid method\n"); + return(0.0); + } +} + +int jacob_mult(J,v) /* J v */ +jacobian *J; +double *v; +{ + if (J->st == JAC_RAW) jacob_dec(J,DEF_METH); + switch (J->st) + { case JAC_CHOL: + return(chol_mult(J->Z,v,J->p,J->p)); + default: + mut_printf("jacob_mult: invalid method\n"); + return(0); + } +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Routines for maximization of a one dimensional function f() + * over an interval [xlo,xhi]. In all cases. the flag argument + * controls the return: + * flag='x', the maximizer xmax is returned. + * otherwise, maximum f(xmax) is returned. + * + * max_grid(f,xlo,xhi,n,flag) + * grid maximization of f() over [xlo,xhi] with n intervals. + * + * max_golden(f,xlo,xhi,n,tol,err,flag) + * golden section maximization. + * If n>2, an initial grid search is performed with n intervals + * (this helps deal with local maxima). + * convergence criterion is |x-xmax| < tol. + * err is an error flag. + * if flag='x', return value is xmax. + * otherwise, return value is f(xmax). + * + * max_quad(f,xlo,xhi,n,tol,err,flag) + * quadratic maximization. + * + * max_nr() + * newton-raphson, handles multivariate case. + * + * TODO: additional error checking, non-convergence stop. + */ + +#include <math.h> +#include "mut.h" + +#define max_val(a,b) ((flag=='x') ? a : b) + +double max_grid(f,xlo,xhi,n,flag) +double (*f)(), xlo, xhi; +int n; +char flag; +{ int i, mi; + double x, y, mx, my; + for (i=0; i<=n; i++) + { x = xlo + (xhi-xlo)*i/n; + y = f(x); + if ((i==0) || (y>my)) + { mx = x; + my = y; + mi = i; + } + } + if (mi==0) return(max_val(xlo,my)); + if (mi==n) return(max_val(xhi,my)); + return(max_val(mx,my)); +} + +double max_golden(f,xlo,xhi,n,tol,err,flag) +double (*f)(), xhi, xlo, tol; +int n, *err; +char flag; +{ double dlt, x0, x1, x2, x3, y0, y1, y2, y3; + *err = 0; + + if (n>2) + { dlt = (xhi-xlo)/n; + x0 = max_grid(f,xlo,xhi,n,'x'); + if (xlo<x0) xlo = x0-dlt; + if (xhi>x0) xhi = x0+dlt; + } + + x0 = xlo; y0 = f(xlo); + x3 = xhi; y3 = f(xhi); + x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1); + x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2); + + while (fabs(x3-x0)>tol) + { if ((y1>=y0) && (y1>=y2)) + { x3 = x2; y3 = y2; + x2 = x1; y2 = y1; + x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1); + } + else if ((y2>=y3) && (y2>=y1)) + { x0 = x1; y0 = y1; + x1 = x2; y1 = y2; + x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2); + } + else + { if (y3>y0) { x0 = x2; y0 = y2; } + else { x3 = x1; y3 = y1; } + x1 = gold_rat*x0 + (1-gold_rat)*x3; y1 = f(x1); + x2 = gold_rat*x3 + (1-gold_rat)*x0; y2 = f(x2); + } + } + if (y0>=y1) return(max_val(x0,y0)); + if (y3>=y2) return(max_val(x3,y3)); + return((y1>y2) ? max_val(x1,y1) : max_val(x2,y2)); +} + +double max_quad(f,xlo,xhi,n,tol,err,flag) +double (*f)(), xhi, xlo, tol; +int n, *err; +char flag; +{ double x0, x1, x2, xnew, y0, y1, y2, ynew, a, b; + *err = 0; + + if (n>2) + { x0 = max_grid(f,xlo,xhi,n,'x'); + if (xlo<x0) xlo = x0-1.0/n; + if (xhi>x0) xhi = x0+1.0/n; + } + + x0 = xlo; y0 = f(x0); + x2 = xhi; y2 = f(x2); + x1 = (x0+x2)/2; y1 = f(x1); + + while (x2-x0>tol) + { + /* first, check (y0,y1,y2) is a peak. If not, + * next interval is the halve with larger of (y0,y2). + */ + if ((y0>y1) | (y2>y1)) + { + if (y0>y2) { x2 = x1; y2 = y1; } + else { x0 = x1; y0 = y1; } + x1 = (x0+x2)/2; + y1 = f(x1); + } + else /* peak */ + { a = (y1-y0)*(x2-x1) + (y1-y2)*(x1-x0); + b = ((y1-y0)*(x2-x1)*(x2+x1) + (y1-y2)*(x1-x0)*(x1+x0))/2; + /* quadratic maximizer is b/a. But first check if a's too + * small, since we may be close to constant. + */ + if ((a<=0) | (b<x0*a) | (b>x2*a)) + { /* split the larger halve */ + xnew = ((x2-x1) > (x1-x0)) ? (x1+x2)/2 : (x0+x1)/2; + } + else + { xnew = b/a; + if (10*xnew < (9*x0+x1)) xnew = (9*x0+x1)/10; + if (10*xnew > (9*x2+x1)) xnew = (9*x2+x1)/10; + if (fabs(xnew-x1) < 0.001*(x2-x0)) + { + if ((x2-x1) > (x1-x0)) + xnew = (99*x1+x2)/100; + else + xnew = (99*x1+x0)/100; + } + } + ynew = f(xnew); + if (xnew>x1) + { if (ynew >= y1) { x0 = x1; y0 = y1; x1 = xnew; y1 = ynew; } + else { x2 = xnew; y2 = ynew; } + } + else + { if (ynew >= y1) { x2 = x1; y2 = y1; x1 = xnew; y1 = ynew; } + else { x0 = xnew; y0 = ynew; } + } + } + } + return(max_val(x1,y1)); +} + +double max_nr(F, coef, old_coef, f1, delta, J, p, maxit, tol, err) +double *coef, *old_coef, *f1, *delta, tol; +int (*F)(), p, maxit, *err; +jacobian *J; +{ double old_f, f, lambda; + int i, j, fr; + double nc, nd, cut; + int rank; + + *err = NR_OK; + J->p = p; + fr = F(coef, &f, f1, J->Z); J->st = JAC_RAW; + + for (i=0; i<maxit; i++) + { memcpy(old_coef,coef,p*sizeof(double)); + old_f = f; + rank = jacob_solve(J,f1); + memcpy(delta,f1,p*sizeof(double)); + + if (rank==0) /* NR won't move! */ + delta[0] = -f/f1[0]; + + lambda = 1.0; + + nc = innerprod(old_coef,old_coef,p); + nd = innerprod(delta, delta, p); + cut = sqrt(nc/nd); + if (cut>1.0) cut = 1.0; + cut *= 0.0001; + do + { for (j=0; j<p; j++) coef[j] = old_coef[j] + lambda*delta[j]; + f = old_f - 1.0; + fr = F(coef, &f, f1, J->Z); J->st = JAC_RAW; + if (fr==NR_BREAK) return(old_f); + + lambda = (fr==NR_REDUCE) ? lambda/2 : lambda/10.0; + } while ((lambda>cut) & (f <= old_f - 1.0e-3)); + + if (f < old_f - 1.0e-3) + { *err = NR_NDIV; + return(f); + } + if (fr==NR_REDUCE) return(f); + + if (fabs(f-old_f) < tol) return(f); + + } + *err = NR_NCON; + return(f); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include <math.h> +#include "mut.h" + +/* qr decomposition of X (n*p organized by column). + * Take w for the ride, if not NULL. + */ +void qr(X,n,p,w) +double *X, *w; +int n, p; +{ int i, j, k, mi; + double c, s, mx, nx, t; + + for (j=0; j<p; j++) + { mi = j; + mx = fabs(X[(n+1)*j]); + nx = mx*mx; + + /* find the largest remaining element in j'th column, row mi. + * flip that row with row j. + */ + for (i=j+1; i<n; i++) + { nx += X[j*n+i]*X[j*n+i]; + if (fabs(X[j*n+i])>mx) + { mi = i; + mx = fabs(X[j*n+i]); + } + } + for (i=j; i<p; i++) + { t = X[i*n+j]; + X[i*n+j] = X[i*n+mi]; + X[i*n+mi] = t; + } + if (w!=NULL) { t = w[j]; w[j] = w[mi]; w[mi] = t; } + + /* want the diag. element -ve, so we do the `good' Householder reflect. + */ + if (X[(n+1)*j]>0) + { for (i=j; i<p; i++) X[i*n+j] = -X[i*n+j]; + if (w!=NULL) w[j] = -w[j]; + } + + nx = sqrt(nx); + c = nx*(nx-X[(n+1)*j]); + if (c!=0) + { for (i=j+1; i<p; i++) + { s = 0; + for (k=j; k<n; k++) + s += X[i*n+k]*X[j*n+k]; + s = (s-nx*X[i*n+j])/c; + for (k=j; k<n; k++) + X[i*n+k] -= s*X[j*n+k]; + X[i*n+j] += s*nx; + } + if (w != NULL) + { s = 0; + for (k=j; k<n; k++) + s += w[k]*X[n*j+k]; + s = (s-nx*w[j])/c; + for (k=j; k<n; k++) + w[k] -= s*X[n*j+k]; + w[j] += s*nx; + } + X[j*n+j] = nx; + } + } +} + +void qrinvx(R,x,n,p) +double *R, *x; +int n, p; +{ int i, j; + for (i=p-1; i>=0; i--) + { for (j=i+1; j<p; j++) x[i] -= R[j*n+i]*x[j]; + x[i] /= R[i*n+i]; + } +} + +void qrtinvx(R,x,n,p) +double *R, *x; +int n, p; +{ int i, j; + for (i=0; i<p; i++) + { for (j=0; j<i; j++) x[i] -= R[i*n+j]*x[j]; + x[i] /= R[i*n+i]; + } +} + +void qrsolv(R,x,n,p) +double *R, *x; +int n, p; +{ qrtinvx(R,x,n,p); + qrinvx(R,x,n,p); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * solve f(x)=c by various methods, with varying stability etc... + * xlo and xhi should be initial bounds for the solution. + * convergence criterion is |f(x)-c| < tol. + * + * double solve_bisect(f,c,xmin,xmax,tol,bd_flag,err) + * double solve_secant(f,c,xmin,xmax,tol,bd_flag,err) + * Bisection and secant methods for solving of f(x)=c. + * xmin and xmax are starting values and bound for solution. + * tol = convergence criterion, |f(x)-c| < tol. + * bd_flag = if (xmin,xmax) doesn't bound a solution, what action to take? + * BDF_NONE returns error. + * BDF_EXPRIGHT increases xmax. + * BDF_EXPLEFT decreases xmin. + * err = error flag. + * The (xmin,xmax) bound is not formally necessary for the secant method. + * But having such a bound vastly improves stability; the code performs + * a bisection step whenever the iterations run outside the bounds. + * + * double solve_nr(f,f1,c,x0,tol,err) + * Newton-Raphson solution of f(x)=c. + * f1 = f'(x). + * x0 = starting value. + * tol = convergence criteria, |f(x)-c| < tol. + * err = error flag. + * No stability checks at present. + * + * double solve_fp(f,x0,tol) + * fixed-point iteration to solve f(x)=x. + * x0 = starting value. + * tol = convergence criteria, stops when |f(x)-x| < tol. + * Convergence requires |f'(x)|<1 in neighborhood of true solution; + * f'(x) \approx 0 gives the fastest convergence. + * No stability checks at present. + * + * TODO: additional error checking, non-convergence stop. + */ + +#include <math.h> +#include "mut.h" + +typedef struct { + double xmin, xmax, x0, x1; + double ymin, ymax, y0, y1; +} solvest; + +int step_expand(f,c,sv,bd_flag) +double (*f)(), c; +solvest *sv; +int bd_flag; +{ double x, y; + if (sv->ymin*sv->ymax <= 0.0) return(0); + if (bd_flag == BDF_NONE) + { mut_printf("invalid bracket\n"); + return(1); /* error */ + } + if (bd_flag == BDF_EXPRIGHT) + { while (sv->ymin*sv->ymax > 0) + { x = sv->xmax + 2*(sv->xmax-sv->xmin); + y = f(x) - c; + sv->xmin = sv->xmax; sv->xmax = x; + sv->ymin = sv->ymax; sv->ymax = y; + } + return(0); + } + if (bd_flag == BDF_EXPLEFT) + { while (sv->ymin*sv->ymax > 0) + { x = sv->xmin - 2*(sv->xmax-sv->xmin); + y = f(x) - c; + sv->xmax = sv->xmin; sv->xmin = x; + sv->ymax = sv->ymin; sv->ymin = y; + } + return(0); + } + mut_printf("step_expand: unknown bd_flag %d.\n",bd_flag); + return(1); +} + +int step_addin(sv,x,y) +solvest *sv; +double x, y; +{ sv->x1 = sv->x0; sv->x0 = x; + sv->y1 = sv->y0; sv->y0 = y; + if (y*sv->ymin > 0) + { sv->xmin = x; + sv->ymin = y; + return(0); + } + if (y*sv->ymax > 0) + { sv->xmax = x; + sv->ymax = y; + return(0); + } + if (y==0) + { sv->xmin = sv->xmax = x; + sv->ymin = sv->ymax = 0; + return(0); + } + return(1); +} + +int step_bisect(f,c,sv) +double (*f)(), c; +solvest *sv; +{ double x, y; + x = sv->x0 = (sv->xmin + sv->xmax)/2; + y = sv->y0 = f(x)-c; + return(step_addin(sv,x,y)); +} + +double solve_bisect(f,c,xmin,xmax,tol,bd_flag,err) +double (*f)(), c, xmin, xmax, tol; +int bd_flag, *err; +{ solvest sv; + int z; + *err = 0; + sv.xmin = xmin; sv.ymin = f(xmin)-c; + sv.xmax = xmax; sv.ymax = f(xmax)-c; + *err = step_expand(f,c,&sv,bd_flag); + if (*err>0) return(sv.xmin); + while(1) /* infinite loop if f is discontinuous */ + { z = step_bisect(f,c,&sv); + if (z) + { *err = 1; + return(sv.x0); + } + if (fabs(sv.y0)<tol) return(sv.x0); + } +} + +int step_secant(f,c,sv) +double (*f)(), c; +solvest *sv; +{ double x, y; + if (sv->y0==sv->y1) return(step_bisect(f,c,sv)); + x = sv->x0 + (sv->x1-sv->x0)*sv->y0/(sv->y0-sv->y1); + if ((x<=sv->xmin) | (x>=sv->xmax)) return(step_bisect(f,c,sv)); + y = f(x)-c; + return(step_addin(sv,x,y)); +} + +double solve_secant(f,c,xmin,xmax,tol,bd_flag,err) +double (*f)(), c, xmin, xmax, tol; +int bd_flag, *err; +{ solvest sv; + int z; + *err = 0; + sv.xmin = xmin; sv.ymin = f(xmin)-c; + sv.xmax = xmax; sv.ymax = f(xmax)-c; + *err = step_expand(f,c,&sv,bd_flag); + if (*err>0) return(sv.xmin); + sv.x0 = sv.xmin; sv.y0 = sv.ymin; + sv.x1 = sv.xmax; sv.y1 = sv.ymax; + while(1) /* infinite loop if f is discontinuous */ + { z = step_secant(f,c,&sv); + if (z) + { *err = 1; + return(sv.x0); + } + if (fabs(sv.y0)<tol) return(sv.x0); + } +} + +double solve_nr(f,f1,c,x0,tol,err) +double (*f)(), (*f1)(), c, x0, tol; +int *err; +{ double y; + do + { y = f(x0)-c; + x0 -= y/f1(x0); + } while (fabs(y)>tol); + return(x0); +} + +double solve_fp(f,x0,tol,maxit) +double (*f)(), x0, tol; +int maxit; +{ double x1; + int i; + for (i=0; i<maxit; i++) + { x1 = f(x0); + if (fabs(x1-x0)<tol) return(x1); + x0 = x1; + } + return(x1); /* although it hasn't converged */ +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include "mut.h" + +void svd(x,p,q,d,mxit) /* svd of square matrix */ +double *x, *p, *q; +int d, mxit; +{ int i, j, k, iter, ms, zer; + double r, u, v, cp, cm, sp, sm, c1, c2, s1, s2, mx; + for (i=0; i<d; i++) + for (j=0; j<d; j++) p[i*d+j] = q[i*d+j] = (i==j); + for (iter=0; iter<mxit; iter++) + { ms = 0; + for (i=0; i<d; i++) + for (j=i+1; j<d; j++) + { s1 = fabs(x[i*d+j]); + s2 = fabs(x[j*d+i]); + mx = (s1>s2) ? s1 : s2; + zer = 1; + if (mx*mx>1.0e-15*fabs(x[i*d+i]*x[j*d+j])) + { if (fabs(x[i*(d+1)])<fabs(x[j*(d+1)])) + { for (k=0; k<d; k++) + { u = x[i*d+k]; x[i*d+k] = x[j*d+k]; x[j*d+k] = u; + u = p[k*d+i]; p[k*d+i] = p[k*d+j]; p[k*d+j] = u; + } + for (k=0; k<d; k++) + { u = x[k*d+i]; x[k*d+i] = x[k*d+j]; x[k*d+j] = u; + u = q[k*d+i]; q[k*d+i] = q[k*d+j]; q[k*d+j] = u; + } + } + cp = x[i*(d+1)]+x[j*(d+1)]; + sp = x[j*d+i]-x[i*d+j]; + r = sqrt(cp*cp+sp*sp); + if (r>0) { cp /= r; sp /= r; } + else { cp = 1.0; zer = 0;} + cm = x[i*(d+1)]-x[j*(d+1)]; + sm = x[i*d+j]+x[j*d+i]; + r = sqrt(cm*cm+sm*sm); + if (r>0) { cm /= r; sm /= r; } + else { cm = 1.0; zer = 0;} + c1 = cm+cp; + s1 = sm+sp; + r = sqrt(c1*c1+s1*s1); + if (r>0) { c1 /= r; s1 /= r; } + else { c1 = 1.0; zer = 0;} + if (fabs(s1)>ms) ms = fabs(s1); + c2 = cm+cp; + s2 = sp-sm; + r = sqrt(c2*c2+s2*s2); + if (r>0) { c2 /= r; s2 /= r; } + else { c2 = 1.0; zer = 0;} + for (k=0; k<d; k++) + { u = x[i*d+k]; v = x[j*d+k]; + x[i*d+k] = c1*u+s1*v; + x[j*d+k] = c1*v-s1*u; + u = p[k*d+i]; v = p[k*d+j]; + p[k*d+i] = c1*u+s1*v; + p[k*d+j] = c1*v-s1*u; + } + for (k=0; k<d; k++) + { u = x[k*d+i]; v = x[k*d+j]; + x[k*d+i] = c2*u-s2*v; + x[k*d+j] = s2*u+c2*v; + u = q[k*d+i]; v = q[k*d+j]; + q[k*d+i] = c2*u-s2*v; + q[k*d+j] = s2*u+c2*v; + } + if (zer) x[i*d+j] = x[j*d+i] = 0.0; + ms = 1; + } + } + if (ms==0) iter=mxit+10; + } + if (iter==mxit) mut_printf("Warning: svd not converged.\n"); + for (i=0; i<d; i++) + if (x[i*d+i]<0) + { x[i*d+i] = -x[i*d+i]; + for (j=0; j<d; j++) p[j*d+i] = -p[j*d+i]; + } +} + +int svdsolve(x,w,P,D,Q,d,tol) /* original X = PDQ^T; comp. QD^{-1}P^T x */ +double *x, *w, *P, *D, *Q, tol; +int d; +{ int i, j, rank; + double mx; + if (tol>0) + { mx = D[0]; + for (i=1; i<d; i++) if (D[i*(d+1)]>mx) mx = D[i*(d+1)]; + tol *= mx; + } + rank = 0; + for (i=0; i<d; i++) + { w[i] = 0.0; + for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j]; + } + for (i=0; i<d; i++) + if (D[i*d+i]>tol) + { w[i] /= D[i*(d+1)]; + rank++; + } + for (i=0; i<d; i++) + { x[i] = 0.0; + for (j=0; j<d; j++) x[i] += Q[i*d+j]*w[j]; + } + return(rank); +} + +void hsvdsolve(x,w,P,D,Q,d,tol) /* original X = PDQ^T; comp. D^{-1/2}P^T x */ +double *x, *w, *P, *D, *Q, tol; +int d; +{ int i, j; + double mx; + if (tol>0) + { mx = D[0]; + for (i=1; i<d; i++) if (D[i*(d+1)]>mx) mx = D[i*(d+1)]; + tol *= mx; + } + for (i=0; i<d; i++) + { w[i] = 0.0; + for (j=0; j<d; j++) w[i] += P[j*d+i]*x[j]; + } + for (i=0; i<d; i++) if (D[i*d+i]>tol) w[i] /= sqrt(D[i*(d+1)]); + for (i=0; i<d; i++) x[i] = w[i]; +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Includes some miscellaneous vector functions: + * setzero(v,p) sets all elements of v to 0. + * unitvec(x,k,p) sets x to k'th unit vector e_k. + * innerprod(v1,v2,p) inner product. + * addouter(A,v1,v2,p,c) A <- A + c * v_1 v2^T + * multmatscal(A,z,n) A <- A*z + * matrixmultiply(A,B,C,m,n,p) C(m*p) <- A(m*n) * B(n*p) + * transpose(x,m,n) inline transpose + * m_trace(x,n) trace + * vecsum(x,n) sum elements. + */ + +#include "mut.h" + +void setzero(v,p) +double *v; +int p; +{ int i; + for (i=0; i<p; i++) v[i] = 0.0; +} + +void unitvec(x,k,p) +double *x; +int k, p; +{ setzero(x,p); + x[k] = 1.0; +} + +double innerprod(v1,v2,p) +double *v1, *v2; +int p; +{ int i; + double s; + s = 0; + for (i=0; i<p; i++) s += v1[i]*v2[i]; + return(s); +} + +void addouter(A,v1,v2,p,c) +double *A, *v1, *v2, c; +int p; +{ int i, j; + for (i=0; i<p; i++) + for (j=0; j<p; j++) + A[i*p+j] += c*v1[i]*v2[j]; +} + +void multmatscal(A,z,n) +double *A, z; +int n; +{ int i; + for (i=0; i<n; i++) A[i] *= z; +} + +/* matrix multiply A (m*n) times B (n*p). + * store in C (m*p). + * all matrices stored by column. + */ +void matrixmultiply(A,B,C,m,n,p) +double *A, *B, *C; +int m, n, p; +{ int i, j, k, ij; + for (i=0; i<m; i++) + for (j=0; j<p; j++) + { ij = j*m+i; + C[ij] = 0.0; + for (k=0; k<n; k++) + C[ij] += A[k*m+i] * B[j*n+k]; + } +} + +/* + * transpose() transposes an m*n matrix in place. + * At input, the matrix has n rows, m columns and + * x[0..n-1] is the is the first column. + * At output, the matrix has m rows, n columns and + * x[0..m-1] is the first column. + */ +void transpose(x,m,n) +double *x; +int m, n; +{ int t0, t, ti, tj; + double z; + for (t0=1; t0<m*n-2; t0++) + { ti = t0%m; tj = t0/m; + do + { t = ti*n+tj; + ti= t%m; + tj= t/m; + } while (t<t0); + z = x[t]; + x[t] = x[t0]; + x[t0] = z; + } +} + +/* trace of an n*n square matrix. */ +double m_trace(x,n) +double *x; +int n; +{ int i; + double sum; + sum = 0; + for (i=0; i<n; i++) + sum += x[i*(n+1)]; + return(sum); +} + +double vecsum(v,n) +double *v; +int n; +{ int i; + double sum; + sum = 0.0; + for (i=0; i<n; i++) sum += v[i]; + return(sum); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + miscellaneous functions that may not be defined in the math + libraries. The implementations are crude. + mut_daws(x) -- dawson's function + mut_exp(x) -- exp(x), but it won't overflow. + + where required, these must be #define'd in header files. + + also includes + ptail(x) -- exp(x*x/2)*int_{-\infty}^x exp(-u^2/2)du for x < -6. + logit(x) -- logistic function. + expit(x) -- inverse of logit. + factorial(n)-- factorial + */ + +#include "mut.h" + +double mut_exp(x) +double x; +{ if (x>700.0) return(1.014232054735004e+304); + return(exp(x)); +} + +double mut_daws(x) +double x; +{ static double val[] = { + 0, 0.24485619356002, 0.46034428261948, 0.62399959848185, 0.72477845900708, + 0.76388186132749, 0.75213621001998, 0.70541701910853, 0.63998807456541, + 0.56917098836654, 0.50187821196415, 0.44274283060424, 0.39316687916687, + 0.35260646480842, 0.31964847250685, 0.29271122077502, 0.27039629581340, + 0.25160207761769, 0.23551176224443, 0.22153505358518, 0.20924575719548, + 0.19833146819662, 0.18855782729305, 0.17974461154688, 0.17175005072385 }; + double h, f0, f1, f2, y, z, xx; + int j, m; + if (x<0) return(-mut_daws(-x)); + if (x>6) + { /* Tail series: 1/x + 1/x^3 + 1.3/x^5 + 1.3.5/x^7 + ... */ + y = z = 1/x; + j = 0; + while (((f0=(2*j+1)/(x*x))<1) && (y>1.0e-10*z)) + { y *= f0; + z += y; + j++; + } + return(z); + } + m = (int) (4*x); + h = x-0.25*m; + if (h>0.125) + { m++; + h = h-0.25; + } + xx = 0.25*m; + f0 = val[m]; + f1 = 1-xx*f0; + z = f0+h*f1; + y = h; + j = 2; + while (fabs(y)>z*1.0e-10) + { f2 = -(j-1)*f0-xx*f1; + y *= h/j; + z += y*f2; + f0 = f1; f1 = f2; + j++; + } + return(z); +} + +double ptail(x) /* exp(x*x/2)*int_{-\infty}^x exp(-u^2/2)du for x < -6 */ +double x; +{ double y, z, f0; + int j; + y = z = -1.0/x; + j = 0; + while ((fabs(f0= -(2*j+1)/(x*x))<1) && (fabs(y)>1.0e-10*z)) + { y *= f0; + z += y; + j++; + } + return(z); +} + +double logit(x) +double x; +{ return(log(x/(1-x))); +} + +double expit(x) +double x; +{ double u; + if (x<0) + { u = exp(x); + return(u/(1+u)); + } + return(1/(1+exp(-x))); +} + +int factorial(n) +int n; +{ if (n<=1) return(1.0); + return(n*factorial(n-1)); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * Constrained maximization of a bivariate function. + * maxbvgrid(f,x,ll,ur,m0,m1) + * maximizes over a grid of m0*m1 points. Returns the maximum, + * and the maximizer through the array x. Usually this is a starter, + * to choose between local maxima, followed by other routines to refine. + * + * maxbvstep(f,x,ymax,h,ll,ur,err) + * essentially multivariate bisection. A 3x3 grid of points is + * built around the starting value (x,ymax). This grid is moved + * around (step size h[0] and h[1] in the two dimensions) until + * the maximum is in the middle. Then, the step size is halved. + * Usually, this will be called in a loop. + * The error flag is set if the maximum can't be centered in a + * reasonable number of steps. + * + * maxbv(f,x,h,ll,ur,m0,m1,tol) + * combines the two previous functions. It begins with a grid search + * (if m0>0 and m1>0), followed by refinement. Refines until both h + * components are < tol. + */ +#include "mut.h" + +#define max(a,b) ((a)>(b) ? (a) : (b)) +#define min(a,b) ((a)<(b) ? (a) : (b)) + +double maxbvgrid(f,x,ll,ur,m0,m1,con) +double (*f)(), *x, *ll, *ur; +int m0, m1, *con; +{ int i, j, im, jm; + double y, ymax; + + im = -1; + for (i=0; i<=m0; i++) + { x[0] = ((m0-i)*ll[0] + i*ur[0])/m0; + for (j=0; j<=m1; j++) + { x[1] = ((m1-j)*ll[1] + j*ur[1])/m1; + y = f(x); + if ((im==-1) || (y>ymax)) + { im = i; jm = j; + ymax = y; + } + } + } + + x[0] = ((m0-im)*ll[0] + im*ur[0])/m0; + x[1] = ((m1-jm)*ll[1] + jm*ur[1])/m1; + con[0] = (im==m0)-(im==0); + con[1] = (jm==m1)-(jm==0); + return(ymax); +} + +double maxbvstep(f,x,ymax,h,ll,ur,err,con) +double (*f)(), *x, ymax, *h, *ll, *ur; +int *err, *con; +{ int i, j, ij, imax, steps, cts[2]; + double newx, X[9][2], y[9]; + + imax =4; y[4] = ymax; + + for (i=(con[0]==-1)-1; i<2-(con[0]==1); i++) + for (j=(con[1]==-1)-1; j<2-(con[1]==1); j++) + { ij = 3*i+j+4; + X[ij][0] = x[0]+i*h[0]; + if (X[ij][0] < ll[0]+0.001*h[0]) X[ij][0] = ll[0]; + if (X[ij][0] > ur[0]-0.001*h[0]) X[ij][0] = ur[0]; + X[ij][1] = x[1]+j*h[1]; + if (X[ij][1] < ll[1]+0.001*h[1]) X[ij][1] = ll[1]; + if (X[ij][1] > ur[1]-0.001*h[1]) X[ij][1] = ur[1]; + if (ij != 4) + { y[ij] = f(X[ij]); + if (y[ij]>ymax) { imax = ij; ymax = y[ij]; } + } + } + + steps = 0; + cts[0] = cts[1] = 0; + while ((steps<20) && (imax != 4)) + { steps++; + if ((con[0]>-1) && ((imax/3)==0)) /* shift right */ + { + cts[0]--; + for (i=8; i>2; i--) + { X[i][0] = X[i-3][0]; y[i] = y[i-3]; + } + imax = imax+3; + if (X[imax][0]==ll[0]) + con[0] = -1; + else + { newx = X[imax][0]-h[0]; + if (newx < ll[0]+0.001*h[0]) newx = ll[0]; + for (i=(con[1]==-1); i<3-(con[1]==1); i++) + { X[i][0] = newx; + y[i] = f(X[i]); + if (y[i]>ymax) { ymax = y[i]; imax = i; } + } + con[0] = 0; + } + } + + if ((con[0]<1) && ((imax/3)==2)) /* shift left */ + { + cts[0]++; + for (i=0; i<6; i++) + { X[i][0] = X[i+3][0]; y[i] = y[i+3]; + } + imax = imax-3; + if (X[imax][0]==ur[0]) + con[0] = 1; + else + { newx = X[imax][0]+h[0]; + if (newx > ur[0]-0.001*h[0]) newx = ur[0]; + for (i=6+(con[1]==-1); i<9-(con[1]==1); i++) + { X[i][0] = newx; + y[i] = f(X[i]); + if (y[i]>ymax) { ymax = y[i]; imax = i; } + } + con[0] = 0; + } + } + + if ((con[1]>-1) && ((imax%3)==0)) /* shift up */ + { + cts[1]--; + for (i=9; i>0; i--) if (i%3 > 0) + { X[i][1] = X[i-1][1]; y[i] = y[i-1]; + } + imax = imax+1; + if (X[imax][1]==ll[1]) + con[1] = -1; + else + { newx = X[imax][1]-h[1]; + if (newx < ll[1]+0.001*h[1]) newx = ll[1]; + for (i=3*(con[0]==-1); i<7-(con[0]==1); i=i+3) + { X[i][1] = newx; + y[i] = f(X[i]); + if (y[i]>ymax) { ymax = y[i]; imax = i; } + } + con[1] = 0; + } + } + + if ((con[1]<1) && ((imax%3)==2)) /* shift down */ + { + cts[1]++; + for (i=0; i<9; i++) if (i%3 < 2) + { X[i][1] = X[i+1][1]; y[i] = y[i+1]; + } + imax = imax-1; + if (X[imax][1]==ur[1]) + con[1] = 1; + else + { newx = X[imax][1]+h[1]; + if (newx > ur[1]-0.001*h[1]) newx = ur[1]; + for (i=2+3*(con[0]==-1); i<9-(con[0]==1); i=i+3) + { X[i][1] = newx; + y[i] = f(X[i]); + if (y[i]>ymax) { ymax = y[i]; imax = i; } + } + con[1] = 0; + } + } +/* if we've taken 3 steps in one direction, try increasing the + * corresponding h. + */ + if ((cts[0]==-2) | (cts[0]==2)) + { h[0] = 2*h[0]; cts[0] = 0; } + if ((cts[1]==-2) | (cts[1]==2)) + { h[1] = 2*h[1]; cts[1] = 0; } + } + + if (steps==40) + *err = 1; + else + { + h[0] /= 2.0; h[1] /= 2.0; + *err = 0; + } + + x[0] = X[imax][0]; + x[1] = X[imax][1]; + return(y[imax]); +} + +#define BQMmaxp 5 + +int boxquadmin(J,b0,p,x0,ll,ur) +jacobian *J; +double *b0, *x0, *ll, *ur; +int p; +{ double b[BQMmaxp], x[BQMmaxp], L[BQMmaxp*BQMmaxp], C[BQMmaxp*BQMmaxp], d[BQMmaxp]; + double f, fmin; + int i, imin, m, con[BQMmaxp], rlx; + + if (p>BQMmaxp) mut_printf("boxquadmin: maxp is 5.\n"); + if (J->st != JAC_RAW) mut_printf("boxquadmin: must start with JAC_RAW.\n"); + + m = 0; + setzero(L,p*p); + setzero(x,p); + memcpy(C,J->Z,p*p*sizeof(double)); + for (i=0; i<p; i++) con[i] = 0; + + do + { +/* first, keep minimizing and add constraints, one at a time. + */ + do + { + matrixmultiply(C,x,b,p,p,1); + for (i=0; i<p; i++) b[i] += b0[i]; + conquadmin(J,b,p,L,d,m); +/* if C matrix is not pd, don't even bother. + * this relies on having used cholesky dec. + */ +if ((J->Z[0]==0.0) | (J->Z[3]==0.0)) return(1); + fmin = 1.0; + for (i=0; i<p; i++) if (con[i]==0) + { f = 1.0; + if (x0[i]+x[i]+b[i] < ll[i]) f = (ll[i]-x[i]-x0[i])/b[i]; + if (x0[i]+x[i]+b[i] > ur[i]) f = (ur[i]-x[i]-x0[i])/b[i]; + if (f<fmin) fmin = f; + imin = i; + } + for (i=0; i<p; i++) x[i] += fmin*b[i]; + if (fmin<1.0) + { L[m*p+imin] = 1; + m++; + con[imin] = (b[imin]<0) ? -1 : 1; + } + } while ((fmin < 1.0) & (m<p)); + +/* now, can I relax any constraints? + * compute slopes at current point. Can relax if: + * slope is -ve on a lower boundary. + * slope is +ve on an upper boundary. + */ + rlx = 0; + if (m>0) + { matrixmultiply(C,x,b,p,p,1); + for (i=0; i<p; i++) b[i] += b0[i]; + for (i=0; i<p; i++) + { if ((con[i]==-1)&& (b[i]<0)) { con[i] = 0; rlx = 1; } + if ((con[i]==1) && (b[i]>0)) { con[i] = 0; rlx = 1; } + } + + if (rlx) /* reconstruct the constraint matrix */ + { setzero(L,p*p); m = 0; + for (i=0; i<p; i++) if (con[i] != 0) + { L[m*p+i] = 1; + m++; + } + } + } + } while (rlx); + + memcpy(b0,x,p*sizeof(double)); /* this is how far we should move from x0 */ + return(0); +} + +double maxquadstep(f,x,ymax,h,ll,ur,err,con) +double (*f)(), *x, ymax, *h, *ll, *ur; +int *err, *con; +{ jacobian J; + double b[2], c[2], d, jwork[12]; + double x0, x1, y0, y1, ym, h0, xl[2], xu[2], xi[2]; + int i, m; + + *err = 0; + +/* first, can we relax any of the initial constraints? + * if so, just do one step away from the boundary, and + * return for restart. + */ + for (i=0; i<2; i++) + if (con[i] != 0) + { xi[0] = x[0]; xi[1] = x[1]; + xi[i] = x[i]-con[i]*h[i]; + y0 = f(xi); + if (y0>ymax) + { memcpy(x,xi,2*sizeof(double)); + con[i] = 0; + return(y0); + } + } + +/* now, all initial constraints remain active. + */ + + m = 9; + for (i=0; i<2; i++) if (con[i]==0) + { m /= 3; + xl[0] = x[0]; xl[1] = x[1]; + xl[i] = max(x[i]-h[i],ll[i]); y0 = f(xl); + x0 = xl[i]-x[i]; y0 -= ymax; + xu[0] = x[0]; xu[1] = x[1]; + xu[i] = min(x[i]+h[i],ur[i]); y1 = f(xu); + x1 = xu[i]-x[i]; y1 -= ymax; + if (x0*x1*(x1-x0)==0) { *err = 1; return(0.0); } + b[i] = (x0*x0*y1-x1*x1*y0)/(x0*x1*(x0-x1)); + c[i] = 2*(x0*y1-x1*y0)/(x0*x1*(x1-x0)); + if (c[i] >= 0.0) { *err = 1; return(0.0); } + xi[i] = (b[i]<0) ? xl[i] : xu[i]; + } + else { c[i] = -1.0; b[i] = 0.0; } /* enforce initial constraints */ + + if ((con[0]==0) && (con[1]==0)) + { x0 = xi[0]-x[0]; + x1 = xi[1]-x[1]; + ym = f(xi) - ymax - b[0]*x0 - c[0]*x0*x0/2 - b[1]*x1 - c[1]*x1*x1/2; + d = ym/(x0*x1); + } + else d = 0.0; + +/* now, maximize the quadratic. + * y[4] + b0*x0 + b1*x1 + 0.5(c0*x0*x0 + c1*x1*x1 + 2*d*x0*x1) + * -ve everything, to call quadmin. + */ + jac_alloc(&J,2,jwork); + J.Z[0] = -c[0]; + J.Z[1] = J.Z[2] = -d; + J.Z[3] = -c[1]; + J.st = JAC_RAW; + J.p = 2; + b[0] = -b[0]; b[1] = -b[1]; + *err = boxquadmin(&J,b,2,x,ll,ur); + if (*err) return(ymax); + +/* test to see if this step successfully increases... + */ + for (i=0; i<2; i++) + { xi[i] = x[i]+b[i]; + if (xi[i]<ll[i]+1e-8*h[i]) xi[i] = ll[i]; + if (xi[i]>ur[i]-1e-8*h[i]) xi[i] = ur[i]; + } + y1 = f(xi); + if (y1 < ymax) /* no increase */ + { *err = 1; + return(ymax); + } + +/* wonderful. update x, h, with the restriction that h can't decrease + * by a factor over 10, or increase by over 2. + */ + for (i=0; i<2; i++) + { x[i] = xi[i]; + if (x[i]==ll[i]) con[i] = -1; + if (x[i]==ur[i]) con[i] = 1; + h0 = fabs(b[i]); + h0 = min(h0,2*h[i]); + h0 = max(h0,h[i]/10); + h[i] = min(h0,(ur[i]-ll[i])/2.0); + } + return(y1); +} + +double maxbv(f,x,h,ll,ur,m0,m1,tol) +double (*f)(), *x, *h, *ll, *ur, tol; +int m0, m1; +{ double ymax; + int err, con[2]; + + con[0] = con[1] = 0; + if ((m0>0) & (m1>0)) + { + ymax = maxbvgrid(f,x,ll,ur,m0,m1,con); + h[0] = (ur[0]-ll[0])/(2*m0); + h[1] = (ur[1]-ll[1])/(2*m1); + } + else + { x[0] = (ll[0]+ur[0])/2; + x[1] = (ll[1]+ur[1])/2; + h[0] = (ur[0]-ll[0])/2; + h[1] = (ur[1]-ll[1])/2; + ymax = f(x); + } + + while ((h[0]>tol) | (h[1]>tol)) + { ymax = maxbvstep(f,x,ymax,h,ll,ur,&err,con); + if (err) mut_printf("maxbvstep failure\n"); + } + + return(ymax); +} + +double maxbvq(f,x,h,ll,ur,m0,m1,tol) +double (*f)(), *x, *h, *ll, *ur, tol; +int m0, m1; +{ double ymax; + int err, con[2]; + + con[0] = con[1] = 0; + if ((m0>0) & (m1>0)) + { + ymax = maxbvgrid(f,x,ll,ur,m0,m1,con); + h[0] = (ur[0]-ll[0])/(2*m0); + h[1] = (ur[1]-ll[1])/(2*m1); + } + else + { x[0] = (ll[0]+ur[0])/2; + x[1] = (ll[1]+ur[1])/2; + h[0] = (ur[0]-ll[0])/2; + h[1] = (ur[1]-ll[1])/2; + ymax = f(x); + } + + while ((h[0]>tol) | (h[1]>tol)) + { /* first, try a quadratric step */ + ymax = maxquadstep(f,x,ymax,h,ll,ur,&err,con); + /* if the quadratic step fails, move the grid around */ + if (err) + { + ymax = maxbvstep(f,x,ymax,h,ll,ur,&err,con); + if (err) + { mut_printf("maxbvstep failure\n"); + return(ymax); + } + } + } + + return(ymax); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include "mut.h" + +prf mut_printf = (prf)printf; + +void mut_redirect(newprf) +prf newprf; +{ mut_printf = newprf; +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +/* + * function to find order of observations in an array. + * + * mut_order(x,ind,i0,i1) + * x array to find order of. + * ind integer array of indexes. + * i0,i1 (integers) range to order. + * + * at output, ind[i0...i1] are permuted so that + * x[ind[i0]] <= x[ind[i0+1]] <= ... <= x[ind[i1]]. + * (with ties, output order of corresponding indices is arbitrary). + * The array x is unchanged. + * + * Typically, if x has length n, then i0=0, i1=n-1 and + * ind is (any permutation of) 0...n-1. + */ + +#include "mut.h" + +double med3(x0,x1,x2) +double x0, x1, x2; +{ if (x0<x1) + { if (x2<x0) return(x0); + if (x1<x2) return(x1); + return(x2); + } +/* x1 < x0 */ + if (x2<x1) return(x1); + if (x0<x2) return(x0); + return(x2); +} + +void mut_order(x,ind,i0,i1) +double *x; +int *ind, i0, i1; +{ double piv; + int i, l, r, z; + + if (i1<=i0) return; + piv = med3(x[ind[i0]],x[ind[i1]],x[ind[(i0+i1)/2]]); + l = i0; r = i0-1; + +/* at each stage, + * x[i0..l-1] < piv + * x[l..r] = piv + * x[r+1..i-1] > piv + * then, decide where to put x[i]. + */ + for (i=i0; i<=i1; i++) + { if (x[ind[i]]==piv) + { r++; + z = ind[i]; ind[i] = ind[r]; ind[r] = z; + } + else if (x[ind[i]]<piv) + { r++; + z = ind[i]; ind[i] = ind[r]; ind[r] = ind[l]; ind[l] = z; + l++; + } + } + + if (l>i0) mut_order(x,ind,i0,l-1); + if (r<i1) mut_order(x,ind,r+1,i1); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include "mut.h" + +#define LOG_2 0.6931471805599453094172321214581765680755 +#define IBETA_LARGE 1.0e30 +#define IBETA_SMALL 1.0e-30 +#define IGAMMA_LARGE 1.0e30 +#define DOUBLE_EP 2.2204460492503131E-16 + +double ibeta(x, a, b) +double x, a, b; +{ int flipped = 0, i, k, count; + double I = 0, temp, pn[6], ak, bk, next, prev, factor, val; + if (x <= 0) return(0); + if (x >= 1) return(1); +/* use ibeta(x,a,b) = 1-ibeta(1-x,b,z) */ + if ((a+b+1)*x > (a+1)) + { flipped = 1; + temp = a; + a = b; + b = temp; + x = 1 - x; + } + pn[0] = 0.0; + pn[2] = pn[3] = pn[1] = 1.0; + count = 1; + val = x/(1.0-x); + bk = 1.0; + next = 1.0; + do + { count++; + k = count/2; + prev = next; + if (count%2 == 0) + ak = -((a+k-1.0)*(b-k)*val)/((a+2.0*k-2.0)*(a+2.0*k-1.0)); + else + ak = ((a+b+k-1.0)*k*val)/((a+2.0*k)*(a+2.0*k-1.0)); + pn[4] = bk*pn[2] + ak*pn[0]; + pn[5] = bk*pn[3] + ak*pn[1]; + next = pn[4] / pn[5]; + for (i=0; i<=3; i++) + pn[i] = pn[i+2]; + if (fabs(pn[4]) >= IBETA_LARGE) + for (i=0; i<=3; i++) + pn[i] /= IBETA_LARGE; + if (fabs(pn[4]) <= IBETA_SMALL) + for (i=0; i<=3; i++) + pn[i] /= IBETA_SMALL; + } while (fabs(next-prev) > DOUBLE_EP*prev); + /* factor = a*log(x) + (b-1)*log(1-x); + factor -= mut_lgamma(a+1) + mut_lgamma(b) - mut_lgamma(a+b); */ + factor = dbeta(x,a,b,1) + log(x/a); + I = exp(factor) * next; + return(flipped ? 1-I : I); +} + +/* + * Incomplete gamma function. + * int_0^x u^{df-1} e^{-u} du / Gamma(df). + */ +double igamma(x, df) +double x, df; +{ double factor, term, gintegral, pn[6], rn, ak, bk; + int i, count, k; + if (x <= 0.0) return(0.0); + + if (df < 1.0) + return( dgamma(x,df+1.0,1.0,0) + igamma(x,df+1.0) ); + + factor = x * dgamma(x,df,1.0,0); + /* factor = exp(df*log(x) - x - lgamma(df)); */ + + if (x > 1.0 && x >= df) + { + pn[0] = 0.0; + pn[2] = pn[1] = 1.0; + pn[3] = x; + count = 1; + rn = 1.0 / x; + do + { count++; + k = count / 2; + gintegral = rn; + if (count%2 == 0) + { bk = 1.0; + ak = (double)k - df; + } else + { bk = x; + ak = (double)k; + } + pn[4] = bk*pn[2] + ak*pn[0]; + pn[5] = bk*pn[3] + ak*pn[1]; + rn = pn[4] / pn[5]; + for (i=0; i<4; i++) + pn[i] = pn[i+2]; + if (pn[4] > IGAMMA_LARGE) + for (i=0; i<4; i++) + pn[i] /= IGAMMA_LARGE; + } while (fabs(gintegral-rn) > DOUBLE_EP*rn); + gintegral = 1.0 - factor*rn; + } + else + { /* For x<df, use the series + * dpois(df,x)*( 1 + x/(df+1) + x^2/((df+1)(df+2)) + ... ) + * This could be slow if df large and x/df is close to 1. + */ + gintegral = term = 1.0; + rn = df; + do + { rn += 1.0; + term *= x/rn; + gintegral += term; + } while (term > DOUBLE_EP*gintegral); + gintegral *= factor/df; + } + return(gintegral); +} + +double pf(q, df1, df2) +double q, df1, df2; +{ return(ibeta(q*df1/(df2+q*df1), df1/2, df2/2)); +} +/* + * Copyright 1996-2006 Catherine Loader. + */ +#include "mut.h" +#include <string.h> + +/* quadmin: minimize the quadratic, + * 2<x,b> + x^T A x. + * x = -A^{-1} b. + * + * conquadmin: min. subject to L'x = d (m constraints) + * x = -A^{-1}(b+Ly) (y = Lagrange multiplier) + * y = -(L'A^{-1}L)^{-1} (L'A^{-1}b) + * x = -A^{-1}b + A^{-1}L (L'A^{-1}L)^{-1} [(L'A^{-1})b + d] + * (non-zero d to be added!!) + * + * qprogmin: min. subject to L'x >= 0. + */ + +void quadmin(J,b,p) +jacobian *J; +double *b; +int p; +{ int i; + jacob_dec(J,JAC_CHOL); + i = jacob_solve(J,b); + if (i<p) mut_printf("quadmin singular %2d %2d\n",i,p); + for (i=0; i<p; i++) b[i] = -b[i]; +} + +/* project vector a (length n) onto + * columns of X (n rows, m columns, organized by column). + * store result in y. + */ +#define pmaxm 10 +#define pmaxn 100 +void project(a,X,y,n,m) +double *a, *X, *y; +int n, m; +{ double xta[pmaxm], R[pmaxn*pmaxm]; + int i; + + if (n>pmaxn) mut_printf("project: n too large\n"); + if (m>pmaxm) mut_printf("project: m too large\n"); + + for (i=0; i<m; i++) xta[i] = innerprod(a,&X[i*n],n); + memcpy(R,X,m*n*sizeof(double)); + qr(R,n,m,NULL); + qrsolv(R,xta,n,m); + + matrixmultiply(X,xta,y,n,m,1); +} + +void resproj(a,X,y,n,m) +double *a, *X, *y; +int n, m; +{ int i; + project(a,X,y,n,m); + for (i=0; i<n; i++) y[i] = a[i]-y[i]; +} + +/* x = -A^{-1}b + A^{-1}L (L'A^{-1}L)^{-1} [(L'A^{-1})b + d] */ +void conquadmin(J,b,n,L,d,m) +jacobian *J; +double *b, *L, *d; +int m, n; +{ double bp[10], L0[100]; + int i, j; + + if (n>10) mut_printf("conquadmin: max. n is 10.\n"); + memcpy(L0,L,n*m*sizeof(double)); + jacob_dec(J,JAC_CHOL); + for (i=0; i<m; i++) jacob_hsolve(J,&L[i*n]); + jacob_hsolve(J,b); + + resproj(b,L,bp,n,m); + + jacob_isolve(J,bp); + for (i=0; i<n; i++) b[i] = -bp[i]; + + qr(L,n,m,NULL); + qrsolv(L,d,n,m); + for (i=0; i<n; i++) + { bp[i] = 0; + for (j=0; j<m; j++) bp[i] += L0[j*n+i]*d[j]; + } + jacob_solve(J,bp); + for (i=0; i<n; i++) b[i] += bp[i]; +} + +void qactivemin(J,b,n,L,d,m,ac) +jacobian *J; +double *b, *L, *d; +int m, n, *ac; +{ int i, nac; + double M[100], dd[10]; + nac = 0; + for (i=0; i<m; i++) if (ac[i]>0) + { memcpy(&M[nac*n],&L[i*n],n*sizeof(double)); + dd[nac] = d[i]; + nac++; + } + conquadmin(J,b,n,M,dd,nac); +} + +/* return 1 for full step; 0 if new constraint imposed. */ +int movefrom(x0,x,n,L,d,m,ac) +double *x0, *x, *L, *d; +int n, m, *ac; +{ int i, imin; + double c0, c1, lb, lmin; + lmin = 1.0; + for (i=0; i<m; i++) if (ac[i]==0) + { c1 = innerprod(&L[i*n],x,n)-d[i]; + if (c1<0.0) + { c0 = innerprod(&L[i*n],x0,n)-d[i]; + if (c0<0.0) + { if (c1<c0) { lmin = 0.0; imin = 1; } + } + else + { lb = c0/(c0-c1); + if (lb<lmin) { lmin = lb; imin = i; } + } + } + } + for (i=0; i<n; i++) + x0[i] = lmin*x[i]+(1-lmin)*x0[i]; + if (lmin==1.0) return(1); + ac[imin] = 1; + return(0); +} + +int qstep(J,b,x0,n,L,d,m,ac,deac) +jacobian *J; +double *b, *x0, *L, *d; +int m, n, *ac, deac; +{ double x[10]; + int i; + + if (m>10) mut_printf("qstep: too many constraints.\n"); + if (deac) + { for (i=0; i<m; i++) if (ac[i]==1) + { ac[i] = 0; + memcpy(x,b,n*sizeof(double)); + qactivemin(J,x,n,L,d,m,ac); + if (innerprod(&L[i*n],x,n)>d[i]) /* deactivate this constraint; should rem. */ + i = m+10; + else + ac[i] = 1; + } + if (i==m) return(0); /* no deactivation possible */ + } + + do + { if (!deac) + { memcpy(x,b,n*sizeof(double)); + qactivemin(J,x,n,L,d,m,ac); + } + i = movefrom(x0,x,n,L,d,m,ac); + + deac = 0; + } while (i==0); + return(1); +} + +/* + * x0 is starting value; should satisfy constraints. + * L is n*m constraint matrix. + * ac is active constraint vector: + * ac[i]=0, inactive. + * ac[i]=1, active, but can be deactivated. + * ac[i]=2, active, cannot be deactivated. + */ + +void qprogmin(J,b,x0,n,L,d,m,ac) +jacobian *J; +double *b, *x0, *L, *d; +int m, n, *ac; +{ int i; + for (i=0; i<m; i++) if (ac[i]==0) + { if (innerprod(&L[i*n],x0,n) < d[i]) ac[i] = 1; } + jacob_dec(J,JAC_CHOL); + qstep(J,b,x0,n,L,d,m,ac,0); + while (qstep(J,b,x0,n,L,d,m,ac,1)); +} + +void qpm(A,b,x0,n,L,d,m,ac) +double *A, *b, *x0, *L, *d; +int *n, *m, *ac; +{ jacobian J; + double wk[1000]; + jac_alloc(&J,*n,wk); + memcpy(J.Z,A,(*n)*(*n)*sizeof(double)); + J.p = *n; + J.st = JAC_RAW; + qprogmin(&J,b,x0,*n,L,d,*m,ac); +}