size_t rawDataWrite(RawData *raw_data, const offset_t offset, const void *src, size_t size, size_t nmemb)
{
size_t count;
size_t len;
if(raw_data->max_size <= offset)
{
dbg_log(SEC_0132_RAW, 0)(LOGSTDOUT, "error:rawDataWrite: max_size %d, but access offset %d overflow\n", raw_data->max_size, offset);
return 0;
}
if(0 == size || 0 == nmemb)
{
return 0;
}
if(raw_data->max_size <= offset + size * nmemb)
{
dbg_log(SEC_0132_RAW, 0)(LOGSTDOUT, "error:rawDataWrite: max_size %d, but access offset(%d) + size(%d) * nmemb(%d) = %d overflow\n",
raw_data->max_size, offset, size, nmemb, offset + size * nmemb);
}
count = DMIN(nmemb, (raw_data->max_size - offset)/size);
len = size * count;
BCOPY(src, raw_data->buffer + offset, len);
if(offset + len > raw_data->cur_size)
{
raw_data->cur_size = offset + len;
}
RAWDATA_SET_DIRTY(raw_data);
return count;
}
size_t rawDataPuts(RawData *raw_data, const offset_t offset, const char *src)
{
size_t len;
if(raw_data->max_size <= offset)
{
dbg_log(SEC_0132_RAW, 0)(LOGSTDOUT, "error:rawDataPuts: max_size %d, but access offset %d overflow\n", raw_data->max_size, offset);
return 0;
}
if(raw_data->max_size <= offset + strlen(src))
{
dbg_log(SEC_0132_RAW, 0)(LOGSTDOUT, "error:rawDataPuts: max_size %d, but access offset(%d) + strlen(%d) = %d overflow\n",
raw_data->max_size, offset, strlen(src), offset + strlen(src));
}
len = DMIN(strlen(src), raw_data->max_size - offset);
BCOPY(src, raw_data->buffer + offset, len);
if(offset + len > raw_data->cur_size)
{
raw_data->cur_size = offset + len;
}
RAWDATA_SET_DIRTY(raw_data);
return len;
}
void Sphere::MakeBound( Bound &out_bound ) const
{
float tuMin = mThetamaxRad * mURange[0];
float tuMax = mThetamaxRad * mURange[1];
float alphaMin = DASin( mZMin / mRadius );
float alphaMax = DASin( mZMax / mRadius );
float aVMin = DMix( alphaMin, alphaMax, mVRange[0] );
float aVMax = DMix( alphaMin, alphaMax, mVRange[1] );
float rVMin = DCos( aVMin ) * mRadius;
float rVMax = DCos( aVMax ) * mRadius;
float rMin = DMIN( rVMin, rVMax );
float rMax;
if ( aVMin < 0 && aVMax > 0 )
rMax = mRadius;
else
rMax = DMAX( rVMin, rVMax );
out_bound.Reset();
bounds2DSweepL( out_bound, rMin, rMax, tuMin, tuMax );
out_bound.mBox[0].z() = DSin( aVMin ) * mRadius;
out_bound.mBox[1].z() = DSin( aVMax ) * mRadius;
}
double DMIN_VECTOR(double v[], unsigned long l)
/* Find smallest value in v */
{
unsigned long i;
double m=v[0];
for (i=1;i<l;i++) m=DMIN(m,v[i]);
return(m);
}
void palUnpcd( double disco, double * x, double *y ) {
const double THIRD = 1.0/3.0;
double rp,q,r,d,w,s,t,f,c,t3,f1,f2,f3,w1,w2,w3;
double c2;
/* Distance of the point from the origin. */
rp = sqrt( (*x)*(*x)+(*y)*(*y));
/* If zero, or if no distortion, no action is necessary. */
if (rp != 0.0 && disco != 0.0) {
/* Begin algebraic solution. */
q = 1.0/(3.0*disco);
r = rp/(2.0*disco);
w = q*q*q+r*r;
/* Continue if one real root, or three of which only one is positive. */
if (w > 0.0) {
d = sqrt(w);
w = r+d;
s = COPYSIGN(pow(fabs(w),THIRD),w);
w = r-d;
t = COPYSIGN(pow(fabs(w),THIRD),w);
f = s+t;
} else {
/* Three different real roots: use geometrical method instead. */
w = 2.0/sqrt(-3.0*disco);
c = 4.0*rp/(disco*w*w*w);
c2 = c*c;
s = sqrt(1.0-DMIN(c2,1.0));
t3 = atan2(s,c);
/* The three solutions. */
f1 = w*cos((PAL__D2PI-t3)/3.0);
f2 = w*cos((t3)/3.0);
f3 = w*cos((PAL__D2PI+t3)/3.0);
/* Pick the one that moves [X,Y] least. */
w1 = fabs(f1-rp);
w2 = fabs(f2-rp);
w3 = fabs(f3-rp);
if (w1 < w2) {
f = ( w1 < w3 ? f1 : f3 );
} else {
f = ( w2 < w3 ? f2 : f3 );
}
}
/* Remove the distortion. */
f = f/rp;
*x *= f;
*y *= f;
}
}
double DMIN_MATRIX(double **m, unsigned long naxes[])
/* Find smallest value in m */
{
unsigned long ii,jj;
double min=*(*(m+0)+0);
for (jj=0;jj<naxes[1];jj++)
for (ii=0;ii<naxes[0];ii++)
min=DMIN(min, *(*(m+jj)+ii));
return(min);
}
void Hyperboloid::MakeBound( Bound &out_bound ) const
{
float tuMin = mThetamaxRad * mURange[0];
float tuMax = mThetamaxRad * mURange[1];
Float2_ uvMin( 0.f, mVRange[0] );
Float2_ uvMax( 0.f, mVRange[1] );
Float3_ pMin; EvalP( uvMin, pMin );
Float3_ pMax; EvalP( uvMax, pMax );
out_bound.Reset();
bounds2DSweepP( out_bound, pMin.x()[0], pMin.y()[0], tuMin, tuMax );
bounds2DSweepP( out_bound, pMax.x()[0], pMax.y()[0], tuMin, tuMax );
out_bound.mBox[0].z() = DMIN( pMin.z()[0], pMax.z()[0] );
out_bound.mBox[1].z() = DMAX( pMin.z()[0], pMax.z()[0] );
}
size_t rawDataRead(RawData *raw_data, const offset_t offset, void *des, size_t size, size_t nmemb)
{
size_t count;
size_t len;
if(raw_data->cur_size <= offset)
{
dbg_log(SEC_0132_RAW, 0)(LOGSTDOUT, "error:rawDataRead: cur_size %d, but access offset %d overflow\n", raw_data->cur_size, offset);
return 0;
}
if(raw_data->cur_size < offset + size * nmemb)
{
dbg_log(SEC_0132_RAW, 0)(LOGSTDOUT, "error:rawDataRead: cur_size %d, but access offset(%d) + size(%d) * nmemb(%d) = %d overflow\n",
raw_data->cur_size, offset, size, nmemb, offset + size * nmemb);
}
count = DMIN(nmemb, (raw_data->cur_size - offset)/size);
len = size * count;
BCOPY(raw_data->buffer + offset, des, len);
return count;
}
/**
*
* note:only for chfsnp item!
* return -1 if node < (data, key)
* return 1 if node > (data, key)
* return 0 if node == (data, key)
*
**/
static int __chfsnprb_node_data_cmp(const CHFSNPRB_NODE *node, const uint32_t data, const uint32_t klen, const uint8_t *key)
{
const CHFSNP_ITEM *item;
uint32_t min_len;
int cmp_ret;
if (CHFSNPRB_NODE_DATA(node) < data)
{
return (-1);
}
if (CHFSNPRB_NODE_DATA(node) > data)
{
return (1);
}
item = (const CHFSNP_ITEM *)CHFSNP_RB_NODE_ITEM(node);
min_len = DMIN(CHFSNP_ITEM_K_LEN(item), klen);
cmp_ret = BCMP(CHFSNP_ITEM_KEY(item), key, min_len);
if(0 != cmp_ret)
{
return (cmp_ret);
}
if(CHFSNP_ITEM_K_LEN(item) < klen)
{
return (-1);
}
if(CHFSNP_ITEM_K_LEN(item) < klen)
{
return (1);
}
return (0);
}
//prune this 9/21/2010
int
updategenealogy (int ci, int li, int *topolchange, int *tmrcachange)
{
int ai, mpart, i;
int edge, oldsis, newsis, freededge, accp;
double newpdg, newplg, newpdg_a[MAXLINKED];
double migweight, metropolishastingsterm, U;
double tpw;
double Aterm[MAXLINKED], Atermsum;
double slidedist;
double tlengthpart;
double slideweight, holdslidedist, slidestdv;
struct genealogy *G = &(C[ci]->G[li]);
struct edge *gtree = G->gtree;
int rejectIS;
double like;
double holdt[MAXPERIODS];
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogyloc (ci, li);
}
// initialize and make copies structures that hold quantities for calculating prob of genealogy
copy_treeinfo (&holdgweight_updategenealogy, &G->gweight);
copy_treeinfo (&holdallgweight_updategenealogy, &C[ci]->allgweight);
// store summary stats of the genealogy
storegenealogystats (ci, li, 0);
for (i = 0; i < lastperiodnumber; i++)
holdt[i] = C[ci]->tvals[i]; // JH is this actually necessary ?
*tmrcachange = 0;
*topolchange = 0;
// Atermsum only used for Stepwise mutation model
Atermsum = 0;
/* pick an edge, identify freedup edge (the down edge) and the sister edge */
do
{
edge = randposint (L[li].numlines);
} while (gtree[edge].down == -1);
freededge = gtree[edge].down;
if ((oldsis = gtree[freededge].up[0]) == edge)
oldsis = gtree[freededge].up[1];
/* copy information on the edge, and if it connects to the root, then the sister edge as well */
if (gtree[edge].down == G->root)
{
fillmiginfo (ci, li, gtree, edge, oldsis);
}
else
{
fillmiginfo (ci, li, gtree, edge, -1);
}
/* store information on the genealogy before changing it */
storeoldedges (ci, li, edge, oldsis, freededge);
// remove any migrations from the slidingedge
gtree[edge].mig[0].mt = -1;
/* slide edge, pick a distance and slide it */
slidestdv = DMIN (SLIDESTDVMAX, G->roottime/3 );
holdslidedist = slidedist = normdev (0.0, slidestdv);
// join the sister and the down branches at the point where edge used to connect, this frees up the down branch
joinsisdown (ci, li, oldsis, tmrcachange);
//gtree[edge].down = -1; // not necessary but makes gtreeprint() output easier to read for intermediate stages
//gtree[freededge].down = -1; // not necessary but makes gtreeprint() output easier to read for intermediate stages
// do the slide and identify the new sister branch and where new connection point for the edge is
newsis = oldsis;
if (modeloptions[NOMIGRATION])
slider_nomigration (ci, li, edge, &newsis, &(gtree[edge].time),&slidedist);
else
slider (ci, li, edge, &newsis, &(gtree[edge].time), &slidedist);
*topolchange += (oldsis != newsis);
// now separate the new sister branch into a shorter sis branch and a down branch
splitsisdown (ci, li, edge, freededge, newsis);
if (rootmove)
{
slideweight = -log (normprob (0.0, slidestdv, holdslidedist));
slidestdv = DMIN (SLIDESTDVMAX, G->roottime / 3);
slideweight += log (normprob (0.0, slidestdv, holdslidedist));
}
else
{
slideweight = 0;
}
// add migration events
if (modeloptions[NOMIGRATION])
{
migweight = 0;
}
else
{
migweight = addmigration (ci, li, mpart, tlengthpart, &mpart, &tlengthpart);
}
// copy the migration info in newedgemig and newsismig to the genealogy
copynewmig_to_gtree (ci, li);
// determine all the weights needed for calculating the probability of the genealogy
setzero_genealogy_weights (&G->gweight);
treeweight (ci, li);
sum_subtract_treeinfo (&C[ci]->allgweight, &G->gweight,
&holdgweight_updategenealogy);
/* calculate P(D|G) for new genealogy */
rejectIS = 0; /* use this to catech when P(D|G) for IS model is zero */
newpdg = 0;
newplg = 0;
switch (L[li].model)
{
case HKY:
if (assignmentoptions[JCMODEL] == 1)
{
newpdg_a[0] = likelihoodJC (ci, li, G->uvals[0]);
newpdg = newpdg_a[0];
}
else
{
newpdg = newpdg_a[0] =
likelihoodHKY (ci, li, G->uvals[0], G->kappaval, edge,
freededge, oldsis, newsis);
}
break;
case INFINITESITES:
newpdg = newpdg_a[0] = like = likelihoodIS (ci, li, G->uvals[0]);
rejectIS = (like == REJECTINFINITESITESCONSTANT);
break;
case STEPWISE:
{
for (ai = 0, newpdg = 0; ai < L[li].nlinked; ai++)
{
newpdg_a[ai] =
G->pdg_a[ai] + finishSWupdateA (ci, li, ai, edge, freededge,
oldsis, newsis,
G->uvals[ai], &Aterm[ai]);
newpdg += newpdg_a[ai];
Atermsum += Aterm[ai];
}
// checklikelihoodSW(ci, li,G->u[ai].mcinf.val);
break;
}
case JOINT_IS_SW:
newpdg = newpdg_a[0] = likelihoodIS (ci, li, G->uvals[0]);
rejectIS = (newpdg == REJECTINFINITESITESCONSTANT);
for (ai = 1; ai < L[li].nlinked; ai++)
{
newpdg_a[ai] =
G->pdg_a[ai] + finishSWupdateA (ci, li, ai, edge, freededge,
oldsis, newsis,
G->uvals[ai], &Aterm[ai]);
newpdg += newpdg_a[ai];
Atermsum += Aterm[ai];
}
//checklikelihoodSW(ci, li,Q[ci]->us[li]);
break;
}
accp = 0;
/* final weight calculation */
/* tpw is the ratio of new and old prior probability of the genealogies. It is actually the ratio of the total across all loci, but
since only genealogy li is being changed at the present time, the ratio works out to just be the ratio for genealogy li */
copy_probcalc (&holdallpcalc_updategenealogy, &C[ci]->allpcalc);
/* Find all internal node sequences and mutations of a full genealogy. */
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
if (L[li].model == INFINITESITES)
{
accp = IMA_genealogy_findIntSeq (ci, li);
if (accp == 0)
{
rejectIS = 1;
}
accp = 0;
}
}
if (rejectIS == 0)
{
// the metropolis term includes p(D|G) and p(G),
tpw = -C[ci]->allpcalc.probg;
integrate_tree_prob (ci, &C[ci]->allgweight,
&holdallgweight_updategenealogy, &C[ci]->allpcalc,
&holdallpcalc_updategenealogy, &holdt[0]);
tpw += C[ci]->allpcalc.probg;
metropolishastingsterm = tpw + gbeta*(newpdg - G->pdg);
U = uniform ();
metropolishastingsterm = exp (beta[ci] * metropolishastingsterm + migweight + slideweight + Atermsum);
if (U < DMIN(1.0, metropolishastingsterm)) //9/13/2010
{
/* accept the update */
C[ci]->allpcalc.pdg -= G->pdg;
C[ci]->allpcalc.pdg += newpdg;
G->pdg = newpdg;
for (ai = 0; ai < L[li].nlinked; ai++)
G->pdg_a[ai] = newpdg_a[ai];
if (L[li].model == HKY)
{
copyfraclike (ci, li);
storescalefactors (ci, li);
}
accp = 1;
}
}
/* reject the update */
if (accp == 0)
{
// put the edges back
restoreedges (ci, li, edge, oldsis, freededge, newsis);
// copy summary stats back
storegenealogystats (ci, li, 1);
// reset HKY terms
if (L[li].model == HKY)
restorescalefactors (ci, li);
// copy back all the weights and results associated with calculating the probability of the genealogy
copy_probcalc (&C[ci]->allpcalc, &holdallpcalc_updategenealogy);
copy_treeinfo (&C[ci]->allgweight, &holdallgweight_updategenealogy);
copy_treeinfo (&G->gweight, &holdgweight_updategenealogy);
*topolchange = 0;
*tmrcachange = 0;
}
/* do updates at nodes for stepwise loci, regardless of whether slide update was accepted. This could go somewhere else */
return accp;
}
/* let u refer to the more recent time and d to the older time */
int
changet_RY1 (int ci, int timeperiod) // after Rannala and Yang (2003) - rubberband method
{
double metropolishastingsterm, newt, oldt;
double pdgnew[MAXLOCI + MAXLINKED], pdgnewsum, pdgoldsum, probgnewsum,
temppdg;
double t_u_hterm, t_d_hterm, tpw;
int li, i, j, ecd, ecu, emd, emu, ai, ui;
double U;
struct genealogy *G;
struct edge *gtree;
double t_d, t_u, t_u_prior, t_d_prior;
double holdt[MAXPERIODS];
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
t_u = (timeperiod == 0) ? 0 : C[ci]->tvals[timeperiod - 1];
t_d =
(timeperiod ==
(lastperiodnumber - 1)) ? TIMEMAX : C[ci]->tvals[timeperiod + 1];
t_d_prior = DMIN (T[timeperiod].pr.max, t_d);
t_u_prior = DMAX (T[timeperiod].pr.min, t_u);
oldt = C[ci]->tvals[timeperiod];
newt = getnewt (timeperiod, t_u_prior, t_d_prior, oldt, 1);
t_u_hterm = (newt - t_u) / (oldt - t_u);
if (timeperiod == lastperiodnumber - 1)
{
t_d_hterm = 1;
}
else
{
t_d_hterm = (t_d - newt) / (t_d - oldt);
}
copy_treeinfo (&holdallgweight_t_RY, &C[ci]->allgweight); // try turning this off and forcing all recalculations
copy_probcalc (&holdallpcalc_t_RY, &C[ci]->allpcalc);
for (i = 0; i < lastperiodnumber; i++)
holdt[i] = C[ci]->tvals[i];
pdgoldsum = C[ci]->allpcalc.pdg;
setzero_genealogy_weights (&C[ci]->allgweight);
ecd = ecu = emd = emu = 0;
pdgnewsum = 0;
probgnewsum = 0;
storegenealogystats_all_loci (ci, 0);
C[ci]->tvals[timeperiod] = newt;
for (i = 0; i < nurates; i++)
pdgnew[i] = 0;
for (li = 0; li < nloci; li++)
{
G = &(C[ci]->G[li]);
gtree = G->gtree;
copy_treeinfo (&holdgweight_t_RY[li], &G->gweight);
for (i = 0; i < L[li].numlines; i++)
{
if (gtree[i].down != -1)
{
if (gtree[i].time <= oldt && gtree[i].time > t_u)
{
//assert (skipflag[li][i] == 0);turn off 9/19/10
gtree[i].time =
beforesplit (timeperiod, oldt, newt, /* t_d, */ t_u, gtree[i].time);
assert (gtree[i].time != newt);
ecu++;
}
else
{
if (gtree[i].time > oldt && gtree[i].time < t_d)
{
// assert (skipflag[li][i] == 0); turn off 9/19/10
gtree[i].time =
aftersplit (timeperiod, oldt, newt, t_d, /* t_u, */ gtree[i].time);
assert (gtree[i].time != newt);
ecd++;
}
//else do not change the time
}
j = 0;
while (gtree[i].mig[j].mt > -0.5)
{
assert (gtree[i].mig[j].mt < C[0]->tvals[lastperiodnumber]);
if (gtree[i].mig[j].mt <= oldt && gtree[i].mig[j].mt > t_u)
{
gtree[i].mig[j].mt =
beforesplit (timeperiod, oldt, newt, /* t_d, */ t_u,
gtree[i].mig[j].mt);
emu++;
}
else
{
assert (oldt < C[0]->tvals[lastperiodnumber]);
if (gtree[i].mig[j].mt > oldt && gtree[i].mig[j].mt < t_d)
{
gtree[i].mig[j].mt =
aftersplit (timeperiod, oldt, newt, t_d, /* t_u, */
gtree[i].mig[j].mt);
emd++;
}
// else no need to change the time
}
j++;
}
}
}
if (G->roottime <= oldt && G->roottime > t_u
/* && skipflag[li][G->root] == 0 turn off 9/19/10*/)
G->roottime =
beforesplit (timeperiod, oldt, newt, /* t_d, */ t_u, G->roottime);
else if (G->roottime > oldt && G->roottime < t_d
/* && skipflag[li][G->root] == 0 turn off 9/19/10*/)
G->roottime =
aftersplit (timeperiod, oldt, newt, t_d, /* t_u, */ G->roottime);
setzero_genealogy_weights (&G->gweight);
treeweight (ci, li);
sum_treeinfo (&C[ci]->allgweight, &G->gweight);
ai = 0;
ui = L[li].uii[ai];
switch (L[li].model)
{
assert (pdgnew[ui] == 0);
case HKY:
if (assignmentoptions[JCMODEL] == 1)
{
temppdg = pdgnew[ui] =
likelihoodJC (ci, li, G->uvals[0]);
}
else
{
temppdg = pdgnew[ui] =
likelihoodHKY (ci, li, G->uvals[0], G->kappaval, -1, -1, -1, -1);
}
break;
case INFINITESITES:
temppdg = pdgnew[ui] = likelihoodIS (ci, li, G->uvals[0]);
break;
case STEPWISE:
temppdg = 0;
for (; ai < L[li].nlinked; ai++)
{
ui = L[li].uii[ai];
assert (pdgnew[ui] == 0);
pdgnew[ui] = likelihoodSW (ci, li, ai, G->uvals[ai], 1.0);
temppdg += pdgnew[ui];
}
break;
case JOINT_IS_SW:
temppdg = pdgnew[ui] = likelihoodIS (ci, li, G->uvals[0]);
for (ai = 1; ai < L[li].nlinked; ai++)
{
ui = L[li].uii[ai];
assert (pdgnew[ui] == 0);
pdgnew[ui] = likelihoodSW (ci, li, ai, G->uvals[ai], 1.0);
temppdg += pdgnew[ui];
}
break;
}
pdgnewsum += temppdg;
}
assert (!ODD (ecd));
assert (!ODD (ecu));
ecd /= 2;
ecu /= 2;
integrate_tree_prob (ci, &C[ci]->allgweight, &holdallgweight_t_RY,
&C[ci]->allpcalc, &holdallpcalc_t_RY, &holdt[0]); // try enforcing full cacullation
tpw = gbeta * (pdgnewsum - pdgoldsum);
/* 5/19/2011 JH adding thermodynamic integration */
if (calcoptions[CALCMARGINALLIKELIHOOD])
{
metropolishastingsterm = beta[ci] * tpw + (C[ci]->allpcalc.probg - holdallpcalc_t_RY.probg) + (ecd + emd) * log (t_d_hterm) + (ecu + emu) * log (t_u_hterm);
}
else
{
tpw += C[ci]->allpcalc.probg - holdallpcalc_t_RY.probg;
metropolishastingsterm = beta[ci] * tpw + (ecd + emd) * log (t_d_hterm) + (ecu + emu) * log (t_u_hterm);
}
//assert(metropolishastingsterm >= -1e200 && metropolishastingsterm < 1e200);
U = log (uniform ());
if (U < DMIN(1.0, metropolishastingsterm)) //9/13/2010
//if (metropolishastingsterm >= 0.0 || metropolishastingsterm > U)
{
for (li = 0; li < nloci; li++)
{
C[ci]->G[li].pdg = 0;
for (ai = 0; ai < L[li].nlinked; ai++)
{
C[ci]->G[li].pdg_a[ai] = pdgnew[L[li].uii[ai]];
C[ci]->G[li].pdg += C[ci]->G[li].pdg_a[ai];
}
if (L[li].model == HKY)
{
storescalefactors (ci, li);
copyfraclike (ci, li);
}
}
C[ci]->allpcalc.pdg = pdgnewsum;
C[ci]->poptree[C[ci]->droppops[timeperiod + 1][0]].time =
C[ci]->poptree[C[ci]->droppops[timeperiod + 1][1]].time = newt;
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
return 1;
}
else
{
copy_treeinfo (&C[ci]->allgweight, &holdallgweight_t_RY);
copy_probcalc (&C[ci]->allpcalc, &holdallpcalc_t_RY);
assert (pdgoldsum == C[ci]->allpcalc.pdg);
C[ci]->tvals[timeperiod] = oldt;
for (li = 0; li < nloci; li++)
{
G = &(C[ci]->G[li]);
gtree = G->gtree;
storegenealogystats_all_loci (ci, 1);
copy_treeinfo (&G->gweight, &holdgweight_t_RY[li]);
for (i = 0; i < L[li].numlines; i++)
{
if (gtree[i].down != -1)
{
if (gtree[i].time <= newt && gtree[i].time > t_u)
{
// assert (skipflag[li][i] == 0); turned off 9/19/10
gtree[i].time =
beforesplit (timeperiod, newt, oldt, /* t_d, */ t_u, gtree[i].time);
//cecu++;
}
else
{
if (gtree[i].time > newt && gtree[i].time < t_d)
{
//assert (skipflag[li][i] == 0); turned off 9/19/10
gtree[i].time =
aftersplit (timeperiod, newt, oldt, t_d, /* t_u, */ gtree[i].time);
//cecl++;
}
}
j = 0;
while (gtree[i].mig[j].mt > -0.5)
{
if (gtree[i].mig[j].mt <= newt && gtree[i].mig[j].mt > t_u)
{
gtree[i].mig[j].mt =
beforesplit (timeperiod, newt, oldt, /* t_d, */
t_u, gtree[i].mig[j].mt);
//cemu++;
}
else if (gtree[i].mig[j].mt > newt && gtree[i].mig[j].mt < t_d)
{
gtree[i].mig[j].mt =
aftersplit (timeperiod, newt, oldt, t_d, /* t_u, */
gtree[i].mig[j].mt);
//ceml++;
}
j++;
}
}
}
// assert(fabs(C[ci]->G[li].gtree[ C[ci]->G[li].gtree[C[ci]->G[li].root].up[0]].time - C[ci]->G[li].roottime) < 1e-8);
}
/* assert(ecu==cecu/2);
assert(ecd==cecl/2);
assert(emu==cemu);
assert(emd==ceml); */
for (li = 0; li < nloci; li++)
{
if (L[li].model == HKY)
restorescalefactors (ci, li);
/* have to reset the dlikeA values in the genealogies for stepwise model */
if (L[li].model == STEPWISE)
for (ai = 0; ai < L[li].nlinked; ai++)
likelihoodSW (ci, li, ai, C[ci]->G[li].uvals[ai], 1.0);
if (L[li].model == JOINT_IS_SW)
for (ai = 1; ai < L[li].nlinked; ai++)
likelihoodSW (ci, li, ai, C[ci]->G[li].uvals[ai], 1.0);
// assert(fabs(C[ci]->G[li].gtree[ C[ci]->G[li].gtree[C[ci]->G[li].root].up[0]].time - C[ci]->G[li].roottime) < 1e-8);
}
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
return 0;
}
} /* changet_RY1 */
void palRefro( double zobs, double hm, double tdk, double pmb,
double rh, double wl, double phi, double tlr,
double eps, double * ref ) {
/*
* Fixed parameters
*/
/* 93 degrees in radians */
const double D93 = 1.623156204;
/* Universal gas constant */
const double GCR = 8314.32;
/* Molecular weight of dry air */
const double DMD = 28.9644;
/* Molecular weight of water vapour */
const double DMW = 18.0152;
/* Mean Earth radius (metre) */
const double S = 6378120.;
/* Exponent of temperature dependence of water vapour pressure */
const double DELTA = 18.36;
/* Height of tropopause (metre) */
const double HT = 11000.;
/* Upper limit for refractive effects (metre) */
const double HS = 80000.;
/* Numerical integration: maximum number of strips. */
const int ISMAX=16384l;
/* Local variables */
int is, k, n, i, j;
int optic, loop; /* booleans */
double zobs1,zobs2,hmok,tdkok,pmbok,rhok,wlok,alpha,
tol,wlsq,gb,a,gamal,gamma,gamm2,delm2,
tdc,psat,pwo,w,
c1,c2,c3,c4,c5,c6,r0,tempo,dn0,rdndr0,sk0,f0,
rt,tt,dnt,rdndrt,sine,zt,ft,dnts,rdndrp,zts,fts,
rs,dns,rdndrs,zs,fs,refold,z0,zrange,fb,ff,fo,fe,
h,r,sz,rg,dr,tg,dn,rdndr,t,f,refp,reft;
/* The refraction integrand */
#define refi(DN,RDNDR) RDNDR/(DN+RDNDR)
/* Transform ZOBS into the normal range. */
zobs1 = palDrange(zobs);
zobs2 = DMIN(fabs(zobs1),D93);
/* keep other arguments within safe bounds. */
hmok = DMIN(DMAX(hm,-1e3),HS);
tdkok = DMIN(DMAX(tdk,100.0),500.0);
pmbok = DMIN(DMAX(pmb,0.0),10000.0);
rhok = DMIN(DMAX(rh,0.0),1.0);
wlok = DMAX(wl,0.1);
alpha = DMIN(DMAX(fabs(tlr),0.001),0.01);
/* tolerance for iteration. */
tol = DMIN(DMAX(fabs(eps),1e-12),0.1)/2.0;
/* decide whether optical/ir or radio case - switch at 100 microns. */
optic = wlok < 100.0;
/* set up model atmosphere parameters defined at the observer. */
wlsq = wlok*wlok;
gb = 9.784*(1.0-0.0026*cos(phi+phi)-0.00000028*hmok);
if (optic) {
a = (287.6155+(1.62887+0.01360/wlsq)/wlsq) * 273.15e-6/1013.25;
} else {
a = 77.6890e-6;
}
gamal = (gb*DMD)/GCR;
gamma = gamal/alpha;
gamm2 = gamma-2.0;
delm2 = DELTA-2.0;
tdc = tdkok-273.15;
psat = pow(10.0,(0.7859+0.03477*tdc)/(1.0+0.00412*tdc)) *
(1.0+pmbok*(4.5e-6+6.0e-10*tdc*tdc));
if (pmbok > 0.0) {
pwo = rhok*psat/(1.0-(1.0-rhok)*psat/pmbok);
} else {
pwo = 0.0;
}
w = pwo*(1.0-DMW/DMD)*gamma/(DELTA-gamma);
c1 = a*(pmbok+w)/tdkok;
if (optic) {
c2 = (a*w+11.2684e-6*pwo)/tdkok;
} else {
c2 = (a*w+6.3938e-6*pwo)/tdkok;
}
c3 = (gamma-1.0)*alpha*c1/tdkok;
c4 = (DELTA-1.0)*alpha*c2/tdkok;
if (optic) {
c5 = 0.0;
c6 = 0.0;
} else {
c5 = 375463e-6*pwo/tdkok;
c6 = c5*delm2*alpha/(tdkok*tdkok);
}
/* conditions at the observer. */
r0 = S+hmok;
pal1Atmt(r0,tdkok,alpha,gamm2,delm2,c1,c2,c3,c4,c5,c6,
r0,&tempo,&dn0,&rdndr0);
sk0 = dn0*r0*sin(zobs2);
f0 = refi(dn0,rdndr0);
/* conditions in the troposphere at the tropopause. */
rt = S+DMAX(HT,hmok);
pal1Atmt(r0,tdkok,alpha,gamm2,delm2,c1,c2,c3,c4,c5,c6,
rt,&tt,&dnt,&rdndrt);
sine = sk0/(rt*dnt);
zt = atan2(sine,sqrt(DMAX(1.0-sine*sine,0.0)));
ft = refi(dnt,rdndrt);
/* conditions in the stratosphere at the tropopause. */
pal1Atms(rt,tt,dnt,gamal,rt,&dnts,&rdndrp);
sine = sk0/(rt*dnts);
zts = atan2(sine,sqrt(DMAX(1.0-sine*sine,0.0)));
fts = refi(dnts,rdndrp);
/* conditions at the stratosphere limit. */
rs = S+HS;
pal1Atms(rt,tt,dnt,gamal,rs,&dns,&rdndrs);
sine = sk0/(rs*dns);
zs = atan2(sine,sqrt(DMAX(1.0-sine*sine,0.0)));
fs = refi(dns,rdndrs);
/* variable initialization to avoid compiler warning. */
reft = 0.0;
/* integrate the refraction integral in two parts; first in the
* troposphere (k=1), then in the stratosphere (k=2). */
for (k=1; k<=2; k++) {
/* initialize previous refraction to ensure at least two iterations. */
refold = 1.0;
/* start off with 8 strips. */
is = 8;
/* start z, z range, and start and end values. */
if (k==1) {
z0 = zobs2;
zrange = zt-z0;
fb = f0;
ff = ft;
} else {
z0 = zts;
zrange = zs-z0;
fb = fts;
ff = fs;
}
/* sums of odd and even values. */
fo = 0.0;
fe = 0.0;
/* first time through the loop we have to do every point. */
n = 1;
/* start of iteration loop (terminates at specified precision). */
loop = 1;
while (loop) {
/* strip width. */
h = zrange/((double)is);
/* initialize distance from earth centre for quadrature pass. */
if (k == 1) {
r = r0;
} else {
r = rt;
}
/* one pass (no need to compute evens after first time). */
for (i=1; i<is; i+=n) {
/* sine of observed zenith distance. */
sz = sin(z0+h*(double)(i));
/* find r (to the nearest metre, maximum four iterations). */
if (sz > 1e-20) {
w = sk0/sz;
rg = r;
dr = 1.0e6;
j = 0;
while ( fabs(dr) > 1.0 && j < 4 ) {
j++;
if (k==1) {
pal1Atmt(r0,tdkok,alpha,gamm2,delm2,
c1,c2,c3,c4,c5,c6,rg,&tg,&dn,&rdndr);
} else {
pal1Atms(rt,tt,dnt,gamal,rg,&dn,&rdndr);
}
dr = (rg*dn-w)/(dn+rdndr);
rg = rg-dr;
}
r = rg;
}
/* find the refractive index and integrand at r. */
if (k==1) {
pal1Atmt(r0,tdkok,alpha,gamm2,delm2,
c1,c2,c3,c4,c5,c6,r,&t,&dn,&rdndr);
} else {
pal1Atms(rt,tt,dnt,gamal,r,&dn,&rdndr);
}
f = refi(dn,rdndr);
/* accumulate odd and (first time only) even values. */
if (n==1 && i%2 == 0) {
fe += f;
} else {
fo += f;
}
}
/* evaluate the integrand using simpson's rule. */
refp = h*(fb+4.0*fo+2.0*fe+ff)/3.0;
/* has the required precision been achieved (or can't be)? */
if (fabs(refp-refold) > tol && is < ISMAX) {
/* no: prepare for next iteration.*/
/* save current value for convergence test. */
refold = refp;
/* double the number of strips. */
is += is;
/* sum of all current values = sum of next pass's even values. */
fe += fo;
/* prepare for new odd values. */
fo = 0.0;
/* skip even values next time. */
n = 2;
} else {
/* yes: save troposphere component and terminate the loop. */
if (k==1) reft = refp;
loop = 0;
}
}
}
/* result. */
*ref = reft+refp;
if (zobs1 < 0.0) *ref = -(*ref);
}
//given an animation and a keyframe, this will find the dims that encompass the
//model
bool CDimensionsDlg::FindAnimDims(Model* pModel, uint32 nAnim, uint32 nKeyFrame, LTVector& vDims)
{
//clear out the dims to start out with in case we fail
vDims.Init();
AnimTracker tracker, *pTracker;
tracker.m_TimeRef.Init(pModel, nAnim, nKeyFrame, nAnim, nKeyFrame, 0.0f);
AnimInfo *pAnim = &pModel->m_Anims[nAnim];
pTracker = &tracker;//pAnim->m_pAnim;
static CMoArray<TVert> tVerts;
// Use the model code to setup the vertices.
int nTrackers = 1;
nTrackers = DMIN(nTrackers, MAX_GVP_ANIMS);
GVPStruct gvp;
gvp.m_nAnims = 0;
for(int i = 0; i < nTrackers; i++)
{
gvp.m_Anims[i] = pTracker[i].m_TimeRef;
gvp.m_nAnims++;
}
LTMatrix m;
m.Identity();
int nWantedVerts = pModel->GetTotalNumVerts() * 2;
if(tVerts.GetSize() < nWantedVerts)
{
if(!tVerts.SetSize(nWantedVerts))
return false;
}
gvp.m_VertexStride = sizeof(TVert);
gvp.m_Vertices = tVerts.GetArray();
gvp.m_BaseTransform = m;
gvp.m_CurrentLODDist = 0;
if (AlternateGetVertexPositions(pModel, &gvp, true, false, false, false))
{
LTVector vMax(0, 0, 0);
for (i = 0; i < pModel->GetTotalNumVerts(); i ++)
{
TVert v = tVerts[i];
if (fabs(v.m_vPos.x) > vMax.x) vMax.x = (float)fabs(v.m_vPos.x);
if (fabs(v.m_vPos.y) > vMax.y) vMax.y = (float)fabs(v.m_vPos.y);
if (fabs(v.m_vPos.z) > vMax.z) vMax.z = (float)fabs(v.m_vPos.z);
}
// Setup the new dims....
//round max up to the .1 decimal place
vMax.x = (float)(ceil(vMax.x * 10.0) / 10.0);
vMax.y = (float)(ceil(vMax.y * 10.0) / 10.0);
vMax.z = (float)(ceil(vMax.z * 10.0) / 10.0);
vDims = vMax;
return true;
}
//failure
return false;
}
RootNodeT *
buildTree(int N, MatrixT Dorig, char **taxon)
{
/*
\section{Initialize}
Initialize main variables
*/
int i;
RootNodeT *root;
int Nleft, Nnext; /* number of Nodes left to be joined
and the next index to be used */
MatrixT b=matrix(N); /* the $b_{i;j}$ matrix (eq 0.7) */
/* $q(i)$ array: value which minimizes $R(i,q(i),j)\,\forall j\ne i,q(i)$ */
int *q;
int *q2; /* Second best value */
VectorT R=vector(N); /* $R(i,q(i))$ (eq 0.10) */
VectorT LLR=vector(N); /* $R(i,q(i),q2(i))$ */
VectorT Zscore=vector(N); /* $z(i,q(i))$ */
/*
This auxilary matrices are globally defined in \|weighbor.h| we do
this to make it simplier so we do not always have to pass these
around. Note that the need to be visible here as we will be
calling \|calcR| later in this function and \|calcR| needs these
values
*/
s = matrix(N); /* $s_{ij}$ eq 0.9 */
deltaB = matrix(N); /* $\Delta b_{ij}$ eq 0.8 */
delta2B = matrix(N); /* $\Delta^2 b_{ij}$ */
if(recalcB)
oldDeltaB = matrix(N);
/*
This will hold this orignal $N$ distances plus any distances from
the $N-3$ internal nodes. Note we do not care about the root node
so $N-3$ and not $N-2$
*/
mD=matrix(2*N-3);
/*
This is the renormalization vector $c_i$ (eq 0.39) and matrix
$c_{i;j}$ (eq 0.43 ver0.2.5); again it must
be large enough to hold both the original and the new joined taxa
N.B. \|vector| sets all elements to zero.
*/
vC=vector(2*N-3);
/*
This matrices hold the previous iterations values of $s_{ij}$,
$\Delta b_{ij}$, etc. They are used to speed up the next
iterations calcultions of these quantities.
*/
mS = matrix(2*N-3);
mDelB = matrix(2*N-3);
mDel2B = matrix(2*N-3);
/*
Init \|mS| to -1 to keep track of which entries have not yet been
computed. */
for(i=0;i<2*N-3;++i)
{
int j;
for(j=0;j<2*N-3;++j)
mS[i][j] = -1.0;
}
/*
Make a copy of the original distance matrix; embed it in the
larger matrix which will hold the new distance from the added
internal nodes of the tree.
*/
setMM(N, Dorig, mD);
/*
Allocate and initialize the \|q|, \|q2| and \|nodes| array. $2N-3$
nodes to hold both the original and the added nodes.
*/
q = (int *)malloc(N*sizeof(int));
if(!q) printError("build::buildTree:out of memory-q\n");
q2 = (int *)malloc(N*sizeof(int));
if(!q2) printError("build::buildTree:out of memory-q2\n");
nodes = (NodeT **)malloc( (2*N-3)*sizeof(NodeT *));
if(!nodes) printError("build::buildTree:out of memory-nodes");
for(i=0;i<N;++i) {
nodes[i] = createNode();
nodes[i]->name = taxon[i];
nodes[i]->ind = i;
}
Nleft = N;
Nnext = N;
/*
\section{Loop until 3 taxa left}
While we have more than 3 nodes left do the neighbor joining algorithm.
Each pass of the algorithm will join 2 nodes replacing them with one.
*/
while(Nleft>3)
{
int j, k, ip, ip2;
double minR = 0, min2R = 0;
NodeT *newNode, *tmpNode;
double sigma_inf_i, sigma_inf_ip;
double sig_r, sig_l;
int jj, jjmin;
double LLRp = 0, tR;
/* \subsection{Calculate Residual} */
calc_q(Nleft, q, R, b, q2, LLR, Zscore);
if(printLevel>2)
for(k=0;k<Nleft;++k)
fprintf(outfile, "q[%d]=%d R(%d,%d)=%g\n",
k, q[k], k, q[k], R[k]);
/*
Find $i$ than minimizes $R(i,q(i))$. With the constraint that
$q(q(i))=i$ first if no pair found then find the best $i$
without this constraint.
Note: the \|checkQQI| flag determines if we will use the
$q(q(i))=i$ constraint.
Note: j will hold the next best pair
*/
i = -1;
j = -1;
if(checkQQI) {
for(k=0;k<Nleft;++k)
if(q[q[k]]==k) {
if(R[k]<minR || i==-1)
{
if(printLevel>3)
fprintf(outfile,
"ij=%d%d k=%d q[k]=%d minR = %.16g R[k] = %.16g\n",
i,j,k, q[k], minR, R[k]);
j = i;
min2R = minR;
i = k;
minR = R[k];
}
else if(R[k]>minR && (R[k]<min2R || j==-1) )
{
j = k;
min2R = R[k];
}
}
}
if(i==-1) /* No pair had $q(q(i))=i$ */
{
if(R[0]<R[1]) {
i = 0;
minR = R[0];
j = 1;
min2R = R[1];
} else {
i = 1;
minR = R[1];
j = 0;
min2R = R[0];
}
for(k=1;k<Nleft;++k)
if(R[k]<minR)
{
j = i;
min2R = minR;
i = k;
minR = R[k];
}
else if(R[k] < min2R && R[k] > minR)
{
j = k;
min2R = R[k];
}
if(checkQQI && printLevel>1)
fprintf(outfile, "No pair with q[q[i]]==i ");
else
if(q[q[i]]!=i && printLevel>1)
fprintf(outfile,
"The pair does not satisfy q[q[i]]==i (checking is off)"
);
}
ip = q[i];
ip2 = j;
/*
If the extended tournament option is set (-e) then run two more tournaments for
(i,q[i]) to see who really wins.
*/
if(extendedTourn)
{
double minR1 = 0, minR2 = 0, tmpR, oldR=R[i];
int jmin=-1, jpmin=-1;
/*
First fine the j the minimizes R(i,j)
*/
for(j=0;j<Nleft;++j)
if(j!=i && j!=q[i])
{
if(j!=q2[i])
tmpR = calcR2(Nleft, i, j, q2[i]);
else
tmpR = calcR2(Nleft, i, j, q[i]);
if(tmpR<minR1 || jmin==-1)
{
minR1=tmpR;
jmin = j;
}
}
/*
and now the $j'$ that minimizes $R(j',q[i])$
*/
for(j=0;j<Nleft;++j)
if(j!=i && j!=q[i])
{
if(j!=q2[i])
tmpR = calcR2(Nleft, j, q[i], q2[i]);
else
tmpR = calcR2(Nleft, j, q[i], i);
if(tmpR<minR2 || jpmin==-1)
{
minR2=tmpR;
jpmin = j;
}
}
/*
Now fnd which of the three is the smallest
*/
if(minR1<minR2 && minR1<R[i])
{
ip = jmin;
if(printLevel>1)
fprintf(outfile,
"Extended Tournament New Winner(A): (%d, %d) R=%g\n",
i, ip, minR1);
}
else if(minR2<minR1 && minR2<R[i])
{
i = jpmin;
if(printLevel>1)
fprintf(outfile,
"Extended Tournament New Winner(B): (%d, %d) R=%g\n",
i, ip, minR2);
}
if(printLevel>3)
fprintf(outfile, "R=%g, R1=%g, R2=%g\n", oldR, minR1, minR2);
}
/*
Find the $jj$ that minimizes $R(q(i),i,jj)$ and then print out
the LLR and LLR' values.
*/
jjmin=-1;
for(jj=0;jj<Nleft;++jj)
if(jj!=i && jj!=ip
&& (((tR=calcR(Nleft, ip, jj, i))<LLRp) || jjmin==-1))
{
jjmin = jj;
LLRp = tR;
}
LLRp *= 0.5;
if( (LLR[i]<1e-6) && (LLRp<1e-6) ) {
fprintf(stderr,
"warning: tie scores encountered; topology may depend on sequence order!\n");
fprintf(stderr, "taxon %s and taxon %s\n\n",
nodes[i]->name, nodes[ip]->name);
if(printLevel>1) {
fprintf(outfile,
"warning: tie scores encountered; topology may depend on sequence order!\n");
fprintf(outfile, "taxon %s and taxon %s\n\n",
nodes[i]->name, nodes[ip]->name);
}
}
if(printLevel>0) {
fprintf(outfile,
"\nJoin taxon %s to taxon %s (%s next best choice)\n",
nodes[i]->name, nodes[ip]->name, nodes[q2[i]]->name);
fprintf(outfile, " p-value = %g\n",
DMAX(1.0/(exp(LLR[i])+1.0), 1.0/(exp(LLRp)+1.0)));
if(printLevel>1) {
fprintf(outfile,"\nJoin taxon %s to taxon %s; R=%g\n",
nodes[i]->name, nodes[ip]->name, minR);
if(ip2!=-1 && ip2!=i && ip2!=ip)
fprintf(outfile, "Second best pair (%s, %s); R=%g\n",
nodes[ip2]->name, nodes[q[ip2]]->name, min2R);
else
fprintf(outfile, "No second best pair\n");
}
}
/*
Note due to the way we shuffle around nodes after joining:
i->Nnext, New->i, ip<->Nleft-1, if ip is less than i and
i=Nleft-1 then the new node will be in position ip not i!!
But tc (the global that is suppose to point to the position
of the new node for calcb) is set to i so this will screw us
up. The simpliest solution is to make sure i<ip; swap if they
are not.
*/
if(ip<i) {
int tt;
tt=i;
i=ip;
ip=tt;
}
/*
Need to calculate the new branch lengths $\bar b_{i;i'}$ and
$\bar b_{i';i}$, eq. 0.19.
Note if the z-score is negative then we calculate $\phi$ eq
(0.26) and use it to renormalize $d_{i,i'}$ and recompute
$b_{i;i'}$ and $b_{i';i}$.
*/
if(Zscore[i]<0.0) {
double phi_iip, dBar_iip;
phi_iip = calcPhi(Nleft, i, ip);
if(printLevel>2)
fprintf(outfile, "Renormalizing z[%d,%d] = %g\n", i, ip, Zscore[i]);
if(phi_iip>0)
{
dBar_iip = D(i,ip)-phi_iip;
if(printLevel>2)
fprintf(outfile, "phi=%g dBar_iip=%g\n", phi_iip, dBar_iip);
/* renormalize the b's */
if( dBar_iip >= fabs(deltaB[i][ip]) )
b[i][ip] = (deltaB[i][ip] + dBar_iip)/2.0;
else if( dBar_iip < -deltaB[i][ip] )
b[i][ip] = 0.0;
else
b[i][ip] = dBar_iip;
if( dBar_iip >= fabs(deltaB[ip][i]) )
b[ip][i] = (deltaB[ip][i] + dBar_iip)/2.0;
else if( dBar_iip < -deltaB[ip][i] )
b[ip][i] = 0.0;
else
b[ip][i] = dBar_iip;
}
}
nodes[i ]->rho = b[i][ip];
nodes[ip]->rho = b[ip][i];
if(nodes[i ]->rho < 0.0)
{
if(printLevel>0)
fprintf(outfile,
"WARNING: Negative branch length %lg set to zero\n",
nodes[i ]->rho);
nodes[i ]->rho = 0.0;
nodes[ip]->rho = D(i,ip);
}
else if(nodes[ip]->rho < 0.0)
{
if(printLevel>0)
fprintf(outfile,
"WARNING: Negative branch length %lg set to zero\n",
nodes[ip]->rho);
nodes[ip]->rho = 0.0;
nodes[i ]->rho = D(i,ip);
}
if(printLevel>3)
{
fprintf(outfile, "\\bar b_[%d%d] = %g b_[%d%d]=%g\n",
i, ip, nodes[i]->rho, i, ip, b[i][ip]);
fprintf(outfile, "\\bar b_[%d%d] = %g b_[%d%d]=%g\n\n",
ip, i, nodes[ip]->rho, ip, i, b[ip][i]);
}
newNode = createNode();
newNode->ind = Nnext;
newNode->child_r = nodes[i];
newNode->child_l = nodes[ip];
newNode->name = nodes[i]->name;
nodes[Nnext] = newNode;
/*
Calculate $\sigma^2_\infty(i\bar\imath)$ (eq. 0.27) for each
of the joined taxa.
*/
sigma_inf_i = 0.0;
sigma_inf_ip = 0.0;
for(j=0;j<Nleft;++j)
{
if(j!=i && j!=ip)
{
sigma_inf_i
+= sigma_na(DMAX(b[i][ip],MINB)+C(i),
DMAX(D(i,j)-b[i][ip],MINB)+C(j) );
sigma_inf_ip
+= sigma_na(DMAX(b[ip][i],MINB)+C(ip),
DMAX(D(ip,j)-b[ip][i],MINB)+C(j) );
}
}
/*
Add \|EPSILON| here to make the following formulae a bit simplier
*/
sigma_inf_i += EPSILON;
sigma_inf_ip += EPSILON;
/*
Calculate the new distances from eq. 0.24
$$
d_{\bar\imath k} = {{(d_{ik}-b_{i;i'}+\phi_i)/\sigma^2_\infty(i\bar\imath)+
(d_{i'j}-b_{i';i}+\phi_{i'})/\sigma^2_\infty(i'\bar\imath)}
\over{
{1\over\sigma^2_\infty(i'\bar\imath)} +
{1\over\sigma^2_\infty(i'\bar\imath)}}}
$$
where\hfill\break
$i=$ \|newNode->child_r->ind|,\hfill\break
$i'=$ \|newNode->child_l->ind|,\hfill\break
$b_{i;i'}=$ \|newNode->child_r->rho|,\hfill\break
$b_{i';i}=$ \|newNode->child_l->rho|
Also calcuate the renormalization terms $c_{i;j}$ (eq 0.43 ver0.2.5)
and $c_i$
*/
for(j=0;j<Nleft;++j)
{
if(j!=i && j!=ip)
{
/* $1/\sigma^2_\infty(i\bar\imath)+1/\sigma^2_\infty(i'\bar\imath)$ */
double norm =
1.0/( 1.0/sigma_inf_i + 1.0/sigma_inf_ip);
/*
First calcuate the new distances
*/
D(Nnext,j) = D(j,Nnext) =
norm *
(
(D(i,j)-RHO(newNode->child_r))/(sigma_inf_i)
+
(D(ip,j)-RHO(newNode->child_l))/(sigma_inf_ip)
);
if(D(Nnext,j)<0.0)
D(Nnext,j) = D(j,Nnext) = 0.0;
}
}
D(Nnext,Nnext) = 0.0;
/*
And now the new renormalization quantity $c_{\bar\imath}$
N.B. eq 0.30 has been rewritten from
$$
{1\over{{1\over X}+{1\over Y}}}
$$
to
$$
{XY\over X+Y}
$$
which is better behaved numerically when $X$ or $Y$ is
small (and cheeper since it only has one division).
*/
sig_r = sigma2t(C(i)+DMAX(RHO(newNode->child_r), MINB));
sig_l = sigma2t(C(ip)+DMAX(RHO(newNode->child_l), MINB));
if(sig_r*sig_l>0.0)
{
C(Nnext) = sigma2tinv(sig_r*sig_l/(sig_r+sig_l));
}
else
C(Nnext) = sigma2tinv(0.0);
if(!
(C(Nnext)<=DMIN(DMAX(RHO(newNode->child_r),MINB)+C(i)+1e-14,
DMAX(RHO(newNode->child_l),MINB)+C(ip)+1e-14)))
{
printf("C(Nnext=%d)=%g\n"
"RHO_R=%g C(i=%d)=%g sig_r=%g\nRHO_L=%g C(ip=%d)=%g sig_l=%g -- %g\n",
Nnext, C(Nnext),
RHO(newNode->child_r), i, C(i), sig_r,
RHO(newNode->child_l), ip, C(ip), sig_l,
sig_r*sig_l/(sig_r+sig_l));
}
assert((C(Nnext)<=DMIN(DMAX(RHO(newNode->child_r),MINB)+C(i)+1e-14,
DMAX(RHO(newNode->child_l),MINB)+C(ip)+1e-14)));
/*
Swap $i$ node to the empty node at the end of the list and
place the new node in position $i$ */
nodes[Nnext] = nodes[i];
nodes[i] = newNode;
/*
Swap the $ip$ node and the last node on the list this moves
$ip$ to the end. When we decrease \|Nleft| by one there will be
on less node and the two joined nodes $i$ and $ip$ will now be
after then end (\|Nleft|) of the list
*/
tmpNode = nodes[ip];
nodes[ip] = nodes[Nleft-1];
nodes[Nleft-1] = tmpNode;
/*
In the new node set the child indecies to the
new indexes of the the joined nodes. This info
will be used by \|sigma2_3| in the renormalization
step
*/
newNode->cind_r=Nnext;
newNode->cind_l=Nleft-1;
/*
Set up the \|ta|, \|tb| and \|tc| node array indices. \|ta|
and \|tb| point to the two taxa that where just joined, and
\|tc| points to the newly created taxon.
These globals will be used in the next call to \|calcb|.
*/
ta = Nnext;
tb = Nleft - 1;
tc = i;
--Nleft;
++Nnext;
/*
Print out the values of the various variables
*/
if(printLevel>2)
{
int a, b;
fprintf(outfile, "\nReduced d_ij=\n");
for(a=0;a<Nleft;++a)
{
for(b=0;b<Nleft;++b)
fprintf(outfile,"%7.4lg ", D(a,b));
fprintf(outfile,"\n");
}
fprintf(outfile,"\n");
}
if(printLevel>3) {
int a, b;
for(a=0;a<Nnext;++a)
{
for(b=0;b<Nnext;++b)
fprintf(outfile,"%7.4lg ", mD[a][b]);
fprintf(outfile,"\n");
}
fprintf(outfile,"\n");
fprintf(outfile, "c_i = ");
for(a=0;a<Nleft;++a)
{
fprintf(outfile,"%7.4lg ", C(a));
}
fprintf(outfile,"\n");
for(a=0;a<Nnext;++a)
{
fprintf(outfile,"%7.4lg ", vC[a]);
}
fprintf(outfile,"\n");
fprintf(outfile, "\n");
}
}
/*
\section{Final three taxa}
Now there are just three taxa left. They will join to the root
node of our tree. Find their branch lengths (which we can do
exactly) and set up the root node to be passed back on return from
this functin.
*/
root = createRootNode();
if(!root) printError("build::buildTree:out of memory-root");
root->child_l = nodes[0];
root->child_m = nodes[1];
root->child_r = nodes[2];
/*
Now get the root branch lengths. We can solve this exactly since
we have three equations and three unknows. The equations to solve
are:
$$
\rho_0+\rho_1 = d_{01},
\rho_0+\rho_2 = d_{02},
\rho_1+\rho_2 = d_{12}
$$
And the solution is:
$$
\rho_0={1 \over 2}\left(d_{01}+d_{02}-d_{12}\right),
\rho_1={1 \over 2}\left(d_{01}-d_{02}+d_{12}\right),
\rho_2={1 \over 2}\left(-d_{01}+d_{02}+d_{12}\right)
$$
*/
root->child_l->rho = 0.5*( D(0,1)+D(0,2)-D(1,2));
root->child_m->rho = 0.5*( D(0,1)-D(0,2)+D(1,2));
root->child_r->rho = 0.5*(-D(0,1)+D(0,2)+D(1,2));
/* check for negative lengths and set to zero if found and decrease
the other each by half the the negative length (note + a neg
number is a decrease) */
if(root->child_l->rho < 0.0)
{
root->child_m->rho += 0.5*root->child_l->rho;
root->child_r->rho += 0.5*root->child_l->rho;
root->child_l->rho=0.0;
}
if(root->child_m->rho < 0.0)
{
root->child_l->rho += 0.5*root->child_m->rho;
root->child_r->rho += 0.5*root->child_m->rho;
root->child_m->rho=0.0;
}
if(root->child_r->rho < 0.0)
{
root->child_l->rho += 0.5*root->child_r->rho;
root->child_m->rho += 0.5*root->child_r->rho;
root->child_r->rho=0.0;
}
/*
Clean up
*/
freeMatrix(mD);
freeMatrix(b);
freeMatrix(delta2B);
freeMatrix(deltaB);
if(recalcB)
freeMatrix(oldDeltaB);
freeMatrix(s);
freeVector(R);
freeVector(LLR);
freeVector(Zscore);
freeVector(vC);
freeMatrix(mS);
freeMatrix(mDelB);
freeMatrix(mDel2B);
free(nodes);
free(q);
free(q2);
return(root);
}
void MATRIX_map_rect(MATRIX *matrix, int *x, int *y, int *w, int *h)
{
int rx, ry, rw, rh;
if (matrix->m12 == 0.0F && matrix->m21 == 0.0F)
{
rx = DROUND(matrix->m11 * *x + matrix->dx);
ry = DROUND(matrix->m22 * *y + matrix->dy);
rw = DROUND(matrix->m11 * *w);
rh = DROUND(matrix->m22 * *h);
if (rw < 0)
{
rw = -rw;
rx -= rw - 1;
}
if (rh < 0)
{
rh = -rh;
ry -= rh-1;
}
}
else
{
int left = *x;
int top = *y;
int right = *x + *w;
int bottom = *y + *h;
double x0, y0;
double x, y;
MAPDOUBLE(matrix, left, top, x0, y0 );
double xmin = x0;
double ymin = y0;
double xmax = x0;
double ymax = y0;
MAPDOUBLE(matrix, right, top, x, y );
xmin = DMIN( xmin, x );
ymin = DMIN( ymin, y );
xmax = DMAX( xmax, x );
ymax = DMAX( ymax, y );
MAPDOUBLE(matrix, right, bottom, x, y );
xmin = DMIN( xmin, x );
ymin = DMIN( ymin, y );
xmax = DMAX( xmax, x );
ymax = DMAX( ymax, y );
MAPDOUBLE(matrix, left, bottom, x, y );
xmin = DMIN( xmin, x );
ymin = DMIN( ymin, y );
xmax = DMAX( xmax, x );
ymax = DMAX( ymax, y );
double ww = xmax - xmin;
double hh = ymax - ymin;
xmin -= ( xmin - x0 ) / ww;
ymin -= ( ymin - y0 ) / hh;
xmax -= ( xmax - x0 ) / ww;
ymax -= ( ymax - y0 ) / hh;
rx = DROUND(xmin);
ry = DROUND(ymin);
rw = DROUND(xmax) - DROUND(xmin) + 1;
rh = DROUND(ymax) - DROUND(ymin) + 1;
}
*x = rx;
*y = ry;
*w = rw;
*h = rh;
}
void
array_bounds (const Point *p, int length,
bool transpose_axes, int clip_mode,
double *min_x, double *min_y, double *max_x, double *max_y,
bool spec_min_x, bool spec_min_y,
bool spec_max_x, bool spec_max_y)
{
/* keep compilers happy */
double user_min_x = 0.0, user_min_y = 0.0;
double user_max_x = 0.0, user_max_y = 0.0;
double local_min_x = 0.0, local_min_y = 0.0;
double local_max_x = 0.0, local_max_y = 0.0;
double xx, yy, oldxx, oldyy;
bool point_seen = false;
int i;
if (length == 0)
/* adopt a convention */
{
if (!spec_min_x)
*min_x = 0.0;
if (!spec_min_y)
*min_y = 0.0;
if (!spec_max_x)
*max_x = *min_x;
if (!spec_max_y)
*max_y = *min_y;
return;
}
if (spec_min_x)
user_min_x = *min_x;
else /* won't use user_min_x */
local_min_x = DBL_MAX;
if (spec_max_x)
user_max_x = *max_x;
else /* won't use user_max_x */
local_max_x = -(DBL_MAX);
/* special case: user specified both bounds, but min > max (reversed axis) */
if (spec_min_x && spec_max_x && user_min_x > user_max_x)
{
double tmp;
tmp = user_min_x;
user_min_x = user_max_x;
user_max_x = tmp;
}
if (spec_min_y)
user_min_y = *min_y;
else
local_min_y = DBL_MAX; /* won't use user_min_y */
if (spec_max_y)
user_max_y = *max_y;
else /* won't use user_max_y */
local_max_y = -(DBL_MAX);
/* special case: user specified both bounds, but min > max (reversed axis) */
if (spec_min_y && spec_max_y && user_min_y > user_max_y)
{
double tmp;
tmp = user_min_y;
user_min_y = user_max_y;
user_max_y = tmp;
}
/* loop through points in array; examine each line segment */
oldxx = oldyy = 0.0; /* previous point */
for (i = 0; i < length; i++)
{
double xxr[2], yyr[2]; /* storage for `relevant points' */
int n, j;
int effective_clip_mode;
/* get new point */
xx = (transpose_axes ? p[i].y : p[i].x);
yy = (transpose_axes ? p[i].x : p[i].y);
/* determine clipping mode (see compute_relevant_points() below) */
if (i == 0 || p[i].pendown == false
|| (p[i].linemode <= 0 && p[i].fill_fraction < 0.0))
/* no polyline or filling, each point is isolated */
effective_clip_mode = 0;
else if (p[i].fill_fraction >= 0.0)
effective_clip_mode = 2;
else
effective_clip_mode = clip_mode;
n = compute_relevant_points (xx, yy, oldxx, oldyy,
effective_clip_mode,
user_min_x, user_min_y,
user_max_x, user_max_y,
spec_min_x, spec_min_y,
spec_max_x, spec_max_y,
xxr, yyr);
/* loop through relevant points, updating bounding box */
for (j = 0; j < n; j++)
{
point_seen = true;
if (!spec_min_x)
local_min_x = DMIN(local_min_x, xxr[j]);
if (!spec_min_y)
local_min_y = DMIN(local_min_y, yyr[j]);
if (!spec_max_x)
local_max_x = DMAX(local_max_x, xxr[j]);
if (!spec_max_y)
local_max_y = DMAX(local_max_y, yyr[j]);
}
oldxx = xx;
oldyy = yy;
}
if (!point_seen)
/* a convention */
local_min_x = local_min_y = local_max_x = local_max_y = 0.0;
/* pass back bounds that user didn't specify */
if (!spec_min_x)
*min_x = local_min_x;
if (!spec_min_y)
*min_y = local_min_y;
if (!spec_max_x)
*max_x = local_max_x;
if (!spec_max_y)
*max_y = local_max_y;
return;
}
void do_sph_kick_for_extra_physics(int i, int tstart, int tend, double dt_entr)
{
int j;
#ifdef MAGNETIC
double dt_mag;
if(All.ComovingIntegrationOn)
dt_mag = get_magkick_factor(tstart, tend);
else
dt_mag = (tend - tstart) * All.Timebase_interval;
#endif
for(j = 0; j < 3; j++)
{
#if defined(MAGNETIC) && !defined(EULERPOTENTIALS) && !defined(VECT_POTENTIAL)
SphP[i].b1.B[j] += SphP[i].DtB[j] * dt_mag;
#endif
#ifdef VECT_POTENTIAL
SphP[i].A[j] += SphP[i].DtA[j] * dt_entr;
#endif
}
#if defined(MAGNETIC) && defined(DIVBCLEANING_DEDNER)
SphP[i].Phi += SphP[i].DtPhi * dt_mag;
#endif
#ifdef TIME_DEP_ART_VISC
SphP[i].alpha += SphP[i].Dtalpha * dt_entr;
SphP[i].alpha = DMIN(SphP[i].alpha, All.ArtBulkViscConst);
if(SphP[i].alpha < All.AlphaMin)
SphP[i].alpha = All.AlphaMin;
#endif
#ifdef VORONOI_TIME_DEP_ART_VISC
SphP[i].alpha += SphP[i].Dtalpha * dt_entr;
#ifdef VORONOI_RELAX_VIA_VISC
if(SphP[i].alpha < All.ArtBulkViscConst / 128.0 / 128.0)
SphP[i].alpha = All.ArtBulkViscConst / 128.0 / 128.0;
#else
if(SphP[i].alpha < All.ArtBulkViscConst / 128.0)
SphP[i].alpha = All.ArtBulkViscConst / 128.0;
#endif
#endif
#ifdef TIME_DEP_MAGN_DISP
SphP[i].Balpha += SphP[i].DtBalpha * dt_entr;
SphP[i].Balpha = DMIN(SphP[i].Balpha, All.ArtMagDispConst);
if(SphP[i].Balpha < All.ArtMagDispMin)
SphP[i].Balpha = All.ArtMagDispMin;
#endif
#ifdef NUCLEAR_NETWORK
for(j = 0; j < EOS_NSPECIES; j++)
SphP[i].xnuc[j] += SphP[i].dxnuc[j] * dt_entr * All.UnitTime_in_s;
network_normalize(SphP[i].xnuc, &SphP[i].Entropy, &All.nd, &All.nw);
#endif
#ifdef CHEMISTRY
/* update the chemical abundances for the new density and temperature */
double a_start = All.TimeBegin * exp(tstart * All.Timebase_interval);
double a_end = All.TimeBegin * exp(tend * All.Timebase_interval);
int mode;
/* time in cosmic expansion parameter */
compute_abundances(mode = 1, i, a_start, a_end);
#endif
}
static double
ibeta_internal (double a, double b, double x)
{
double A0, B0;
double A2 = 1.0;
double B2 = 0.0;
double A1 = 1.0;
double B1 = 1.0;
double prefactor;
double f0 = 0.0, f1 = 1.0; /* f0 initted to quiet compiler */
int goodf0, goodf1 = 1;
int j;
prefactor = exp (a * log (x) + b * log (1.0 - x)
+ F_LGAMMA(a + b) - F_LGAMMA(a + 1.0) - F_LGAMMA(b));
for (j = 1; j <= ITERMAX; j++)
{
double aj;
int m;
if (j % 2) /* j odd, j = 2m + 1 */
{
m = (j - 1)/2;
aj = - (a + m) * (a + b + m) * x / ((a + 2 * m) * (a + 2 * m + 1));
}
else /* j even, j = 2m */
{
m = j/2;
aj = m * (b - m) * x / ((a + 2 * m - 1) * (a + 2 * m));
}
A0 = 1.0 * A1 + aj * A2;
B0 = 1.0 * B1 + aj * B2;
if (B0 != 0.0)
{
double ren;
/* renormalize; don't really need to do this on each pass */
ren = 1.0 / B0;
A0 *= ren;
B0 = 1.0;
A1 *= ren;
B1 *= ren;
f0 = A0;
goodf0 = 1;
/* test f0 = A0/B0 = A0 for exit */
if (goodf1 && fabs (f0 - f1) <= DMIN(MACHEPS, fabs (f0) * MACHEPS))
return (prefactor / f0);
}
else
goodf0 = 0;
/* shift down */
A2 = A1;
B2 = B1;
A1 = A0;
B1 = B0;
f1 = f0;
goodf1 = goodf0;
}
/* if we reached here, convergence failed */
return -1.0;
}
void palPertue( double date, double u[13], int *jstat ) {
/* Distance from EMB at which Earth and Moon are treated separately */
const double RNE=1.0;
/* Coincidence with major planet distance */
const double COINC=0.0001;
/* Coefficient relating timestep to perturbing force */
const double TSC=1e-4;
/* Minimum and maximum timestep (days) */
const double TSMIN = 0.01;
const double TSMAX = 10.0;
/* Age limit for major-planet state vector (days) */
const double AGEPMO=5.0;
/* Age limit for major-planet mean elements (days) */
const double AGEPEL=50.0;
/* Margin for error when deciding whether to renew the planetary data */
const double TINY=1e-6;
/* Age limit for the body's osculating elements (before rectification) */
const double AGEBEL=100.0;
/* Gaussian gravitational constant squared */
const double GCON2 = PAL__GCON * PAL__GCON;
/* The final epoch */
double TFINAL;
/* The body's current universal elements */
double UL[13];
/* Current reference epoch */
double T0;
/* Timespan from latest orbit rectification to final epoch (days) */
double TSPAN;
/* Time left to go before integration is complete */
double TLEFT;
/* Time direction flag: +1=forwards, -1=backwards */
double FB;
/* First-time flag */
int FIRST = 0;
/*
* The current perturbations
*/
/* Epoch (days relative to current reference epoch) */
double RTN;
/* Position (AU) */
double PERP[3];
/* Velocity (AU/d) */
double PERV[3];
/* Acceleration (AU/d/d) */
double PERA[3];
/* Length of current timestep (days), and half that */
double TS,HTS;
/* Epoch of middle of timestep */
double T;
/* Epoch of planetary mean elements */
double TPEL = 0.0;
/* Planet number (1=Mercury, 2=Venus, 3=EMB...8=Neptune) */
int NP;
/* Planetary universal orbital elements */
double UP[8][13];
/* Epoch of planetary state vectors */
double TPMO = 0.0;
/* State vectors for the major planets (AU,AU/s) */
double PVIN[8][6];
/* Earth velocity and position vectors (AU,AU/s) */
double VB[3],PB[3],VH[3],PE[3];
/* Moon geocentric state vector (AU,AU/s) and position part */
double PVM[6],PM[3];
/* Date to J2000 de-precession matrix */
double PMAT[3][3];
/*
* Correction terms for extrapolated major planet vectors
*/
/* Sun-to-planet distances squared multiplied by 3 */
double R2X3[8];
/* Sunward acceleration terms, G/2R^3 */
double GC[8];
/* Tangential-to-circular correction factor */
double FC;
/* Radial correction factor due to Sunwards acceleration */
double FG;
/* The body's unperturbed and perturbed state vectors (AU,AU/s) */
double PV0[6],PV[6];
/* The body's perturbed and unperturbed heliocentric distances (AU) cubed */
double R03,R3;
/* The perturbating accelerations, indirect and direct */
double FI[3],FD[3];
/* Sun-to-planet vector, and distance cubed */
double RHO[3],RHO3;
/* Body-to-planet vector, and distance cubed */
double DELTA[3],DELTA3;
/* Miscellaneous */
int I,J;
double R2,W,DT,DT2,R,FT;
int NE;
/* Planetary inverse masses, Mercury through Neptune then Earth and Moon */
const double AMAS[10] = {
6023600., 408523.5, 328900.5, 3098710.,
1047.355, 3498.5, 22869., 19314.,
332946.038, 27068709.
};
/* Preset the status to OK. */
*jstat = 0;
/* Copy the final epoch. */
TFINAL = date;
/* Copy the elements (which will be periodically updated). */
for (I=0; I<13; I++) {
UL[I] = u[I];
}
/* Initialize the working reference epoch. */
T0=UL[2];
/* Total timespan (days) and hence time left. */
TSPAN = TFINAL-T0;
TLEFT = TSPAN;
/* Warn if excessive. */
if (fabs(TSPAN) > 36525.0) *jstat=101;
/* Time direction: +1 for forwards, -1 for backwards. */
FB = COPYSIGN(1.0,TSPAN);
/* Initialize relative epoch for start of current timestep. */
RTN = 0.0;
/* Reset the perturbations (position, velocity, acceleration). */
for (I=0; I<3; I++) {
PERP[I] = 0.0;
PERV[I] = 0.0;
PERA[I] = 0.0;
}
/* Set "first iteration" flag. */
FIRST = 1;
/* Step through the time left. */
while (FB*TLEFT > 0.0) {
/* Magnitude of current acceleration due to planetary attractions. */
if (FIRST) {
TS = TSMIN;
} else {
R2 = 0.0;
for (I=0; I<3; I++) {
W = FD[I];
R2 = R2+W*W;
}
W = sqrt(R2);
/* Use the acceleration to decide how big a timestep can be tolerated. */
if (W != 0.0) {
TS = DMIN(TSMAX,DMAX(TSMIN,TSC/W));
} else {
TS = TSMAX;
}
}
TS = TS*FB;
/* Override if final epoch is imminent. */
TLEFT = TSPAN-RTN;
if (fabs(TS) > fabs(TLEFT)) TS=TLEFT;
/* Epoch of middle of timestep. */
HTS = TS/2.0;
T = T0+RTN+HTS;
/* Is it time to recompute the major-planet elements? */
if (FIRST || fabs(T-TPEL)-AGEPEL >= TINY) {
/* Yes: go forward in time by just under the maximum allowed. */
TPEL = T+FB*AGEPEL;
/* Compute the state vector for the new epoch. */
for (NP=1; NP<=8; NP++) {
palPlanet(TPEL,NP,PV,&J);
/* Warning if remote epoch, abort if error. */
if (J == 1) {
*jstat = 102;
} else if (J != 0) {
goto ABORT;
}
/* Transform the vector into universal elements. */
palPv2ue(PV,TPEL,0.0,&(UP[NP-1][0]),&J);
if (J != 0) goto ABORT;
}
}
/* Is it time to recompute the major-planet motions? */
if (FIRST || fabs(T-TPMO)-AGEPMO >= TINY) {
/* Yes: look ahead. */
TPMO = T+FB*AGEPMO;
/* Compute the motions of each planet (AU,AU/d). */
for (NP=1; NP<=8; NP++) {
/* The planet's position and velocity (AU,AU/s). */
palUe2pv(TPMO,&(UP[NP-1][0]),&(PVIN[NP-1][0]),&J);
if (J != 0) goto ABORT;
/* Scale velocity to AU/d. */
for (J=3; J<6; J++) {
PVIN[NP-1][J] = PVIN[NP-1][J]*PAL__SPD;
}
/* Precompute also the extrapolation correction terms. */
R2 = 0.0;
for (I=0; I<3; I++) {
W = PVIN[NP-1][I];
R2 = R2+W*W;
}
R2X3[NP-1] = R2*3.0;
GC[NP-1] = GCON2/(2.0*R2*sqrt(R2));
}
}
/* Reset the first-time flag. */
FIRST = 0;
/* Unperturbed motion of the body at middle of timestep (AU,AU/s). */
palUe2pv(T,UL,PV0,&J);
if (J != 0) goto ABORT;
/* Perturbed position of the body (AU) and heliocentric distance cubed. */
R2 = 0.0;
for (I=0; I<3; I++) {
W = PV0[I]+PERP[I]+(PERV[I]+PERA[I]*HTS/2.0)*HTS;
PV[I] = W;
R2 = R2+W*W;
}
R3 = R2*sqrt(R2);
/* The body's unperturbed heliocentric distance cubed. */
R2 = 0.0;
for (I=0; I<3; I++) {
W = PV0[I];
R2 = R2+W*W;
}
R03 = R2*sqrt(R2);
/* Compute indirect and initialize direct parts of the perturbation. */
for (I=0; I<3; I++) {
FI[I] = PV0[I]/R03-PV[I]/R3;
FD[I] = 0.0;
}
/* Ready to compute the direct planetary effects. */
/* Reset the "near-Earth" flag. */
NE = 0;
/* Interval from state-vector epoch to middle of current timestep. */
DT = T-TPMO;
DT2 = DT*DT;
/* Planet by planet, including separate Earth and Moon. */
for (NP=1; NP<10; NP++) {
/* Which perturbing body? */
if (NP <= 8) {
/* Planet: compute the extrapolation in longitude (squared). */
R2 = 0.0;
for (J=3; J<6; J++) {
W = PVIN[NP-1][J]*DT;
R2 = R2+W*W;
}
/* Hence the tangential-to-circular correction factor. */
FC = 1.0+R2/R2X3[NP-1];
/* The radial correction factor due to the inwards acceleration. */
FG = 1.0-GC[NP-1]*DT2;
/* Planet's position. */
for (I=0; I<3; I++) {
RHO[I] = FG*(PVIN[NP-1][I]+FC*PVIN[NP-1][I+3]*DT);
}
} else if (NE) {
/* Near-Earth and either Earth or Moon. */
if (NP == 9) {
/* Earth: position. */
palEpv(T,PE,VH,PB,VB);
for (I=0; I<3; I++) {
RHO[I] = PE[I];
}
} else {
/* Moon: position. */
palPrec(palEpj(T),2000.0,PMAT);
palDmoon(T,PVM);
eraRxp(PMAT,PVM,PM);
for (I=0; I<3; I++) {
RHO[I] = PM[I]+PE[I];
}
}
}
/* Proceed unless Earth or Moon and not the near-Earth case. */
if (NP <= 8 || NE) {
/* Heliocentric distance cubed. */
R2 = 0.0;
for (I=0; I<3; I++) {
W = RHO[I];
R2 = R2+W*W;
}
R = sqrt(R2);
RHO3 = R2*R;
/* Body-to-planet vector, and distance. */
R2 = 0.0;
for (I=0; I<3; I++) {
W = RHO[I]-PV[I];
DELTA[I] = W;
R2 = R2+W*W;
}
R = sqrt(R2);
/* If this is the EMB, set the near-Earth flag appropriately. */
if (NP == 3 && R < RNE) NE = 1;
/* Proceed unless EMB and this is the near-Earth case. */
if ( ! (NE && NP == 3) ) {
/* If too close, ignore this planet and set a warning. */
if (R < COINC) {
*jstat = NP;
} else {
/* Accumulate "direct" part of perturbation acceleration. */
DELTA3 = R2*R;
W = AMAS[NP-1];
for (I=0; I<3; I++) {
FD[I] = FD[I]+(DELTA[I]/DELTA3-RHO[I]/RHO3)/W;
}
}
}
}
}
/* Update the perturbations to the end of the timestep. */
RTN += TS;
for (I=0; I<3; I++) {
W = (FI[I]+FD[I])*GCON2;
FT = W*TS;
PERP[I] = PERP[I]+(PERV[I]+FT/2.0)*TS;
PERV[I] = PERV[I]+FT;
PERA[I] = W;
}
/* Time still to go. */
TLEFT = TSPAN-RTN;
/* Is it either time to rectify the orbit or the last time through? */
if (fabs(RTN) >= AGEBEL || FB*TLEFT <= 0.0) {
/* Yes: update to the end of the current timestep. */
T0 += RTN;
RTN = 0.0;
/* The body's unperturbed motion (AU,AU/s). */
palUe2pv(T0,UL,PV0,&J);
if (J != 0) goto ABORT;
/* Add and re-initialize the perturbations. */
for (I=0; I<3; I++) {
J = I+3;
PV[I] = PV0[I]+PERP[I];
PV[J] = PV0[J]+PERV[I]/PAL__SPD;
PERP[I] = 0.0;
PERV[I] = 0.0;
PERA[I] = FD[I]*GCON2;
}
/* Use the position and velocity to set up new universal elements. */
palPv2ue(PV,T0,0.0,UL,&J);
if (J != 0) goto ABORT;
/* Adjust the timespan and time left. */
TSPAN = TFINAL-T0;
TLEFT = TSPAN;
}
/* Next timestep. */
}
/* Return the updated universal-element set. */
for (I=0; I<13; I++) {
u[I] = UL[I];
}
/* Finished. */
return;
/* Miscellaneous numerical error. */
ABORT:
*jstat = -1;
return;
}
/* changeu picks new values on a log scale over a very wide range
the recorded range is set in IMn1.c to be 1/1000 -1000 (i.e. over a
1,000,000 fold range. However the actual range is set to be much greater
This effectively sets the range to be infinite, and causes mutation rates to be picked without a prior.
By setting the actual range to be very wide, the prior probability within the range of 1/1000 -1000 becomes flat */
int
changeu (int ci, int j, int *k)
{
/* update the ratio between two mutation rate scalars . The upate is drawn from a uniform log scale between 1/maxratio and maxratio
maxratio is set to be 3 times the maximum value of the individual scalars */
double U, newr, metropolishastingsterm, olduj, olduk;
double newpdg[2], newkappa[2], likenewsum;
double d, r;
int i, li, lj, lk, ai, aj, ak;
static int start = 0;
static double windowsize, maxratio;
double tempr, temp;
int urcheck, counturcheck, uindex;
#ifdef _DEBUG
double prodcheck;
#endif /* _DEBUG */
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
/* windowsize and maxratio are on log scales
all mutation rate scalars have the same priors and windowsize */
if (start == 0)
{
/* 5/27/2011 changed the range expansion factor from 3 to 2, based on modeling that showed 2 is sufficient */
/* 11/6/2011 changed back to 3. no point changing to 2, and makes new results inconsistent with old results */
maxratio = 3.0 * L[0].u_rec[0].pr.max;
if (calcoptions[MUTATIONPRIORRANGE])
windowsize = L[0].u_rec[0].pr.max;
else
windowsize = L[0].u_rec[0].win;
start = 1;
}
/* 5/19/2011 JH adding thermodynamic integration (TI)*/
/* during TI one chain has a beta of zero, and in the case of updating mutation rate scalars this causes
all mutation scalar updates to be accepted - even very extreme values
don't quite understand this, but have turned off this update when beta=0, and have fixed mutation scalars to 1 in these cases
if (calcoptions[CALCMARGINALLIKELIHOOD] && ci == numchains - 1)
{
C[ci]->G[lj].uvals[aj] = C[ci]->G[lk].uvals[ak] = 1.0;
return 0;
} */
#ifdef _DEBUG
for (prodcheck = 1, ai = 0; ai < nurates; ai++)
{
prodcheck *= C[ci]->G[ul[ai].l].uvals[ul[ai].u];
}
assert (prodcheck < 1.00001 && prodcheck > 0.99999);
#endif /* _DEBUG */
if (nurates > 2)
{
do
{
*k = (int) (uniform () * nurates);
}
while (*k == j || *k < 0 || *k >= nurates);
}
else
{
assert (j == 0);
*k = 1;
}
lj = ul[j].l;
aj = ul[j].u;
lk = ul[*k].l;
ak = ul[*k].u;
assert (*k != j);
olduj = C[ci]->G[lj].uvals[aj];
olduk = C[ci]->G[lk].uvals[ak];
r = log (olduj / olduk);
if (calcoptions[MUTATIONPRIORRANGE])
{
counturcheck = 0;
do
{
if (counturcheck >= MAXURCHECK)
{
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
return -1;
}
U = uniform ();
if (U > 0.5)
newr = r + (2.0 * U - 1.0) * windowsize;
else
newr = r - windowsize * U * 2.0;
if (newr > maxratio)
newr = 2.0 * maxratio - newr;
else if (newr < -maxratio)
newr = 2.0 * (-maxratio) - newr;
d = exp ((newr - r) / 2);
/* urri[i][j] has a 0 if neither i nor j has a prior. 1 if i has a prior and j does not, -1 if i does not have a pior and j does */
/* must check if the new rates will cause the ratios among scalars to fall outside of the allowed ranges of ratios */
/* if they do, then they are rejected */
urcheck = 0;
counturcheck++;
if (urri[j][*k] == 2) // both scalars have priors
{
urcheck = (newr <= urrlow[j][*k] || newr >= urrhi[j][*k]);
for (uindex = 0; uindex < nurates; uindex++)
{
if (urri[j][uindex] == 2 && uindex != *k)
{
tempr =
log (d * C[ci]->G[lj].uvals[aj] /
C[ci]->G[ul[uindex].l].uvals[ul[uindex].u]);
urcheck = urcheck || (tempr <= urrlow[j][uindex]
|| tempr >= urrhi[j][uindex]);
}
if (urcheck)
break;
}
for (uindex = 0; uindex < nurates; uindex++)
{
if (urri[uindex][*k] == 2 && uindex != j)
{
tempr =
log (C[ci]->G[ul[uindex].l].uvals[ul[uindex].u] /
(C[ci]->G[lk].uvals[ak] / d));
urcheck = urcheck || (tempr <= urrlow[uindex][*k]
|| tempr >= urrhi[uindex][*k]);
}
if (urcheck)
break;
}
}
else
{
if (urri[j][*k] == 1) // the uj scalar has a prior, but not the uk scalar
{
for (uindex = 0; uindex < nurates; uindex++)
{
if (urri[j][uindex] == 2)
{
tempr =
log (d * C[ci]->G[lj].uvals[aj] /
C[ci]->G[ul[uindex].l].uvals[ul[uindex].u]);
urcheck = urcheck || (tempr <= urrlow[j][uindex]
|| tempr >= urrhi[j][uindex]);
}
if (urcheck)
break;
}
}
if (urri[j][*k] == -1) // the uk scalar has a prior, but not the uj scalar
{
for (uindex = 0; uindex < nurates; uindex++)
{
if (urri[uindex][*k] == 2)
{
tempr =
log (C[ci]->G[ul[uindex].l].uvals[ul[uindex].u] /
(C[ci]->G[lk].uvals[ak] / d));
urcheck = urcheck || (tempr <= urrlow[uindex][*k]
|| tempr >= urrhi[uindex][*k]);
}
if (urcheck)
break;
}
}
}
}
while (urcheck);
}
else
{
U = uniform ();
if (U > 0.5)
newr = r + (2.0 * U - 1.0) * windowsize;
else
newr = r - windowsize * U * 2.0;
if (newr > maxratio)
newr = 2.0 * maxratio - newr;
else if (newr < -maxratio)
newr = 2.0 * (-maxratio) - newr;
d = exp ((newr - r) / 2);
}
C[ci]->G[lj].uvals[aj] *= d;
C[ci]->G[lk].uvals[ak] /= d;
/*JH 5/25/2011, 9/28/2011 */
/* this block of code causes rejection of cases when the scalar is outside the range of tempUMAX
played around with this 5/25/2011 and 9/28/2011 to see effect on marginal likelihood estimates
using thermodynamic integration. Did not see much effect.
This does cause a nonzero rejection rate when run without data and in the absence of data it causes
the curves for these scalaras to not be flat, but have a broad curving peak centered on 1
as of 10/3/2011 - not in use */
/*#define tempUMAX UMAX //1000.0 //1000.0
if (C[ci]->G[lj].uvals[aj] > tempUMAX || C[ci]->G[lj].uvals[aj] < 1.0/tempUMAX || C[ci]->G[lk].uvals[ak] > tempUMAX || C[ci]->G[lk].uvals[ak] < 1.0/tempUMAX)
{
C[ci]->G[lj].uvals[aj] = olduj;
C[ci]->G[lk].uvals[ak] = olduk;
if (L[lj].umodel[aj] == HKY)
restorescalefactors (ci, lj);
if (L[lk].umodel[ak] == HKY)
restorescalefactors (ci, lk);
return 0;
} */
likenewsum = 0;
for (i = 0; i < 2; i++)
{
if (i == 0)
{
li = lj;
ai = aj;
}
else
{
li = lk;
ai = ak;
}
switch (L[li].umodel[ai])
{
case HKY:
if (assignmentoptions[JCMODEL] == 1)
{
newpdg[i] = likelihoodJC (ci, li, C[ci]->G[li].uvals[ai]);
}
else
{
U = uniform ();
if (U > 0.5)
{
newkappa[i] =
C[ci]->G[li].kappaval + (2.0 * U - 1.0) * L[li].kappa_rec->win;
if (newkappa[i] > L[li].kappa_rec->pr.max)
newkappa[i] = 2.0 * L[li].kappa_rec->pr.max - newkappa[i];
}
else
{
newkappa[i] = C[ci]->G[li].kappaval - L[li].kappa_rec->win * U * 2.0;
if (newkappa[i] < 0)
newkappa[i] = -newkappa[i];
}
if (ci == 0)
L[li].kappa_rec->upinf->tries++;
newpdg[i] =
likelihoodHKY (ci, li, C[ci]->G[li].uvals[ai], newkappa[i],
-1, -1, -1, -1);
}
break;
case INFINITESITES:
newpdg[i] = likelihoodIS (ci, li, C[ci]->G[li].uvals[ai]);
break;
case STEPWISE:
newpdg[i] = likelihoodSW (ci, li, ai, C[ci]->G[li].uvals[ai], 1.0);
break;
}
likenewsum += newpdg[i] - C[ci]->G[li].pdg_a[ai];
/* 5/19/2011 JH adding thermodynamic integration */
/* this use of beta is not affected by whether or not the probability of the genealogy is included for this update,
since it is not present in this MH term */
metropolishastingsterm = exp (beta[ci] * gbeta * likenewsum); // 8/26/2011 this should be outside the loop
}
U = uniform ();
if (U < DMIN(1.0, metropolishastingsterm)) //9/13/2010
//if (metropolishastingsterm >= 1.0 || metropolishastingsterm > U)
{
for (i = 0; i < 2; i++)
{
if (i == 0)
{
li = lj;
ai = aj;
}
else
{
li = lk;
ai = ak;
}
C[ci]->G[li].pdg += newpdg[i] - C[ci]->G[li].pdg_a[ai];
C[ci]->G[li].pdg_a[ai] = newpdg[i];
if (L[li].umodel[ai] == HKY)
{
assert (ai == 0);
C[ci]->G[li].kappaval = newkappa[i];
if (ci == 0)
L[li].kappa_rec->upinf->accp++;
copyfraclike (ci, li);
storescalefactors (ci, li);
}
}
C[ci]->allpcalc.pdg += likenewsum;
#ifdef _DEBUG
for (prodcheck = 1, ai = 0; ai < nurates; ai++)
{
prodcheck *= C[ci]->G[ul[ai].l].uvals[ul[ai].u];
}
assert (prodcheck < 1.00001 && prodcheck > 0.99999);
#endif /* */
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
return 1;
}
else
{
C[ci]->G[lj].uvals[aj] = olduj;
C[ci]->G[lk].uvals[ak] = olduk;
if (L[lj].umodel[aj] == HKY)
restorescalefactors (ci, lj);
if (L[lk].umodel[ak] == HKY)
restorescalefactors (ci, lk);
if (L[lj].umodel[aj] == STEPWISE)
{
temp = likelihoodSW (ci, lj, aj, olduj, 1.0);
C[ci]->G[lj].pdg += temp - C[ci]->G[lj].pdg_a[aj];
C[ci]->G[lj].pdg_a[aj] = temp;
}
if (L[lk].umodel[ak] == STEPWISE)
{
temp = likelihoodSW (ci, lk, ak, olduk, 1.0);
C[ci]->G[lk].pdg += temp - C[ci]->G[lk].pdg_a[ak];
C[ci]->G[lk].pdg_a[ak] = temp;
}
#ifdef _DEBUG
for (prodcheck = 1, ai = 0; ai < nurates; ai++)
{
prodcheck *= C[ci]->G[ul[ai].l].uvals[ul[ai].u];
}
assert (prodcheck < 1.00001 && prodcheck > 0.99999);
#endif /* */
if (assignmentoptions[POPULATIONASSIGNMENTCHECKPOINT] == 1)
{
assertgenealogy (ci);
}
return 0;
}
#ifdef _DEBUG
for (i = 0, temp = 0; i < nloci; i++)
for (ai = 0; ai < L[i].nlinked; ai++)
temp += C[0]->G[i].pdg_a[ai];
#endif /* */
assert (fabs (temp - C[0]->allpcalc.pdg) < 1e-10);
} /* changeu */
/* Ouput: y=updated_conc, xx=end_time, hdid=achieved_stepsize , hnext= estimated_next_ss */
bool stifbs(double y[], double dydx[], int nv, double *xx, double htry, double eps,
double yscal[], double *hdid, double *hnext,
void (*derivs)(double, double [], double [], int, long, double), long node)
{
int i,iq,k,kk,km;
static int first=1,kmax,kopt,nvold = -1;
static double epsold = -1.0,xnew;
double eps1,errmax,fact,h,red,scale,work,wrkmin,xest;
double *dfdx,**dfdy,*err,*yerr,*ysav,*yseq;
static double a[IMAXX+1];
static double alf[KMAXX+1][KMAXX+1];
static int nseq[IMAXX+1]={0,2,6,10,14,22,34,50,70};
// static int nseq[IMAXX+1]={0,2,6,10,14,22,34,50,70,98,138,194,274,386};
// Num Recip S. 744
// Differenz zwischen zwei Werten muss ein Vielfaches von 4 sein
// So wählen, dass Verhältnis der Werte <= 5/7 ist
// z.B. 10, 14 -> 10/14 <= 5/7
// nächster wäre 18, aber 14/18 > 5/7, deshalb 22 mit 14/22 <= 5/7
int reduct,exitflag=0;
km=0; //SB avoid warnings
red = 0.0; //SB avoid warning
errmax = 0.0; // SB avoid warning
scale = 0.0; // SB avoid warning
bool success = true;
d=dmatrix(1,nv,1,KMAXX);
dfdx=dvector(1,nv);
dfdy=dmatrix(1,nv,1,nv);
err=dvector(1,KMAXX);
x=dvector(1,KMAXX);
yerr=dvector(1,nv);
ysav=dvector(1,nv);
yseq=dvector(1,nv);
// reinitialize as eps is a new tolerance
// or if nv have changed
if(eps != epsold || nv != nvold)
{
*hnext = xnew = -1.0e29; // impossible values
eps1=SAFE1*eps;
// compute work coefficients Ak
a[1]=nseq[1]+1;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
// compute alpha(k,q)
for (iq=2;iq<=KMAXX;iq++)
{
for (k=1;k<iq;k++)
alf[k][iq]=pow(eps1,((a[k+1]-a[iq+1])/
((a[iq+1]-a[1]+1.0)*(2*k+1))));
}
epsold=eps;
// save nv
nvold=nv;
// add cost of Jacobian evals to work coeffs a[]
a[1] += nv;
for (k=1;k<=KMAXX;k++) a[k+1]=a[k]+nseq[k+1];
// Determine opt. row no. for convergence
for (kopt=2;kopt<KMAXX;kopt++)
if (a[kopt+1] > a[kopt]*alf[kopt-1][kopt]) break;
kmax=kopt;
}
h=htry;
// save starting values
for (i=1;i<=nv;i++) ysav[i]=y[i];
// evaluate jacobian matrix, update dfdx, dfdy
jacobn(*xx,y,dfdx,dfdy,nv,node);
// new stepsize or new integration --> re-establish order window
if (*xx != xnew || h != (*hnext))
{
first=1;
kopt=kmax;
}
reduct=0;
// start stepping
for (;;)
{
// evaluate the sequence of modified midpoint integrations
for (k=1;k<=kmax;k++)
{
xnew=(*xx)+h;
if (xnew == (*xx)) //CB
{
std::cout << "step size underflow in stifbs" << "\n";
//nrerror("step size underflow in stifbs");
success = false;
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(dfdy,1,nv,1,nv);
free_dvector(dfdx,1,nv);
free_dmatrix(d,1,KMAXX,1,KMAXX);
return success;
}
// semi-implicite midpoint algorithm
success = simpr(ysav,dydx,dfdx,dfdy,nv,*xx,h,nseq[k],yseq,derivs,node);
if(success==0){
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(dfdy,1,nv,1,nv);
free_dvector(dfdx,1,nv);
free_dmatrix(d,1,KMAXX,1,KMAXX);
return success;
}
// squared since error is even
xest=DSQR(h/nseq[k]);
pzextr(k,xest,yseq,y,yerr,nv);
// compute normalized error estimate eps(k)
if (k != 1)
{
errmax=TINY;
for (i=1;i<=nv;i++) errmax=DMAX(errmax,fabs(yerr[i]/yscal[i]));
// scale error relative to tolerance
errmax /= eps;
km=k-1;
err[km]=pow(errmax/SAFE1,1.0/(double)(2*km+1));
}
if (k != 1 && (k >= kopt-1 || first))
{
if (errmax < 1.0)
{
exitflag=1;
break;
}
if (k == kmax || k == kopt+1)
{
red=SAFE2/err[km];
break;
}
else if (k == kopt && alf[kopt-1][kopt] < err[km])
{
red=1.0/err[km];
break;
}
else if (kopt == kmax && alf[km][kmax-1] < err[km])
{
red=alf[km][kmax-1]*SAFE2/err[km];
break;
}
else if (alf[km][kopt] < err[km])
{
red=alf[km][kopt-1]/err[km];
break;
}
}
}
// if (exitflag) std::cout << " Exitflag > 0 in stifbs of biodegradation" << "\n";
if (exitflag) break;
// reduce stepsize by at least REDMIN and at most by REDMAX
red=DMIN(red,REDMIN);
red=DMAX(red,REDMAX);
h *= red;
reduct=1;
} // try again
// successfull step was taken
*xx=xnew;
*hdid=h;
first=0;
wrkmin=1.0e35;
// compute optimal row for convergence and corresponding stepsize
for (kk=1;kk<=km;kk++)
{
fact=DMAX(err[kk],SCALMX);
work=fact*a[kk+1];
if (work < wrkmin)
{
scale=fact;
wrkmin=work;
kopt=kk+1;
}
}
*hnext=h/scale;
if (kopt >= k && kopt != kmax && !reduct)
{
// check for possible order increse but not if stepsize was just reduced
fact=DMAX(scale/alf[kopt-1][kopt],SCALMX);
if (a[kopt+1]*fact <= wrkmin)
{
*hnext=h/fact;
kopt++;
}
}
free_dvector(yseq,1,nv);
free_dvector(ysav,1,nv);
free_dvector(yerr,1,nv);
free_dvector(x,1,KMAXX);
free_dvector(err,1,KMAXX);
free_dmatrix(dfdy,1,nv,1,nv);
free_dvector(dfdx,1,nv);
free_dmatrix(d,1,KMAXX,1,KMAXX);
return success;
}
double
igamma (double a, double x)
{
double arg, prefactor;
int i;
#ifdef HAVE_MATHERR
#ifdef __cplusplus
struct __exception exc;
#else
struct exception exc;
#endif
#endif
if (x < 0.0 || a <= 0.0) /* DOMAIN error */
{
#ifdef HAVE_MATHERR
exc.name = (char *)"igamma";
exc.arg1 = a;
exc.arg2 = x;
exc.retval = HUGE_VAL;
exc.type = DOMAIN;
if (!matherr (&exc))
{
fprintf (stderr, "igamma: DOMAIN error\n");
errno = EDOM;
}
return exc.retval;
#else
errno = EDOM;
return HUGE_VAL;
#endif
}
if (x > XBIG) /* TLOSS error */
{
#ifdef HAVE_MATHERR
exc.name = (char *)"igamma";
exc.arg1 = a;
exc.arg2 = x;
exc.retval = 1.0;
exc.type = TLOSS;
if (!matherr (&exc))
{
fprintf (stderr, "igamma: TLOSS error\n");
errno = EDOM;
}
return exc.retval;
#else
errno = EDOM;
return 1.0;
#endif
}
if (x == 0.0)
return 0.0;
/* check exponentiation in prefactor */
arg = a * log (x) - x - F_LGAMMA(a + 1.0);
if (arg < MINEXP)
{
#ifdef HAVE_MATHERR
exc.name = (char *)"igamma";
exc.arg1 = a;
exc.arg2 = x;
exc.retval = 0.0;
exc.type = TLOSS;
if (!matherr (&exc))
{
fprintf (stderr, "ibeta: TLOSS error\n");
errno = EDOM;
}
return exc.retval;
#else
errno = EDOM;
return 0.0;
#endif
}
prefactor = exp (arg);
if ((x > 1.0) && (x >= a + 2.0))
/* use the continued fraction, not Pearson's series; generate its
convergents by Wallis's method */
{
double A0, B0, A1, B1, A2, B2;
double f0 = 0.0, f1; /* f0 initted to quiet compiler */
double aa, bb;
int goodf0, goodf1 = 1;
aa = 1.0 - a;
bb = aa + x + 1.0;
A2 = 1.0;
B2 = x;
A1 = x + 1.0;
B1 = x * bb;
f1 = A1 / B1;
for (i = 1; i <= ITERMAX; i++)
{
aa++;
bb += 2.0;
A0 = bb * A1 - i * aa * A2;
B0 = bb * B1 - i * aa * B2;
if (B0 != 0.0)
{
f0 = A0 / B0;
if (goodf1 &&
fabs (f0 - f1) <= DMIN(MACHEPS, fabs (f0) * MACHEPS))
return (1.0 - prefactor * a * f0);
goodf0 = 1;
}
else
goodf0 = 0;
/* shift down */
A2 = A1;
B2 = B1;
A1 = A0;
B1 = B0;
f1 = f0;
goodf1 = goodf0;
if (fabs(A0) >= OFLOW)
/* renormalize */
{
A2 /= OFLOW;
B2 /= OFLOW;
A1 /= OFLOW;
B1 /= OFLOW;
}
}
}
else
/* use Pearson's series, not the continued fraction */
{
double aa, bb, cc;
aa = a;
bb = 1.0;
cc = 1.0;
for (i = 0; i <= ITERMAX; i++)
{
aa++;
cc *= (x / aa);
bb += cc;
if (cc < bb * MACHEPS)
return prefactor * bb;
}
}
/* if we reached here, convergence failed */
#ifdef HAVE_MATHERR
exc.name = (char *)"igamma";
exc.arg1 = a;
exc.arg2 = x;
exc.retval = HUGE_VAL;
exc.type = TLOSS;
if (!matherr (&exc))
{
fprintf (stderr, "ibeta: TLOSS error\n");
errno = EDOM;
}
return exc.retval;
#else
errno = EDOM;
return HUGE_VAL;
#endif
}
bool rkqs(double y[], double dydx[], int n, double *x, double htry, double eps,
double yscal[], double *hdid, double *hnext,
void (*derivs)(double , double [], double [], int, long, double), long node)
{
void rkck(double y[], double dydx[], int n, double x, double h,
double yout[], double yerr[], void (*derivs)(double , double [], double [], int, long, double), long node);
int i;
double errmax,h,htemp,xnew,*yerr,*ytemp;
bool success = false;
double *epsi;
// y[] - c(step)
// dydx[] - dcdt(step)
// yscal - scaling of errors
yerr=dvector(1,n);
ytemp=dvector(1,n);
h=htry; //set stepsize to initial trial value
epsi=dvector(1,n);
for (i=1;i<=n;i++)
epsi[i]=eps;
//epsi[2]= eps*1000;
//epsi[20]= eps*1000;
//epsi[21]= eps*1000;
for (;;) {
rkck(y,dydx,n,*x,h,ytemp,yerr,derivs, node); // take a step --> update yerr, ytemp
//evaluate accuracy
errmax=0.0;
//for (i=1;i<=n;i++)
//yscal[i]=fabs(y[i])+fabs(dydx[i]*h)+TINY; // for Runge Kutta, this is done in driver routine odeint
//yscal[i]= eps + eps*DMAX(fabs(y[i]),fabs(ytemp[i]));
//yscal[i]= 0 + eps*fabs(y[i]);
for (i=1;i<=n;i++)
//if(i==2 || i==20 || i==21 )
// errmax=DMAX(errmax,errmax/10); // find max rel. error |d0/d1|
// errmax=DMAX(errmax, fabs(yerr[i]/yscal[i])); // find max rel. error |d0/d1|
errmax=DMAX(errmax, fabs(yerr[i]/yscal[i])/(epsi[i])); // find max rel. error |d0/d1|
//errmax += pow(yerr[i]/yscal[i],2);
//errmax = DMAX(errmax, pow(yerr[i]/yscal[i],2));
//errmax /= eps; // SCALE relative to required tolerance
//errmax = sqrt(errmax/n);
//errmax = sqrt(errmax);
if(DEBUGRK>0){
rkqs_yerr << "node" << node <<" " << *x << " " << h << " ";
for (i=1;i<=25;i++) rkqs_yerr << " " << fabs(yerr[ idcs[i] ]) ;
rkqs_yerr << "\n";
rkqs_ytemp << "node" << node <<" " << *x << " " << h << " ";
for (i=1;i<=25;i++) rkqs_ytemp << " " << ytemp[ idcs[i] ] ;
rkqs_ytemp << "\n";
rkqs_errmax << "node" << node <<" " << *x << " " << h << " ";
for (i=1;i<=25;i++) {
rkqs_errmax << " " << fabs((yerr[ idcs[i] ] / yscal[ idcs[i] ]))/epsi[i];
}
rkqs_errmax << " " << errmax;
rkqs_errmax << "\n";
}
if (errmax <= 1.0) break; // success, break and compute next step size
// failed, truncation error too large
htemp=SAFETY*h*pow(errmax,PSHRNK); // reduce step size
h=(h >= 0.0 ? (htemp,0.1*h) : DMIN(htemp,0.1*h)); // no more than factor of 1/10
xnew=(*x)+h;
if (xnew == *x){
std::cout << "step size underflow in rkqs" << "\n";
//nrerror("stepsize underflow in rkqs");
free_dvector(ytemp,1,n);
free_dvector(yerr,1,n);
success = false;
return success;
}
}
if(DEBUGRK>0){
rkqs_yerr << "\n";
rkqs_ytemp << "\n";
rkqs_errmax << "\n";
}
if (errmax > ERRCON) *hnext=SAFETY*h*pow(errmax,PGROW);
else *hnext=5.0*h;
//if(*hnext > 3500) *hnext=3500;
*x += (*hdid=h);
for (i=1;i<=n;i++) y[i]=ytemp[i]; // update y[i] concentration vector
free_dvector(ytemp,1,n);
free_dvector(yerr,1,n);
free_dvector(epsi,1,n);
success = true;
return success;
}