/*private*/
int
LineIntersector::computeIntersect(const Coordinate& p1,const Coordinate& p2,const Coordinate& q1,const Coordinate& q2)
{
#if GEOS_DEBUG
cerr<<"LineIntersector::computeIntersect called"<<endl;
cerr<<" p1:"<<p1.toString()<<" p2:"<<p2.toString()<<" q1:"<<q1.toString()<<" q2:"<<q2.toString()<<endl;
#endif // GEOS_DEBUG
isProperVar=false;
// first try a fast test to see if the envelopes of the lines intersect
if (!Envelope::intersects(p1,p2,q1,q2))
{
#if GEOS_DEBUG
cerr<<" NO_INTERSECTION"<<endl;
#endif
return NO_INTERSECTION;
}
// for each endpoint, compute which side of the other segment it lies
// if both endpoints lie on the same side of the other segment,
// the segments do not intersect
int Pq1=CGAlgorithms::orientationIndex(p1,p2,q1);
int Pq2=CGAlgorithms::orientationIndex(p1,p2,q2);
if ((Pq1>0 && Pq2>0) || (Pq1<0 && Pq2<0))
{
#if GEOS_DEBUG
cerr<<" NO_INTERSECTION"<<endl;
#endif
return NO_INTERSECTION;
}
int Qp1=CGAlgorithms::orientationIndex(q1,q2,p1);
int Qp2=CGAlgorithms::orientationIndex(q1,q2,p2);
if ((Qp1>0 && Qp2>0)||(Qp1<0 && Qp2<0)) {
#if GEOS_DEBUG
cerr<<" NO_INTERSECTION"<<endl;
#endif
return NO_INTERSECTION;
}
bool collinear=Pq1==0 && Pq2==0 && Qp1==0 && Qp2==0;
if (collinear) {
#if GEOS_DEBUG
cerr<<" computingCollinearIntersection"<<endl;
#endif
return computeCollinearIntersection(p1,p2,q1,q2);
}
/**
* At this point we know that there is a single intersection point
* (since the lines are not collinear).
*/
/*
* Check if the intersection is an endpoint.
* If it is, copy the endpoint as
* the intersection point. Copying the point rather than
* computing it ensures the point has the exact value,
* which is important for robustness. It is sufficient to
* simply check for an endpoint which is on the other line,
* since at this point we know that the inputLines must
* intersect.
*/
if (Pq1==0 || Pq2==0 || Qp1==0 || Qp2==0) {
#if COMPUTE_Z
int hits=0;
double z=0.0;
#endif
isProperVar=false;
/* Check for two equal endpoints.
* This is done explicitly rather than by the orientation tests
* below in order to improve robustness.
*
* (A example where the orientation tests fail
* to be consistent is:
*
* LINESTRING ( 19.850257749638203 46.29709338043669,
* 20.31970698357233 46.76654261437082 )
* and
* LINESTRING ( -48.51001596420236 -22.063180333403878,
* 19.850257749638203 46.29709338043669 )
*
* which used to produce the result:
* (20.31970698357233, 46.76654261437082, NaN)
*/
if ( p1.equals2D(q1) || p1.equals2D(q2) ) {
intPt[0]=p1;
#if COMPUTE_Z
if ( !ISNAN(p1.z) )
{
z += p1.z;
hits++;
}
#endif
}
else if ( p2.equals2D(q1) || p2.equals2D(q2) ) {
intPt[0]=p2;
#if COMPUTE_Z
if ( !ISNAN(p2.z) )
{
z += p2.z;
hits++;
}
#endif
}
/**
* Now check to see if any endpoint lies on the interior of the other segment.
*/
else if (Pq1==0) {
intPt[0]=q1;
#if COMPUTE_Z
if ( !ISNAN(q1.z) )
{
z += q1.z;
hits++;
}
#endif
}
else if (Pq2==0) {
intPt[0]=q2;
#if COMPUTE_Z
if ( !ISNAN(q2.z) )
{
z += q2.z;
hits++;
}
#endif
}
else if (Qp1==0) {
intPt[0]=p1;
#if COMPUTE_Z
if ( !ISNAN(p1.z) )
{
z += p1.z;
hits++;
}
#endif
}
else if (Qp2==0) {
intPt[0]=p2;
#if COMPUTE_Z
if ( !ISNAN(p2.z) )
{
z += p2.z;
hits++;
}
#endif
}
#if COMPUTE_Z
#if GEOS_DEBUG
cerr<<"LineIntersector::computeIntersect: z:"<<z<<" hits:"<<hits<<endl;
#endif // GEOS_DEBUG
if ( hits ) intPt[0].z = z/hits;
#endif // COMPUTE_Z
} else {
isProperVar=true;
intersection(p1, p2, q1, q2, intPt[0]);
}
#if GEOS_DEBUG
cerr<<" POINT_INTERSECTION; intPt[0]:"<<intPt[0].toString()<<endl;
#endif // GEOS_DEBUG
return POINT_INTERSECTION;
}
bool Main::compileHeuristic() {
m_options->ibound = min(m_options->ibound, m_pseudotree->getWidthCond());
size_t sz = 0;
if (m_options->memlimit != NONE) {
sz = m_heuristic->limitSize(m_options->memlimit,
& m_search->getAssignment() );
sz *= sizeof(double) / (1024*1024.0);
cout << "Enforcing memory limit resulted in i-bound " << m_options->ibound
<< " with " << sz << " MByte." << endl;
}
if (m_options->nosearch) {
cout << "Simulating mini bucket heuristic..." << endl;
sz = m_heuristic->build(& m_search->getAssignment(), false); // false = just compute memory estimate
}
else {
time(&_time_pre);
bool mbFromFile = false;
if (!m_options->in_minibucketFile.empty()) {
mbFromFile = m_heuristic->readFromFile(m_options->in_minibucketFile);
sz = m_heuristic->getSize();
}
if (!mbFromFile) {
cout << "Computing mini bucket heuristic..." << endl;
sz = m_heuristic->build(& m_search->getAssignment(), true); // true = actually compute heuristic
time_t cur_time;
time(&cur_time);
double time_passed = difftime(cur_time, _time_pre);
cout << "\tMini bucket finished in " << time_passed << " seconds" << endl;
}
if (!mbFromFile && !m_options->in_minibucketFile.empty()) {
cout << "\tWriting mini bucket to file " << m_options->in_minibucketFile << " ..." << flush;
m_heuristic->writeToFile(m_options->in_minibucketFile);
cout << " done" << endl;
}
}
cout << '\t' << (sz / (1024*1024.0)) * sizeof(double) << " MBytes" << endl;
// heuristic might have changed problem functions, pseudotree needs remapping
m_pseudotree->addFunctionInfo(m_problem->getFunctions());
// set initial lower bound if provided (but only if no subproblem was specified)
if (m_options->in_subproblemFile.empty() ) {
if (m_options->in_boundFile.size()) {
cout << "Loading initial lower bound from file " << m_options->in_boundFile << '.' << endl;
if (!m_search->loadInitialBound(m_options->in_boundFile)) {
err_txt("Loading initial bound failed");
return false;
}
} else if (!ISNAN ( m_options->initialBound )) {
#ifdef NO_ASSIGNMENT
cout << "Setting external lower bound " << m_options->initialBound << endl;
m_search->updateSolution(m_options->initialBound);
#else
err_txt("Compiled with tuple support, value-based bound not possible.");
return false;
#endif
}
}
#ifndef NO_HEURISTIC
if (m_search->getCurOptValue() >= m_heuristic->getGlobalUB()) {
m_solved = true;
cout << endl << "--------- Solved during preprocessing ---------" << endl;
}
else if (m_heuristic->isAccurate()) {
cout << endl << "Heuristic is accurate!" << endl;
m_options->lds = 0; // set LDS to 0 (sufficient given perfect heuristic)
m_solved = true;
}
#endif
return true;
}
struct fpn *
fpu_add(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1, *y = &fe->fe_f2, *r;
u_int r0, r1, r2, r3;
int rd;
/*
* Put the `heavier' operand on the right (see fpu_emu.h).
* Then we will have one of the following cases, taken in the
* following order:
*
* - y = NaN. Implied: if only one is a signalling NaN, y is.
* The result is y.
* - y = Inf. Implied: x != NaN (is 0, number, or Inf: the NaN
* case was taken care of earlier).
* If x = -y, the result is NaN. Otherwise the result
* is y (an Inf of whichever sign).
* - y is 0. Implied: x = 0.
* If x and y differ in sign (one positive, one negative),
* the result is +0 except when rounding to -Inf. If same:
* +0 + +0 = +0; -0 + -0 = -0.
* - x is 0. Implied: y != 0.
* Result is y.
* - other. Implied: both x and y are numbers.
* Do addition a la Hennessey & Patterson.
*/
DPRINTF(FPE_REG, ("fpu_add:\n"));
DUMPFPN(FPE_REG, x);
DUMPFPN(FPE_REG, y);
DPRINTF(FPE_REG, ("=>\n"));
ORDER(x, y);
if (ISNAN(y)) {
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, y);
return (y);
}
if (ISINF(y)) {
if (ISINF(x) && x->fp_sign != y->fp_sign) {
fe->fe_cx |= FPSCR_VXISI;
return (fpu_newnan(fe));
}
DUMPFPN(FPE_REG, y);
return (y);
}
rd = ((fe->fe_fpscr) & FPSCR_RN);
if (ISZERO(y)) {
if (rd != FP_RM) /* only -0 + -0 gives -0 */
y->fp_sign &= x->fp_sign;
else /* any -0 operand gives -0 */
y->fp_sign |= x->fp_sign;
DUMPFPN(FPE_REG, y);
return (y);
}
if (ISZERO(x)) {
DUMPFPN(FPE_REG, y);
return (y);
}
/*
* We really have two numbers to add, although their signs may
* differ. Make the exponents match, by shifting the smaller
* number right (e.g., 1.011 => 0.1011) and increasing its
* exponent (2^3 => 2^4). Note that we do not alter the exponents
* of x and y here.
*/
r = &fe->fe_f3;
r->fp_class = FPC_NUM;
if (x->fp_exp == y->fp_exp) {
r->fp_exp = x->fp_exp;
r->fp_sticky = 0;
} else {
if (x->fp_exp < y->fp_exp) {
/*
* Try to avoid subtract case iii (see below).
* This also guarantees that x->fp_sticky = 0.
*/
SWAP(x, y);
}
/* now x->fp_exp > y->fp_exp */
r->fp_exp = x->fp_exp;
r->fp_sticky = fpu_shr(y, x->fp_exp - y->fp_exp);
}
r->fp_sign = x->fp_sign;
if (x->fp_sign == y->fp_sign) {
FPU_DECL_CARRY
/*
* The signs match, so we simply add the numbers. The result
* may be `supernormal' (as big as 1.111...1 + 1.111...1, or
* 11.111...0). If so, a single bit shift-right will fix it
* (but remember to adjust the exponent).
*/
/* r->fp_mant = x->fp_mant + y->fp_mant */
FPU_ADDS(r->fp_mant[3], x->fp_mant[3], y->fp_mant[3]);
FPU_ADDCS(r->fp_mant[2], x->fp_mant[2], y->fp_mant[2]);
FPU_ADDCS(r->fp_mant[1], x->fp_mant[1], y->fp_mant[1]);
FPU_ADDC(r0, x->fp_mant[0], y->fp_mant[0]);
if ((r->fp_mant[0] = r0) >= FP_2) {
(void) fpu_shr(r, 1);
r->fp_exp++;
}
} else {
FPU_DECL_CARRY
/*
* The signs differ, so things are rather more difficult.
* H&P would have us negate the negative operand and add;
* this is the same as subtracting the negative operand.
* This is quite a headache. Instead, we will subtract
* y from x, regardless of whether y itself is the negative
* operand. When this is done one of three conditions will
* hold, depending on the magnitudes of x and y:
* case i) |x| > |y|. The result is just x - y,
* with x's sign, but it may need to be normalized.
* case ii) |x| = |y|. The result is 0 (maybe -0)
* so must be fixed up.
* case iii) |x| < |y|. We goofed; the result should
* be (y - x), with the same sign as y.
* We could compare |x| and |y| here and avoid case iii,
* but that would take just as much work as the subtract.
* We can tell case iii has occurred by an overflow.
*
* N.B.: since x->fp_exp >= y->fp_exp, x->fp_sticky = 0.
*/
/* r->fp_mant = x->fp_mant - y->fp_mant */
FPU_SET_CARRY(y->fp_sticky);
FPU_SUBCS(r3, x->fp_mant[3], y->fp_mant[3]);
FPU_SUBCS(r2, x->fp_mant[2], y->fp_mant[2]);
FPU_SUBCS(r1, x->fp_mant[1], y->fp_mant[1]);
FPU_SUBC(r0, x->fp_mant[0], y->fp_mant[0]);
if (r0 < FP_2) {
/* cases i and ii */
if ((r0 | r1 | r2 | r3) == 0) {
/* case ii */
r->fp_class = FPC_ZERO;
r->fp_sign = rd == FP_RM;
return (r);
}
} else {
/*
* Oops, case iii. This can only occur when the
* exponents were equal, in which case neither
* x nor y have sticky bits set. Flip the sign
* (to y's sign) and negate the result to get y - x.
*/
#ifdef DIAGNOSTIC
if (x->fp_exp != y->fp_exp || r->fp_sticky)
panic("fpu_add");
#endif
r->fp_sign = y->fp_sign;
FPU_SUBS(r3, 0, r3);
FPU_SUBCS(r2, 0, r2);
FPU_SUBCS(r1, 0, r1);
FPU_SUBC(r0, 0, r0);
}
r->fp_mant[3] = r3;
r->fp_mant[2] = r2;
r->fp_mant[1] = r1;
r->fp_mant[0] = r0;
if (r0 < FP_1)
fpu_norm(r);
}
DUMPFPN(FPE_REG, r);
return (r);
}
SEXP
KalmanLike(SEXP sy, SEXP mod, SEXP sUP, SEXP op, SEXP update)
{
int lop = asLogical(op);
mod = PROTECT(duplicate(mod));
SEXP sZ = getListElement(mod, "Z"), sa = getListElement(mod, "a"),
sP = getListElement(mod, "P"), sT = getListElement(mod, "T"),
sV = getListElement(mod, "V"), sh = getListElement(mod, "h"),
sPn = getListElement(mod, "Pn");
if (TYPEOF(sy) != REALSXP || TYPEOF(sZ) != REALSXP ||
TYPEOF(sa) != REALSXP || TYPEOF(sP) != REALSXP ||
TYPEOF(sPn) != REALSXP ||
TYPEOF(sT) != REALSXP || TYPEOF(sV) != REALSXP)
error(_("invalid argument type"));
int n = LENGTH(sy), p = LENGTH(sa);
double *y = REAL(sy), *Z = REAL(sZ), *T = REAL(sT), *V = REAL(sV),
*P = REAL(sP), *a = REAL(sa), *Pnew = REAL(sPn), h = asReal(sh);
double *anew = (double *) R_alloc(p, sizeof(double));
double *M = (double *) R_alloc(p, sizeof(double));
double *mm = (double *) R_alloc(p * p, sizeof(double));
// These are only used if(lop), but avoid -Wall trouble
SEXP ans = R_NilValue, resid = R_NilValue, states = R_NilValue;
if(lop) {
PROTECT(ans = allocVector(VECSXP, 3));
SET_VECTOR_ELT(ans, 1, resid = allocVector(REALSXP, n));
SET_VECTOR_ELT(ans, 2, states = allocMatrix(REALSXP, n, p));
SEXP nm = PROTECT(allocVector(STRSXP, 3));
SET_STRING_ELT(nm, 0, mkChar("values"));
SET_STRING_ELT(nm, 1, mkChar("resid"));
SET_STRING_ELT(nm, 2, mkChar("states"));
setAttrib(ans, R_NamesSymbol, nm);
UNPROTECT(1);
}
double sumlog = 0.0, ssq = 0.0;
int nu = 0;
for (int l = 0; l < n; l++) {
for (int i = 0; i < p; i++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += T[i + p * k] * a[k];
anew[i] = tmp;
}
if (l > asInteger(sUP)) {
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += T[i + p * k] * P[k + p * j];
mm[i + p * j] = tmp;
}
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = V[i + p * j];
for (int k = 0; k < p; k++)
tmp += mm[i + p * k] * T[j + p * k];
Pnew[i + p * j] = tmp;
}
}
if (!ISNAN(y[l])) {
nu++;
double *rr = NULL /* -Wall */;
if(lop) rr = REAL(resid);
double resid0 = y[l];
for (int i = 0; i < p; i++)
resid0 -= Z[i] * anew[i];
double gain = h;
for (int i = 0; i < p; i++) {
double tmp = 0.0;
for (int j = 0; j < p; j++)
tmp += Pnew[i + j * p] * Z[j];
M[i] = tmp;
gain += Z[i] * M[i];
}
ssq += resid0 * resid0 / gain;
if(lop) rr[l] = resid0 / sqrt(gain);
sumlog += log(gain);
for (int i = 0; i < p; i++)
a[i] = anew[i] + M[i] * resid0 / gain;
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++)
P[i + j * p] = Pnew[i + j * p] - M[i] * M[j] / gain;
} else {
double *rr = NULL /* -Wall */;
if(lop) rr = REAL(resid);
for (int i = 0; i < p; i++)
a[i] = anew[i];
for (int i = 0; i < p * p; i++)
P[i] = Pnew[i];
if(lop) rr[l] = NA_REAL;
}
if(lop) {
double *rs = REAL(states);
for (int j = 0; j < p; j++) rs[l + n*j] = a[j];
}
}
SEXP res = PROTECT(allocVector(REALSXP, 2));
REAL(res)[0] = ssq/nu; REAL(res)[1] = sumlog/nu;
if(lop) {
SET_VECTOR_ELT(ans, 0, res);
if(asLogical(update)) setAttrib(ans, install("mod"), mod);
UNPROTECT(3);
return ans;
} else {
if(asLogical(update)) setAttrib(res, install("mod"), mod);
UNPROTECT(2);
return res;
}
}
double qtukey(double p, double rr, double cc, double df,
int lower_tail, int log_p)
{
const double eps = 0.0001;
const int maxiter = 50;
double ans = 0.0, valx0, valx1, x0, x1, xabs;
int iter;
#ifdef IEEE_754
if (ISNAN(p) || ISNAN(rr) || ISNAN(cc) || ISNAN(df)) {
ML_ERROR(ME_DOMAIN);
return p + rr + cc + df;
}
#endif
R_Q_P01_check(p);
if (p == 1) ML_ERR_return_NAN;
/* df must be > 1 */
/* there must be at least two values */
if (df < 2 || rr < 1 || cc < 2) ML_ERR_return_NAN;
if (p == R_DT_0) return 0;
p = R_DT_qIv(p); /* lower_tail,non-log "p" */
/* Initial value */
x0 = qinv(p, cc, df);
/* Find prob(value < x0) */
valx0 = ptukey(x0, rr, cc, df, /*LOWER*/TRUE, /*LOG_P*/FALSE) - p;
/* Find the second iterate and prob(value < x1). */
/* If the first iterate has probability value */
/* exceeding p then second iterate is 1 less than */
/* first iterate; otherwise it is 1 greater. */
if (valx0 > 0.0)
x1 = fmax2(0.0, x0 - 1.0);
else
x1 = x0 + 1.0;
valx1 = ptukey(x1, rr, cc, df, /*LOWER*/TRUE, /*LOG_P*/FALSE) - p;
/* Find new iterate */
for(iter=1 ; iter < maxiter ; iter++) {
ans = x1 - ((valx1 * (x1 - x0)) / (valx1 - valx0));
valx0 = valx1;
/* New iterate must be >= 0 */
x0 = x1;
if (ans < 0.0) {
ans = 0.0;
valx1 = -p;
}
/* Find prob(value < new iterate) */
valx1 = ptukey(ans, rr, cc, df, /*LOWER*/TRUE, /*LOG_P*/FALSE) - p;
x1 = ans;
/* If the difference between two successive */
/* iterates is less than eps, stop */
xabs = fabs(x1 - x0);
if (xabs < eps)
return ans;
}
/* The process did not converge in 'maxiter' iterations */
ML_ERROR(ME_NOCONV);
return ans;
}
/*
* The multiplication algorithm for normal numbers is as follows:
*
* The fraction of the product is built in the usual stepwise fashion.
* Each step consists of shifting the accumulator right one bit
* (maintaining any guard bits) and, if the next bit in y is set,
* adding the multiplicand (x) to the accumulator. Then, in any case,
* we advance one bit leftward in y. Algorithmically:
*
* A = 0;
* for (bit = 0; bit < FP_NMANT; bit++) {
* sticky |= A & 1, A >>= 1;
* if (Y & (1 << bit))
* A += X;
* }
*
* (X and Y here represent the mantissas of x and y respectively.)
* The resultant accumulator (A) is the product's mantissa. It may
* be as large as 11.11111... in binary and hence may need to be
* shifted right, but at most one bit.
*
* Since we do not have efficient multiword arithmetic, we code the
* accumulator as four separate words, just like any other mantissa.
* We use local variables in the hope that this is faster than memory.
* We keep x->fp_mant in locals for the same reason.
*
* In the algorithm above, the bits in y are inspected one at a time.
* We will pick them up 32 at a time and then deal with those 32, one
* at a time. Note, however, that we know several things about y:
*
* - the guard and round bits at the bottom are sure to be zero;
*
* - often many low bits are zero (y is often from a single or double
* precision source);
*
* - bit FP_NMANT-1 is set, and FP_1*2 fits in a word.
*
* We can also test for 32-zero-bits swiftly. In this case, the center
* part of the loop---setting sticky, shifting A, and not adding---will
* run 32 times without adding X to A. We can do a 32-bit shift faster
* by simply moving words. Since zeros are common, we optimize this case.
* Furthermore, since A is initially zero, we can omit the shift as well
* until we reach a nonzero word.
*/
struct fpn *
fpu_mul(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1, *y = &fe->fe_f2;
u_int a3, a2, a1, a0, x3, x2, x1, x0, bit, m;
int sticky;
FPU_DECL_CARRY;
/*
* Put the `heavier' operand on the right (see fpu_emu.h).
* Then we will have one of the following cases, taken in the
* following order:
*
* - y = NaN. Implied: if only one is a signalling NaN, y is.
* The result is y.
* - y = Inf. Implied: x != NaN (is 0, number, or Inf: the NaN
* case was taken care of earlier).
* If x = 0, the result is NaN. Otherwise the result
* is y, with its sign reversed if x is negative.
* - x = 0. Implied: y is 0 or number.
* The result is 0 (with XORed sign as usual).
* - other. Implied: both x and y are numbers.
* The result is x * y (XOR sign, multiply bits, add exponents).
*/
DPRINTF(FPE_REG, ("fpu_mul:\n"));
DUMPFPN(FPE_REG, x);
DUMPFPN(FPE_REG, y);
DPRINTF(FPE_REG, ("=>\n"));
ORDER(x, y);
if (ISNAN(y)) {
y->fp_sign ^= x->fp_sign;
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, y);
return (y);
}
if (ISINF(y)) {
if (ISZERO(x)) {
fe->fe_cx |= FPSCR_VXIMZ;
return (fpu_newnan(fe));
}
y->fp_sign ^= x->fp_sign;
DUMPFPN(FPE_REG, y);
return (y);
}
if (ISZERO(x)) {
x->fp_sign ^= y->fp_sign;
DUMPFPN(FPE_REG, x);
return (x);
}
/*
* Setup. In the code below, the mask `m' will hold the current
* mantissa byte from y. The variable `bit' denotes the bit
* within m. We also define some macros to deal with everything.
*/
x3 = x->fp_mant[3];
x2 = x->fp_mant[2];
x1 = x->fp_mant[1];
x0 = x->fp_mant[0];
sticky = a3 = a2 = a1 = a0 = 0;
#define ADD /* A += X */ \
FPU_ADDS(a3, a3, x3); \
FPU_ADDCS(a2, a2, x2); \
FPU_ADDCS(a1, a1, x1); \
FPU_ADDC(a0, a0, x0)
#define SHR1 /* A >>= 1, with sticky */ \
sticky |= a3 & 1, a3 = (a3 >> 1) | (a2 << 31), \
a2 = (a2 >> 1) | (a1 << 31), a1 = (a1 >> 1) | (a0 << 31), a0 >>= 1
#define SHR32 /* A >>= 32, with sticky */ \
sticky |= a3, a3 = a2, a2 = a1, a1 = a0, a0 = 0
#define STEP /* each 1-bit step of the multiplication */ \
SHR1; if (bit & m) { ADD; }; bit <<= 1
/*
* We are ready to begin. The multiply loop runs once for each
* of the four 32-bit words. Some words, however, are special.
* As noted above, the low order bits of Y are often zero. Even
* if not, the first loop can certainly skip the guard bits.
* The last word of y has its highest 1-bit in position FP_NMANT-1,
* so we stop the loop when we move past that bit.
*/
if ((m = y->fp_mant[3]) == 0) {
/* SHR32; */ /* unneeded since A==0 */
} else {
bit = 1 << FP_NG;
do {
STEP;
} while (bit != 0);
}
if ((m = y->fp_mant[2]) == 0) {
SHR32;
} else {
bit = 1;
do {
STEP;
} while (bit != 0);
}
if ((m = y->fp_mant[1]) == 0) {
SHR32;
} else {
bit = 1;
do {
STEP;
} while (bit != 0);
}
m = y->fp_mant[0]; /* definitely != 0 */
bit = 1;
do {
STEP;
} while (bit <= m);
/*
* Done with mantissa calculation. Get exponent and handle
* 11.111...1 case, then put result in place. We reuse x since
* it already has the right class (FP_NUM).
*/
m = x->fp_exp + y->fp_exp;
if (a0 >= FP_2) {
SHR1;
m++;
}
x->fp_sign ^= y->fp_sign;
x->fp_exp = m;
x->fp_sticky = sticky;
x->fp_mant[3] = a3;
x->fp_mant[2] = a2;
x->fp_mant[1] = a1;
x->fp_mant[0] = a0;
DUMPFPN(FPE_REG, x);
return (x);
}
void Clmbr::set_tol( double tol )
// set accuracy and scale parameters
{
if ( ISNAN(tol) || tol<=0 || tol>=1 ) stop( _("invalid 'tol' value") );
subints = 5; // average number of subintervals per data interval for grid searches
tol_rho = 0.0001; // tolerance for finding 'rho' values by 'bisect' or 'rho_inv'
tol_sl_abs = tol; // maximum absolute error in significance level estimates
tol_sl_rel = min( 10*tol, 0.01 ); // maximum relative error in significance level estimates
int i;
// maximum error in x- boundaries
tol_xb = (xs[ns-1] - xs[0])*tol_sl_rel/64.;
i=1; while( tol_xb < ldexp(1.,-i) ) i++; tol_xb = ldexp(1.,-i);
// maximum error in y- boundaries
double maxY= -Inf, minY= Inf;
for(i=0;i<n;i++) { if( (*py)[i] > maxY ) maxY= (*py)[i]; if( (*py)[i] < minY ) minY= (*py)[i]; }
const double dY= maxY - minY;
tol_yb = dY*tol_sl_rel/64.;
i=1; while( tol_yb < ldexp(1.,-i) ) i++; tol_yb = ldexp(1.,-i);
// precision for integration limits
inc_x = tol_xb;
for( i= max( k1, 0 ); i < ns-2; i++ ) {
double inc= ( xs[i+1] - xs[i] )/subints;
if( inc < inc_x ) inc_x= inc;
}
i=1; while( inc_x < ldexp(1.,-i) ) i++; inc_x = ldexp(1.,-i);
// increment for x- grid searches, and for printout of confidence regions by 'cr'
double inc = ( xs[ns-1] - xs[0] )/(ns-1)/subints;
double inc_seed[3] = { 5., 2., 1. };
double inc10 = 1.;
while ( inc > inc10) inc10 *= 10;
i = 0; while ( inc < inc_seed[i]*inc10 - zero_eq ) { i++; if(i==3) { i=0; inc10 /= 10; } }
inc = inc_seed[i]*inc10;
xinc = inc;
// starting increment for y- grid searches
inc_y = dY/128;
i=1; while( inc_y < ldexp(1.,-i) ) i++; inc_y = ldexp(1.,-i);
// output digits and minimum increment
const int digits= 6;
Rcout << setprecision( digits );
rel_print_eps = pow( 10., -(digits-1) );
// check for trivial case
trivial= false;
if ( variance_unknown && omega/m < zero_eq ) trivial= true;
return;
}
/** Returns the SK-vsafe. */
double
MSCFModel::maximumSafeFollowSpeed(double gap, double egoSpeed, double predSpeed, double predMaxDecel, bool onInsertion) const {
// the speed is safe if allows the ego vehicle to come to a stop behind the leader even if
// the leaders starts braking hard until stopped
// unfortunately it is not sufficient to compare stopping distances if the follower can brake harder than the leader
// (the trajectories might intersect before both vehicles are stopped even if the follower has a shorter stopping distance than the leader)
// To make things safe, we ensure that the leaders brake distance is computed with an deceleration that is at least as high as the follower's.
// @todo: this is a conservative estimate for safe speed which could be increased
// // For negative gaps, we return the lowest meaningful value by convention
// // XXX: check whether this is desireable (changes test results, therefore I exclude it for now (Leo), refs. #2575)
// // It must be done. Otherwise, negative gaps at high speeds can create nonsense results from the call to maximumSafeStopSpeed() below
// if(gap<0){
// if(MSGlobals::gSemiImplicitEulerUpdate){
// return 0.;
// } else {
// return -INVALID_SPEED;
// }
// }
// The following commented code is a variant to assure brief stopping behind a stopped leading vehicle:
// if leader is stopped, calculate stopSpeed without time-headway to prevent creeping stop
// NOTE: this can lead to the strange phenomenon (for the Krauss-model at least) that if the leader comes to a stop,
// the follower accelerates for a short period of time. Refs #2310 (Leo)
// const double headway = predSpeed > 0. ? myHeadwayTime : 0.;
const double headway = myHeadwayTime;
double x = maximumSafeStopSpeed(gap + brakeGap(predSpeed, MAX2(myDecel, predMaxDecel), 0), egoSpeed, onInsertion, headway);
if (myDecel != myEmergencyDecel && !onInsertion && !MSGlobals::gComputeLC) {
double origSafeDecel = SPEED2ACCEL(egoSpeed - x);
if (origSafeDecel > myDecel + NUMERICAL_EPS) {
// Braking harder than myDecel was requested -> calculate required emergency deceleration.
// Note that the resulting safeDecel can be smaller than the origSafeDecel, since the call to maximumSafeStopSpeed() above
// can result in corrupted values (leading to intersecting trajectories) if, e.g. leader and follower are fast (leader still faster) and the gap is very small,
// such that braking harder than myDecel is required.
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << SIMTIME << " initial vsafe=" << x
<< " egoSpeed=" << egoSpeed << " (origSafeDecel=" << origSafeDecel << ")"
<< " predSpeed=" << predSpeed << " (predDecel=" << predMaxDecel << ")"
<< std::endl;
}
#endif
double safeDecel = EMERGENCY_DECEL_AMPLIFIER * calculateEmergencyDeceleration(gap, egoSpeed, predSpeed, predMaxDecel);
// Don't be riskier than the usual method (myDecel <= safeDecel may occur, because a headway>0 is used above)
safeDecel = MAX2(safeDecel, myDecel);
// don't brake harder than originally planned (possible due to euler/ballistic mismatch)
safeDecel = MIN2(safeDecel, origSafeDecel);
x = egoSpeed - ACCEL2SPEED(safeDecel);
if (MSGlobals::gSemiImplicitEulerUpdate) {
x = MAX2(x, 0.);
}
#ifdef DEBUG_EMERGENCYDECEL
if (DEBUG_COND2) {
std::cout << " -> corrected emergency deceleration: " << safeDecel << " newVSafe=" << x << std::endl;
}
#endif
}
}
assert(x >= 0 || !MSGlobals::gSemiImplicitEulerUpdate);
assert(!ISNAN(x));
return x;
}
/*
* Perform a compare instruction (with or without unordered exception).
* This updates the fcc field in the fsr.
*
* If either operand is NaN, the result is unordered. For cmpe, this
* causes an NV exception. Everything else is ordered:
* |Inf| > |numbers| > |0|.
* We already arranged for fp_class(Inf) > fp_class(numbers) > fp_class(0),
* so we get this directly. Note, however, that two zeros compare equal
* regardless of sign, while everything else depends on sign.
*
* Incidentally, two Infs of the same sign compare equal (per the 80387
* manual---it would be nice if the SPARC documentation were more
* complete).
*/
void
__fpu_compare(struct fpemu *fe, int cmpe, int fcc)
{
struct fpn *a, *b;
int cc;
FPU_DECL_CARRY
a = &fe->fe_f1;
b = &fe->fe_f2;
if (ISNAN(a) || ISNAN(b)) {
/*
* In any case, we already got an exception for signalling
* NaNs; here we may replace that one with an identical
* exception, but so what?.
*/
if (cmpe)
fe->fe_cx = FSR_NV;
cc = FSR_CC_UO;
goto done;
}
/*
* Must handle both-zero early to avoid sign goofs. Otherwise,
* at most one is 0, and if the signs differ we are done.
*/
if (ISZERO(a) && ISZERO(b)) {
cc = FSR_CC_EQ;
goto done;
}
if (a->fp_sign) { /* a < 0 (or -0) */
if (!b->fp_sign) { /* b >= 0 (or if a = -0, b > 0) */
cc = FSR_CC_LT;
goto done;
}
} else { /* a > 0 (or +0) */
if (b->fp_sign) { /* b <= -0 (or if a = +0, b < 0) */
cc = FSR_CC_GT;
goto done;
}
}
/*
* Now the signs are the same (but may both be negative). All
* we have left are these cases:
*
* |a| < |b| [classes or values differ]
* |a| > |b| [classes or values differ]
* |a| == |b| [classes and values identical]
*
* We define `diff' here to expand these as:
*
* |a| < |b|, a,b >= 0: a < b => FSR_CC_LT
* |a| < |b|, a,b < 0: a > b => FSR_CC_GT
* |a| > |b|, a,b >= 0: a > b => FSR_CC_GT
* |a| > |b|, a,b < 0: a < b => FSR_CC_LT
*/
#define opposite_cc(cc) ((cc) == FSR_CC_LT ? FSR_CC_GT : FSR_CC_LT)
#define diff(magnitude) (a->fp_sign ? opposite_cc(magnitude) : (magnitude))
if (a->fp_class < b->fp_class) { /* |a| < |b| */
cc = diff(FSR_CC_LT);
goto done;
}
if (a->fp_class > b->fp_class) { /* |a| > |b| */
cc = diff(FSR_CC_GT);
goto done;
}
/* now none can be 0: only Inf and numbers remain */
if (ISINF(a)) { /* |Inf| = |Inf| */
cc = FSR_CC_EQ;
goto done;
}
/*
* Only numbers remain. To compare two numbers in magnitude, we
* simply subtract them.
*/
a = __fpu_sub(fe);
if (a->fp_class == FPC_ZERO)
cc = FSR_CC_EQ;
else
cc = diff(FSR_CC_GT);
done:
fe->fe_fsr = (fe->fe_fsr & fcc_nmask[fcc]) |
((u_long)cc << fcc_shift[fcc]);
}
/*
* Our task is to calculate the square root of a floating point number x0.
* This number x normally has the form:
*
* exp
* x = mant * 2 (where 1 <= mant < 2 and exp is an integer)
*
* This can be left as it stands, or the mantissa can be doubled and the
* exponent decremented:
*
* exp-1
* x = (2 * mant) * 2 (where 2 <= 2 * mant < 4)
*
* If the exponent `exp' is even, the square root of the number is best
* handled using the first form, and is by definition equal to:
*
* exp/2
* sqrt(x) = sqrt(mant) * 2
*
* If exp is odd, on the other hand, it is convenient to use the second
* form, giving:
*
* (exp-1)/2
* sqrt(x) = sqrt(2 * mant) * 2
*
* In the first case, we have
*
* 1 <= mant < 2
*
* and therefore
*
* sqrt(1) <= sqrt(mant) < sqrt(2)
*
* while in the second case we have
*
* 2 <= 2*mant < 4
*
* and therefore
*
* sqrt(2) <= sqrt(2*mant) < sqrt(4)
*
* so that in any case, we are sure that
*
* sqrt(1) <= sqrt(n * mant) < sqrt(4), n = 1 or 2
*
* or
*
* 1 <= sqrt(n * mant) < 2, n = 1 or 2.
*
* This root is therefore a properly formed mantissa for a floating
* point number. The exponent of sqrt(x) is either exp/2 or (exp-1)/2
* as above. This leaves us with the problem of finding the square root
* of a fixed-point number in the range [1..4).
*
* Though it may not be instantly obvious, the following square root
* algorithm works for any integer x of an even number of bits, provided
* that no overflows occur:
*
* let q = 0
* for k = NBITS-1 to 0 step -1 do -- for each digit in the answer...
* x *= 2 -- multiply by radix, for next digit
* if x >= 2q + 2^k then -- if adding 2^k does not
* x -= 2q + 2^k -- exceed the correct root,
* q += 2^k -- add 2^k and adjust x
* fi
* done
* sqrt = q / 2^(NBITS/2) -- (and any remainder is in x)
*
* If NBITS is odd (so that k is initially even), we can just add another
* zero bit at the top of x. Doing so means that q is not going to acquire
* a 1 bit in the first trip around the loop (since x0 < 2^NBITS). If the
* final value in x is not needed, or can be off by a factor of 2, this is
* equivalant to moving the `x *= 2' step to the bottom of the loop:
*
* for k = NBITS-1 to 0 step -1 do if ... fi; x *= 2; done
*
* and the result q will then be sqrt(x0) * 2^floor(NBITS / 2).
* (Since the algorithm is destructive on x, we will call x's initial
* value, for which q is some power of two times its square root, x0.)
*
* If we insert a loop invariant y = 2q, we can then rewrite this using
* C notation as:
*
* q = y = 0; x = x0;
* for (k = NBITS; --k >= 0;) {
* #if (NBITS is even)
* x *= 2;
* #endif
* t = y + (1 << k);
* if (x >= t) {
* x -= t;
* q += 1 << k;
* y += 1 << (k + 1);
* }
* #if (NBITS is odd)
* x *= 2;
* #endif
* }
*
* If x0 is fixed point, rather than an integer, we can simply alter the
* scale factor between q and sqrt(x0). As it happens, we can easily arrange
* for the scale factor to be 2**0 or 1, so that sqrt(x0) == q.
*
* In our case, however, x0 (and therefore x, y, q, and t) are multiword
* integers, which adds some complication. But note that q is built one
* bit at a time, from the top down, and is not used itself in the loop
* (we use 2q as held in y instead). This means we can build our answer
* in an integer, one word at a time, which saves a bit of work. Also,
* since 1 << k is always a `new' bit in q, 1 << k and 1 << (k+1) are
* `new' bits in y and we can set them with an `or' operation rather than
* a full-blown multiword add.
*
* We are almost done, except for one snag. We must prove that none of our
* intermediate calculations can overflow. We know that x0 is in [1..4)
* and therefore the square root in q will be in [1..2), but what about x,
* y, and t?
*
* We know that y = 2q at the beginning of each loop. (The relation only
* fails temporarily while y and q are being updated.) Since q < 2, y < 4.
* The sum in t can, in our case, be as much as y+(1<<1) = y+2 < 6, and.
* Furthermore, we can prove with a bit of work that x never exceeds y by
* more than 2, so that even after doubling, 0 <= x < 8. (This is left as
* an exercise to the reader, mostly because I have become tired of working
* on this comment.)
*
* If our floating point mantissas (which are of the form 1.frac) occupy
* B+1 bits, our largest intermediary needs at most B+3 bits, or two extra.
* In fact, we want even one more bit (for a carry, to avoid compares), or
* three extra. There is a comment in fpu_emu.h reminding maintainers of
* this, so we have some justification in assuming it.
*/
struct fpn *
fpu_sqrt(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1;
u_int bit, q, tt;
u_int x0, x1, x2, x3;
u_int y0, y1, y2, y3;
u_int d0, d1, d2, d3;
int e;
FPU_DECL_CARRY;
/*
* Take care of special cases first. In order:
*
* sqrt(NaN) = NaN
* sqrt(+0) = +0
* sqrt(-0) = -0
* sqrt(x < 0) = NaN (including sqrt(-Inf))
* sqrt(+Inf) = +Inf
*
* Then all that remains are numbers with mantissas in [1..2).
*/
DPRINTF(FPE_REG, ("fpu_sqer:\n"));
DUMPFPN(FPE_REG, x);
DPRINTF(FPE_REG, ("=>\n"));
if (ISNAN(x)) {
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, x);
return (x);
}
if (ISZERO(x)) {
fe->fe_cx |= FPSCR_ZX;
x->fp_class = FPC_INF;
DUMPFPN(FPE_REG, x);
return (x);
}
if (x->fp_sign) {
return (fpu_newnan(fe));
}
if (ISINF(x)) {
fe->fe_cx |= FPSCR_VXSQRT;
DUMPFPN(FPE_REG, 0);
return (0);
}
/*
* Calculate result exponent. As noted above, this may involve
* doubling the mantissa. We will also need to double x each
* time around the loop, so we define a macro for this here, and
* we break out the multiword mantissa.
*/
#ifdef FPU_SHL1_BY_ADD
#define DOUBLE_X { \
FPU_ADDS(x3, x3, x3); FPU_ADDCS(x2, x2, x2); \
FPU_ADDCS(x1, x1, x1); FPU_ADDC(x0, x0, x0); \
}
#else
#define DOUBLE_X { \
x0 = (x0 << 1) | (x1 >> 31); x1 = (x1 << 1) | (x2 >> 31); \
x2 = (x2 << 1) | (x3 >> 31); x3 <<= 1; \
}
#endif
#if (FP_NMANT & 1) != 0
# define ODD_DOUBLE DOUBLE_X
# define EVEN_DOUBLE /* nothing */
#else
# define ODD_DOUBLE /* nothing */
# define EVEN_DOUBLE DOUBLE_X
#endif
x0 = x->fp_mant[0];
x1 = x->fp_mant[1];
x2 = x->fp_mant[2];
x3 = x->fp_mant[3];
e = x->fp_exp;
if (e & 1) /* exponent is odd; use sqrt(2mant) */
DOUBLE_X;
/* THE FOLLOWING ASSUMES THAT RIGHT SHIFT DOES SIGN EXTENSION */
x->fp_exp = e >> 1; /* calculates (e&1 ? (e-1)/2 : e/2 */
/*
* Now calculate the mantissa root. Since x is now in [1..4),
* we know that the first trip around the loop will definitely
* set the top bit in q, so we can do that manually and start
* the loop at the next bit down instead. We must be sure to
* double x correctly while doing the `known q=1.0'.
*
* We do this one mantissa-word at a time, as noted above, to
* save work. To avoid `(1U << 31) << 1', we also do the top bit
* outside of each per-word loop.
*
* The calculation `t = y + bit' breaks down into `t0 = y0, ...,
* t3 = y3, t? |= bit' for the appropriate word. Since the bit
* is always a `new' one, this means that three of the `t?'s are
* just the corresponding `y?'; we use `#define's here for this.
* The variable `tt' holds the actual `t?' variable.
*/
/* calculate q0 */
#define t0 tt
bit = FP_1;
EVEN_DOUBLE;
/* if (x >= (t0 = y0 | bit)) { */ /* always true */
q = bit;
x0 -= bit;
y0 = bit << 1;
/* } */
ODD_DOUBLE;
while ((bit >>= 1) != 0) { /* for remaining bits in q0 */
EVEN_DOUBLE;
t0 = y0 | bit; /* t = y + bit */
if (x0 >= t0) { /* if x >= t then */
x0 -= t0; /* x -= t */
q |= bit; /* q += bit */
y0 |= bit << 1; /* y += bit << 1 */
}
ODD_DOUBLE;
}
x->fp_mant[0] = q;
#undef t0
/* calculate q1. note (y0&1)==0. */
#define t0 y0
#define t1 tt
q = 0;
y1 = 0;
bit = 1 << 31;
EVEN_DOUBLE;
t1 = bit;
FPU_SUBS(d1, x1, t1);
FPU_SUBC(d0, x0, t0); /* d = x - t */
if ((int)d0 >= 0) { /* if d >= 0 (i.e., x >= t) then */
x0 = d0, x1 = d1; /* x -= t */
q = bit; /* q += bit */
y0 |= 1; /* y += bit << 1 */
}
ODD_DOUBLE;
while ((bit >>= 1) != 0) { /* for remaining bits in q1 */
EVEN_DOUBLE; /* as before */
t1 = y1 | bit;
FPU_SUBS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1;
q |= bit;
y1 |= bit << 1;
}
ODD_DOUBLE;
}
x->fp_mant[1] = q;
#undef t1
/* calculate q2. note (y1&1)==0; y0 (aka t0) is fixed. */
#define t1 y1
#define t2 tt
q = 0;
y2 = 0;
bit = 1 << 31;
EVEN_DOUBLE;
t2 = bit;
FPU_SUBS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y1 |= 1; /* now t1, y1 are set in concrete */
}
ODD_DOUBLE;
while ((bit >>= 1) != 0) {
EVEN_DOUBLE;
t2 = y2 | bit;
FPU_SUBS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y2 |= bit << 1;
}
ODD_DOUBLE;
}
x->fp_mant[2] = q;
#undef t2
/* calculate q3. y0, t0, y1, t1 all fixed; y2, t2, almost done. */
#define t2 y2
#define t3 tt
q = 0;
y3 = 0;
bit = 1 << 31;
EVEN_DOUBLE;
t3 = bit;
FPU_SUBS(d3, x3, t3);
FPU_SUBCS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
ODD_DOUBLE;
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y2 |= 1;
}
while ((bit >>= 1) != 0) {
EVEN_DOUBLE;
t3 = y3 | bit;
FPU_SUBS(d3, x3, t3);
FPU_SUBCS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y3 |= bit << 1;
}
ODD_DOUBLE;
}
x->fp_mant[3] = q;
/*
* The result, which includes guard and round bits, is exact iff
* x is now zero; any nonzero bits in x represent sticky bits.
*/
x->fp_sticky = x0 | x1 | x2 | x3;
DUMPFPN(FPE_REG, x);
return (x);
}
double lgammafn_sign(double x, int *sgn)
{
double ans, y, sinpiy;
#ifdef NOMORE_FOR_THREADS
static double xmax = 0.;
static double dxrel = 0.;
if (xmax == 0) {/* initialize machine dependent constants _ONCE_ */
xmax = d1mach(2)/log(d1mach(2));/* = 2.533 e305 for IEEE double */
dxrel = sqrt (d1mach(4));/* sqrt(Eps) ~ 1.49 e-8 for IEEE double */
}
#else
/* For IEEE double precision DBL_EPSILON = 2^-52 = 2.220446049250313e-16 :
xmax = DBL_MAX / log(DBL_MAX) = 2^1024 / (1024 * log(2)) = 2^1014 / log(2)
dxrel = sqrt(DBL_EPSILON) = 2^-26 = 5^26 * 1e-26 (is *exact* below !)
*/
#define xmax 2.5327372760800758e+305
#define dxrel 1.490116119384765625e-8
#endif
if (sgn != NULL) *sgn = 1;
#ifdef IEEE_754
if(ISNAN(x)) return x;
#endif
if (sgn != NULL && x < 0 && fmod(floor(-x), 2.) == 0)
*sgn = -1;
if (x <= 0 && x == trunc(x)) { /* Negative integer argument */
ML_ERROR(ME_RANGE, "lgamma");
return ML_POSINF;/* +Inf, since lgamma(x) = log|gamma(x)| */
}
y = fabs(x);
if (y < 1e-306) return -log(y); // denormalized range, R change
if (y <= 10) return log(fabs(gammafn(x)));
/*
ELSE y = |x| > 10 ---------------------- */
if (y > xmax) {
ML_ERROR(ME_RANGE, "lgamma");
return ML_POSINF;
}
if (x > 0) { /* i.e. y = x > 10 */
#ifdef IEEE_754
if(x > 1e17)
return(x*(log(x) - 1.));
else if(x > 4934720.)
return(M_LN_SQRT_2PI + (x - 0.5) * log(x) - x);
else
#endif
return M_LN_SQRT_2PI + (x - 0.5) * log(x) - x + lgammacor(x);
}
/* else: x < -10; y = -x */
sinpiy = fabs(sin(M_PI * y));
if (sinpiy == 0) { /* Negative integer argument ===
Now UNNECESSARY: caught above */
MATHLIB_WARNING(" ** should NEVER happen! *** [lgamma.c: Neg.int, y=%g]\n",y);
ML_ERR_return_NAN;
}
ans = M_LN_SQRT_PId2 + (x - 0.5) * log(y) - x - log(sinpiy) - lgammacor(y);
if(fabs((x - trunc(x - 0.5)) * ans / x) < dxrel) {
/* The answer is less than half precision because
* the argument is too near a negative integer. */
ML_ERROR(ME_PRECISION, "lgamma");
}
return ans;
}
bool ImplicitList::compute()
{
if (m_bComputed == true)
{
return true;
}
m_iSize = -1;
if (isComputable() == true)
{
m_iSize = 0;
if (m_eOutType == ScilabDouble)
{
m_pDblStart = m_poStart->getAs<Double>();
double dblStart = m_pDblStart->get(0);
m_pDblStep = m_poStep->getAs<Double>();
double dblStep = m_pDblStep->get(0);
m_pDblEnd = m_poEnd->getAs<Double>();
double dblEnd = m_pDblEnd->get(0);
// othe way to compute
// nan value
if (ISNAN(dblStart) || ISNAN(dblStep) || ISNAN(dblEnd))
{
m_iSize = -1;
m_bComputed = true;
return true;
}
// no finite values
if ( finite(dblStart) == 0 || finite(dblStep) == 0 || finite(dblEnd) == 0)
{
if ((dblStep > 0 && dblStart < dblEnd) ||
(dblStep < 0 && dblStart > dblEnd))
{
// return nan
m_iSize = -1;
}
// else return []
m_bComputed = true;
return true;
}
// step null
if (dblStep == 0) // return []
{
m_bComputed = true;
return true;
}
double dblVal = dblStart; // temp value
double dblEps = NumericConstants::eps;
double dblPrec = 2 * std::max(fabs(dblStart), fabs(dblEnd)) * dblEps;
while (dblStep * (dblVal - dblEnd) <= 0)
{
m_iSize++;
dblVal = dblStart + m_iSize * dblStep;
}
if (fabs(dblVal - dblEnd) < dblPrec)
{
m_iSize++;
}
}
else //m_eOutType == ScilabInt
{
if (m_eOutType == ScilabInt8 ||
m_eOutType == ScilabInt16 ||
m_eOutType == ScilabInt32 ||
m_eOutType == ScilabInt64)
{
//signed
long long llStart = convert_input(m_poStart);
long long llStep = convert_input(m_poStep);
long long llEnd = convert_input(m_poEnd);
#ifdef _MSC_VER
m_iSize = static_cast<int>(floor( static_cast<double>(_abs64(llEnd - llStart) / _abs64(llStep)) )) + 1;
#else
m_iSize = static_cast<int>(floor( static_cast<double>(llabs(llEnd - llStart) / llabs(llStep)) )) + 1;
#endif
}
else
{
//unsigned
unsigned long long ullStart = convert_unsigned_input(m_poStart);
unsigned long long ullStep = convert_unsigned_input(m_poStep);
unsigned long long ullEnd = convert_unsigned_input(m_poEnd);
#ifdef _MSC_VER
m_iSize = static_cast<int>(floor(static_cast<double>(_abs64(ullEnd - ullStart) / _abs64(ullStep)) )) + 1;
#else
m_iSize = static_cast<int>(floor(static_cast<double>(llabs(ullEnd - ullStart) / llabs(ullStep)) )) + 1;
#endif
}
}
m_bComputed = true;
return true;
}
else
{
return false;
}
}
double sign(double x)
{
if (ISNAN(x))
return x;
return ((x > 0) ? 1 : ((x == 0)? 0 : -1));
}
/*private*/
int
LineIntersector::computeCollinearIntersection(const Coordinate& p1,const Coordinate& p2,const Coordinate& q1,const Coordinate& q2)
{
#if COMPUTE_Z
double ztot;
int hits;
double p2z;
double p1z;
double q1z;
double q2z;
#endif // COMPUTE_Z
#if GEOS_DEBUG
cerr<<"LineIntersector::computeCollinearIntersection called"<<endl;
cerr<<" p1:"<<p1.toString()<<" p2:"<<p2.toString()<<" q1:"<<q1.toString()<<" q2:"<<q2.toString()<<endl;
#endif // GEOS_DEBUG
bool p1q1p2=Envelope::intersects(p1,p2,q1);
bool p1q2p2=Envelope::intersects(p1,p2,q2);
bool q1p1q2=Envelope::intersects(q1,q2,p1);
bool q1p2q2=Envelope::intersects(q1,q2,p2);
if (p1q1p2 && p1q2p2) {
#if GEOS_DEBUG
cerr<<" p1q1p2 && p1q2p2"<<endl;
#endif
intPt[0]=q1;
#if COMPUTE_Z
ztot=0;
hits=0;
q1z = interpolateZ(q1, p1, p2);
if (!ISNAN(q1z)) { ztot+=q1z; hits++; }
if (!ISNAN(q1.z)) { ztot+=q1.z; hits++; }
if ( hits ) intPt[0].z = ztot/hits;
#endif
intPt[1]=q2;
#if COMPUTE_Z
ztot=0;
hits=0;
q2z = interpolateZ(q2, p1, p2);
if (!ISNAN(q2z)) { ztot+=q2z; hits++; }
if (!ISNAN(q2.z)) { ztot+=q2.z; hits++; }
if ( hits ) intPt[1].z = ztot/hits;
#endif
#if GEOS_DEBUG
cerr<<" intPt[0]: "<<intPt[0].toString()<<endl;
cerr<<" intPt[1]: "<<intPt[1].toString()<<endl;
#endif
return COLLINEAR_INTERSECTION;
}
if (q1p1q2 && q1p2q2) {
#if GEOS_DEBUG
cerr<<" q1p1q2 && q1p2q2"<<endl;
#endif
intPt[0]=p1;
#if COMPUTE_Z
ztot=0;
hits=0;
p1z = interpolateZ(p1, q1, q2);
if (!ISNAN(p1z)) { ztot+=p1z; hits++; }
if (!ISNAN(p1.z)) { ztot+=p1.z; hits++; }
if ( hits ) intPt[0].z = ztot/hits;
#endif
intPt[1]=p2;
#if COMPUTE_Z
ztot=0;
hits=0;
p2z = interpolateZ(p2, q1, q2);
if (!ISNAN(p2z)) { ztot+=p2z; hits++; }
if (!ISNAN(p2.z)) { ztot+=p2.z; hits++; }
if ( hits ) intPt[1].z = ztot/hits;
#endif
return COLLINEAR_INTERSECTION;
}
if (p1q1p2 && q1p1q2) {
#if GEOS_DEBUG
cerr<<" p1q1p2 && q1p1q2"<<endl;
#endif
intPt[0]=q1;
#if COMPUTE_Z
ztot=0;
hits=0;
q1z = interpolateZ(q1, p1, p2);
if (!ISNAN(q1z)) { ztot+=q1z; hits++; }
if (!ISNAN(q1.z)) { ztot+=q1.z; hits++; }
if ( hits ) intPt[0].z = ztot/hits;
#endif
intPt[1]=p1;
#if COMPUTE_Z
ztot=0;
hits=0;
p1z = interpolateZ(p1, q1, q2);
if (!ISNAN(p1z)) { ztot+=p1z; hits++; }
if (!ISNAN(p1.z)) { ztot+=p1.z; hits++; }
if ( hits ) intPt[1].z = ztot/hits;
#endif
#if GEOS_DEBUG
cerr<<" intPt[0]: "<<intPt[0].toString()<<endl;
cerr<<" intPt[1]: "<<intPt[1].toString()<<endl;
#endif
return (q1==p1) && !p1q2p2 && !q1p2q2 ? POINT_INTERSECTION : COLLINEAR_INTERSECTION;
}
if (p1q1p2 && q1p2q2) {
#if GEOS_DEBUG
cerr<<" p1q1p2 && q1p2q2"<<endl;
#endif
intPt[0]=q1;
#if COMPUTE_Z
ztot=0;
hits=0;
q1z = interpolateZ(q1, p1, p2);
if (!ISNAN(q1z)) { ztot+=q1z; hits++; }
if (!ISNAN(q1.z)) { ztot+=q1.z; hits++; }
if ( hits ) intPt[0].z = ztot/hits;
#endif
intPt[1]=p2;
#if COMPUTE_Z
ztot=0;
hits=0;
p2z = interpolateZ(p2, q1, q2);
if (!ISNAN(p2z)) { ztot+=p2z; hits++; }
if (!ISNAN(p2.z)) { ztot+=p2.z; hits++; }
if ( hits ) intPt[1].z = ztot/hits;
#endif
#if GEOS_DEBUG
cerr<<" intPt[0]: "<<intPt[0].toString()<<endl;
cerr<<" intPt[1]: "<<intPt[1].toString()<<endl;
#endif
return (q1==p2) && !p1q2p2 && !q1p1q2 ? POINT_INTERSECTION : COLLINEAR_INTERSECTION;
}
if (p1q2p2 && q1p1q2) {
#if GEOS_DEBUG
cerr<<" p1q2p2 && q1p1q2"<<endl;
#endif
intPt[0]=q2;
#if COMPUTE_Z
ztot=0;
hits=0;
q2z = interpolateZ(q2, p1, p2);
if (!ISNAN(q2z)) { ztot+=q2z; hits++; }
if (!ISNAN(q2.z)) { ztot+=q2.z; hits++; }
if ( hits ) intPt[0].z = ztot/hits;
#endif
intPt[1]=p1;
#if COMPUTE_Z
ztot=0;
hits=0;
p1z = interpolateZ(p1, q1, q2);
if (!ISNAN(p1z)) { ztot+=p1z; hits++; }
if (!ISNAN(p1.z)) { ztot+=p1.z; hits++; }
if ( hits ) intPt[1].z = ztot/hits;
#endif
#if GEOS_DEBUG
cerr<<" intPt[0]: "<<intPt[0].toString()<<endl;
cerr<<" intPt[1]: "<<intPt[1].toString()<<endl;
#endif
return (q2==p1) && !p1q1p2 && !q1p2q2 ? POINT_INTERSECTION : COLLINEAR_INTERSECTION;
}
if (p1q2p2 && q1p2q2) {
#if GEOS_DEBUG
cerr<<" p1q2p2 && q1p2q2"<<endl;
#endif
intPt[0]=q2;
#if COMPUTE_Z
ztot=0;
hits=0;
q2z = interpolateZ(q2, p1, p2);
if (!ISNAN(q2z)) { ztot+=q2z; hits++; }
if (!ISNAN(q2.z)) { ztot+=q2.z; hits++; }
if ( hits ) intPt[0].z = ztot/hits;
#endif
intPt[1]=p2;
#if COMPUTE_Z
ztot=0;
hits=0;
p2z = interpolateZ(p2, q1, q2);
if (!ISNAN(p2z)) { ztot+=p2z; hits++; }
if (!ISNAN(p2.z)) { ztot+=p2.z; hits++; }
if ( hits ) intPt[1].z = ztot/hits;
#endif
#if GEOS_DEBUG
cerr<<" intPt[0]: "<<intPt[0].toString()<<endl;
cerr<<" intPt[1]: "<<intPt[1].toString()<<endl;
#endif
return (q2==p2) && !p1q1p2 && !q1p1q2 ? POINT_INTERSECTION : COLLINEAR_INTERSECTION;
}
return NO_INTERSECTION;
}
apop_data *apop_data_from_frame(SEXP in){
apop_data *out;
if (TYPEOF(in)==NILSXP) return NULL;
PROTECT(in);
assert(TYPEOF(in)==VECSXP); //I should write a check for this on the R side.
int total_cols=LENGTH(in);
int total_rows=LENGTH(VECTOR_ELT(in,0));
int char_cols = 0;
for (int i=0; i< total_cols; i++){
SEXP this_col = VECTOR_ELT(in, i);
char_cols += (TYPEOF(this_col)==STRSXP);
}
SEXP rl, cl;
//const char *rn, *cn;
//GetMatrixDimnames(in, &rl, &cl, &rn, &cn);
PROTECT(cl = getAttrib(in, R_NamesSymbol));
PROTECT(rl = getAttrib(in, R_RowNamesSymbol));
int current_numeric_col=0, current_text_col=0, found_vector=0;
if(cl !=R_NilValue && TYPEOF(cl)==STRSXP) //just check for now.
for (int ndx=0; ndx < LENGTH(cl) && !found_vector; ndx++)
if (!strcmp(translateChar(STRING_ELT(cl, ndx)), "Vector")) found_vector++;
int matrix_cols= total_cols-char_cols-found_vector;
out= apop_data_alloc((found_vector?total_rows:0), (matrix_cols?total_rows:0), matrix_cols);
if (char_cols) out=apop_text_alloc(out, total_rows, char_cols);
if(rl !=R_NilValue)
for (int ndx=0; ndx < LENGTH(rl); ndx++)
if (TYPEOF(rl)==STRSXP)
apop_name_add(out->names, translateChar(STRING_ELT(rl, ndx)), 'r');
else //let us guess that it's a numeric list and hope the R Project one day documents this stuff.
{char *ss; asprintf(&ss, "%i", ndx); apop_name_add(out->names, ss, 'r'); free(ss);}
for (int i=0; i< total_cols; i++){
const char *colname = NULL;
if(cl !=R_NilValue)
colname = translateChar(STRING_ELT(cl, i));
SEXP this_col = VECTOR_ELT(in, i);
if (TYPEOF(this_col) == STRSXP){
//could this be via aliases instead of copying?
//printf("col %i is chars\n", i);
if(colname) apop_name_add(out->names, colname, 't');
for (int j=0; j< total_rows; j++)
apop_text_add(out, j, current_text_col,
(STRING_ELT(this_col,j)==NA_STRING ? apop_opts.nan_string : translateChar(STRING_ELT(this_col, j))));
current_text_col++;
continue;
} else { //plain old matrix data.
int col_in_question = current_numeric_col;
if (colname && !strcmp(colname, "Vector")) {
out->vector = gsl_vector_alloc(total_rows);
col_in_question = -1;
} else {current_numeric_col++;}
Apop_col_v(out, col_in_question, onecol);
if (TYPEOF(this_col) == INTSXP){
//printf("col %i is ints\n", i);
int *vals = INTEGER(this_col);
for (int j=0; j< onecol->size; j++){
//printf("%i\n",vals[j]);
gsl_vector_set(onecol, j, (vals[j]==NA_INTEGER ? GSL_NAN : vals[j]));
}
} else {
double *vals = REAL(this_col);
for (int j=0; j< onecol->size; j++)
gsl_vector_set(onecol, j, (ISNAN(vals[j])||ISNA(vals[j]) ? GSL_NAN : vals[j]));
}
if(colname && col_in_question > -1) apop_name_add(out->names, colname, 'c');
else apop_name_add(out->names, colname, 'v'); //which is "vector".
}
//Factors
SEXP ls = getAttrib(this_col, R_LevelsSymbol);
if (ls){
apop_data *end;//find last page for adding factors.
for(end=out; end->more!=NULL; end=end->more);
end->more = get_factors(ls, colname);
}
}
UNPROTECT(3);
return out;
}
void fadaptiverollmeanExact(double *x, uint_fast64_t nx, double_ans_t *ans, int *k, double fill, bool narm, int hasna, bool verbose) {
if (verbose) Rprintf("%s: running for input length %lu, hasna %d, narm %d\n", __func__, nx, hasna, (int) narm);
volatile bool truehasna = hasna>0; // flag to re-run if NAs detected
if (!truehasna || !narm) { // narm=FALSE handled here as NAs properly propagated in exact algo
const int threads = MIN(getDTthreads(), nx);
#pragma omp parallel num_threads(threads) shared(truehasna)
{
#pragma omp for schedule(static)
for (uint_fast64_t i=0; i<nx; i++) { // loop on every observation to produce final answer
if (narm && truehasna) continue; // if NAs detected no point to continue
if (i+1 < k[i]) ans->ans[i] = fill; // position in a vector smaller than obs window width - partial window
else {
long double w = 0.0;
for (int j=-k[i]+1; j<=0; j++) { // sub-loop on window width
w += x[i+j]; // sum of window for particular observation
}
if (R_FINITE((double) w)) { // no need to calc roundoff correction if NAs detected as will re-call all below in truehasna==1
long double res = w / k[i]; // keep results as long double for intermediate processing
long double err = 0.0; // roundoff corrector
for (int j=-k[i]+1; j<=0; j++) { // sub-loop on window width
err += x[i+j] - res; // measure difference of obs in sub-loop to calculated fun for obs
}
ans->ans[i] = (double) (res + (err / k[i])); // adjust calculated fun with roundoff correction
} else {
if (!narm) ans->ans[i] = (double) (w / k[i]); // NAs should be propagated
truehasna = 1; // NAs detected for this window, set flag so rest of windows will not be re-run
}
}
}
} // end of parallel region
if (truehasna) {
if (hasna==-1) { // raise warning
ans->status = 2;
sprintf(ans->message[2], "%s: hasNA=FALSE used but NA (or other non-finite) value(s) are present in input, use default hasNA=NA to avoid this warning", __func__);
}
if (verbose) {
if (narm) Rprintf("%s: NA (or other non-finite) value(s) are present in input, re-running with extra care for NAs\n", __func__);
else Rprintf("%s: NA (or other non-finite) value(s) are present in input, na.rm was FALSE so in 'exact' implementation NAs were handled already, no need to re-run\n", __func__);
}
}
}
if (truehasna && narm) {
const int threads = MIN(getDTthreads(), nx);
#pragma omp parallel num_threads(threads)
{
#pragma omp for schedule(static)
for (uint_fast64_t i=0; i<nx; i++) { // loop over observations to produce final answer
if (i+1 < k[i]) ans->ans[i] = fill; // partial window
else {
long double w = 0.0; // window to accumulate values in particular window
long double err = 0.0; // accumulate roundoff error
long double res; // keep results as long double for intermediate processing
int nc = 0; // NA counter within current window
for (int j=-k[i]+1; j<=0; j++) { // sub-loop on window width
if (ISNAN(x[i+j])) nc++; // increment NA count in current window
else w += x[i+j]; // add observation to current window
}
if (nc == 0) { // no NAs in current window
res = w / k[i];
for (int j=-k[i]+1; j<=0; j++) { // sub-loop on window width to accumulate roundoff error
err += x[i+j] - res; // measure roundoff for each obs in window
}
ans->ans[i] = (double) (res + (err / k[i])); // adjust calculated fun with roundoff correction
} else if (nc < k[i]) {
res = w / (k[i]-nc);
for (int j=-k[i]+1; j<=0; j++) { // sub-loop on window width to accumulate roundoff error
if (!ISNAN(x[i+j])) err += x[i+j] - res; // measure roundoff for each obs in window
}
ans->ans[i] = (double) (res + (err / (k[i] - nc))); // adjust calculated fun with roundoff correction
} else { // nc == k[i]
ans->ans[i] = R_NaN; // this branch assume narm so R_NaN always here
}
}
}
} // end of parallel region
} // end of truehasna
}
void pnorm_both(double x, double *cum, double *ccum, int i_tail, int log_p)
{
/* i_tail in {0,1,2} means: "lower", "upper", or "both" :
if(lower) return *cum := P[X <= x]
if(upper) return *ccum := P[X > x] = 1 - P[X <= x]
*/
const static double a[5] = {
2.2352520354606839287,
161.02823106855587881,
1067.6894854603709582,
18154.981253343561249,
0.065682337918207449113
};
const static double b[4] = {
47.20258190468824187,
976.09855173777669322,
10260.932208618978205,
45507.789335026729956
};
const static double c[9] = {
0.39894151208813466764,
8.8831497943883759412,
93.506656132177855979,
597.27027639480026226,
2494.5375852903726711,
6848.1904505362823326,
11602.651437647350124,
9842.7148383839780218,
1.0765576773720192317e-8
};
const static double d[8] = {
22.266688044328115691,
235.38790178262499861,
1519.377599407554805,
6485.558298266760755,
18615.571640885098091,
34900.952721145977266,
38912.003286093271411,
19685.429676859990727
};
const static double p[6] = {
0.21589853405795699,
0.1274011611602473639,
0.022235277870649807,
0.001421619193227893466,
2.9112874951168792e-5,
0.02307344176494017303
};
const static double q[5] = {
1.28426009614491121,
0.468238212480865118,
0.0659881378689285515,
0.00378239633202758244,
7.29751555083966205e-5
};
double xden, xnum, temp, del, eps, xsq, y;
#ifdef NO_DENORMS
double min = DBL_MIN;
#endif
int i, lower, upper;
#ifdef IEEE_754
if(ISNAN(x)) { *cum = *ccum = x; return; }
#endif
/* Consider changing these : */
eps = DBL_EPSILON * 0.5;
/* i_tail in {0,1,2} =^= {lower, upper, both} */
lower = i_tail != 1;
upper = i_tail != 0;
y = fabs(x);
if (y <= 0.67448975) { /* qnorm(3/4) = .6744.... -- earlier had 0.66291 */
if (y > eps) {
xsq = x * x;
xnum = a[4] * xsq;
xden = xsq;
for (i = 0; i < 3; ++i) {
xnum = (xnum + a[i]) * xsq;
xden = (xden + b[i]) * xsq;
}
} else xnum = xden = 0.0;
temp = x * (xnum + a[3]) / (xden + b[3]);
if(lower) *cum = 0.5 + temp;
if(upper) *ccum = 0.5 - temp;
if(log_p) {
if(lower) *cum = log(*cum);
if(upper) *ccum = log(*ccum);
}
}
else if (y <= M_SQRT_32) {
/* Evaluate pnorm for 0.674.. = qnorm(3/4) < |x| <= sqrt(32) ~= 5.657 */
xnum = c[8] * y;
xden = y;
for (i = 0; i < 7; ++i) {
xnum = (xnum + c[i]) * y;
xden = (xden + d[i]) * y;
}
temp = (xnum + c[7]) / (xden + d[7]);
#define do_del(X) \
xsq = trunc(X * SIXTEN) / SIXTEN; \
del = (X - xsq) * (X + xsq); \
if(log_p) { \
*cum = (-xsq * xsq * 0.5) + (-del * 0.5) + log(temp); \
if((lower && x > 0.) || (upper && x <= 0.)) \
*ccum = log1p(-exp(-xsq * xsq * 0.5) * \
exp(-del * 0.5) * temp); \
} \
else { \
*cum = exp(-xsq * xsq * 0.5) * exp(-del * 0.5) * temp; \
*ccum = 1.0 - *cum; \
}
#define swap_tail \
if (x > 0.) {/* swap ccum <--> cum */ \
temp = *cum; if(lower) *cum = *ccum; *ccum = temp; \
}
do_del(y);
swap_tail;
}
/* else |x| > sqrt(32) = 5.657 :
* the next two case differentiations were really for lower=T, log=F
* Particularly *not* for log_p !
* Cody had (-37.5193 < x && x < 8.2924) ; R originally had y < 50
*
* Note that we do want symmetry(0), lower/upper -> hence use y
*/
else if((log_p && y < 1e170) /* avoid underflow below */
/* ^^^^^ MM FIXME: can speedup for log_p and much larger |x| !
* Then, make use of Abramowitz & Stegun, 26.2.13, something like
xsq = x*x;
if(xsq * DBL_EPSILON < 1.)
del = (1. - (1. - 5./(xsq+6.)) / (xsq+4.)) / (xsq+2.);
else
del = 0.;
*cum = -.5*xsq - M_LN_SQRT_2PI - log(x) + log1p(-del);
*ccum = log1p(-exp(*cum)); /.* ~ log(1) = 0 *./
swap_tail;
[Yes, but xsq might be infinite.]
*/
|| (lower && -37.5193 < x && x < 8.2924)
|| (upper && -8.2924 < x && x < 37.5193)
) {
/* Evaluate pnorm for x in (-37.5, -5.657) union (5.657, 37.5) */
xsq = 1.0 / (x * x); /* (1./x)*(1./x) might be better */
xnum = p[5] * xsq;
xden = xsq;
for (i = 0; i < 4; ++i) {
xnum = (xnum + p[i]) * xsq;
xden = (xden + q[i]) * xsq;
}
temp = xsq * (xnum + p[4]) / (xden + q[4]);
temp = (M_1_SQRT_2PI - temp) / y;
do_del(x);
swap_tail;
} else { /* large x such that probs are 0 or 1 */
if(x > 0) { *cum = R_D__1; *ccum = R_D__0; }
else { *cum = R_D__0; *ccum = R_D__1; }
}
#ifdef NO_DENORMS
/* do not return "denormalized" -- we do in R */
if(log_p) {
if(*cum > -min) *cum = -0.;
if(*ccum > -min)*ccum = -0.;
}
else {
if(*cum < min) *cum = 0.;
if(*ccum < min) *ccum = 0.;
}
#endif
return;
}
struct fpn *
fpu_div(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1, *y = &fe->fe_f2;
u_int q, bit;
u_int r0, r1, r2, r3, d0, d1, d2, d3, y0, y1, y2, y3;
FPU_DECL_CARRY
/*
* Since divide is not commutative, we cannot just use ORDER.
* Check either operand for NaN first; if there is at least one,
* order the signalling one (if only one) onto the right, then
* return it. Otherwise we have the following cases:
*
* Inf / Inf = NaN, plus NV exception
* Inf / num = Inf [i.e., return x]
* Inf / 0 = Inf [i.e., return x]
* 0 / Inf = 0 [i.e., return x]
* 0 / num = 0 [i.e., return x]
* 0 / 0 = NaN, plus NV exception
* num / Inf = 0
* num / num = num (do the divide)
* num / 0 = Inf, plus DZ exception
*/
DPRINTF(FPE_REG, ("fpu_div:\n"));
DUMPFPN(FPE_REG, x);
DUMPFPN(FPE_REG, y);
DPRINTF(FPE_REG, ("=>\n"));
if (ISNAN(x) || ISNAN(y)) {
ORDER(x, y);
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, y);
return (y);
}
/*
* Need to split the following out cause they generate different
* exceptions.
*/
if (ISINF(x)) {
if (x->fp_class == y->fp_class) {
fe->fe_cx |= FPSCR_VXIDI;
return (fpu_newnan(fe));
}
DUMPFPN(FPE_REG, x);
return (x);
}
if (ISZERO(x)) {
fe->fe_cx |= FPSCR_ZX;
if (x->fp_class == y->fp_class) {
fe->fe_cx |= FPSCR_VXZDZ;
return (fpu_newnan(fe));
}
DUMPFPN(FPE_REG, x);
return (x);
}
/* all results at this point use XOR of operand signs */
x->fp_sign ^= y->fp_sign;
if (ISINF(y)) {
x->fp_class = FPC_ZERO;
DUMPFPN(FPE_REG, x);
return (x);
}
if (ISZERO(y)) {
fe->fe_cx = FPSCR_ZX;
x->fp_class = FPC_INF;
DUMPFPN(FPE_REG, x);
return (x);
}
/*
* Macros for the divide. See comments at top for algorithm.
* Note that we expand R, D, and Y here.
*/
#define SUBTRACT /* D = R - Y */ \
FPU_SUBS(d3, r3, y3); FPU_SUBCS(d2, r2, y2); \
FPU_SUBCS(d1, r1, y1); FPU_SUBC(d0, r0, y0)
#define NONNEGATIVE /* D >= 0 */ \
((int)d0 >= 0)
#ifdef FPU_SHL1_BY_ADD
#define SHL1 /* R <<= 1 */ \
FPU_ADDS(r3, r3, r3); FPU_ADDCS(r2, r2, r2); \
FPU_ADDCS(r1, r1, r1); FPU_ADDC(r0, r0, r0)
#else
#define SHL1 \
r0 = (r0 << 1) | (r1 >> 31), r1 = (r1 << 1) | (r2 >> 31), \
r2 = (r2 << 1) | (r3 >> 31), r3 <<= 1
#endif
#define LOOP /* do ... while (bit >>= 1) */ \
do { \
SHL1; \
SUBTRACT; \
if (NONNEGATIVE) { \
q |= bit; \
r0 = d0, r1 = d1, r2 = d2, r3 = d3; \
} \
} while ((bit >>= 1) != 0)
#define WORD(r, i) /* calculate r->fp_mant[i] */ \
q = 0; \
bit = 1 << 31; \
LOOP; \
(x)->fp_mant[i] = q
/* Setup. Note that we put our result in x. */
r0 = x->fp_mant[0];
r1 = x->fp_mant[1];
r2 = x->fp_mant[2];
r3 = x->fp_mant[3];
y0 = y->fp_mant[0];
y1 = y->fp_mant[1];
y2 = y->fp_mant[2];
y3 = y->fp_mant[3];
bit = FP_1;
SUBTRACT;
if (NONNEGATIVE) {
x->fp_exp -= y->fp_exp;
r0 = d0, r1 = d1, r2 = d2, r3 = d3;
q = bit;
bit >>= 1;
} else {
void
EditableDenseThreeDimensionalModel::setColumn(size_t index,
const Column &values)
{
QWriteLocker locker(&m_lock);
while (index >= m_data.size()) {
m_data.push_back(Column());
m_trunc.push_back(0);
}
bool allChange = false;
// if (values.size() > m_yBinCount) m_yBinCount = values.size();
for (size_t i = 0; i < values.size(); ++i) {
float value = values[i];
if (ISNAN(value) || ISINF(value)) {
continue;
}
if (!m_haveExtents || value < m_minimum) {
m_minimum = value;
allChange = true;
}
if (!m_haveExtents || value > m_maximum) {
m_maximum = value;
allChange = true;
}
m_haveExtents = true;
}
truncateAndStore(index, values);
// assert(values == expandAndRetrieve(index));
long windowStart = index;
windowStart *= m_resolution;
if (m_notifyOnAdd) {
if (allChange) {
emit modelChanged();
} else {
emit modelChanged(windowStart, windowStart + m_resolution);
}
} else {
if (allChange) {
m_sinceLastNotifyMin = -1;
m_sinceLastNotifyMax = -1;
emit modelChanged();
} else {
if (m_sinceLastNotifyMin == -1 ||
windowStart < m_sinceLastNotifyMin) {
m_sinceLastNotifyMin = windowStart;
}
if (m_sinceLastNotifyMax == -1 ||
windowStart > m_sinceLastNotifyMax) {
m_sinceLastNotifyMax = windowStart;
}
}
}
}
double qnchisq(double p, double df, double ncp, int lower_tail, int log_p)
{
const static double accu = 1e-13;
const static double racc = 4*DBL_EPSILON;
/* these two are for the "search" loops, can have less accuracy: */
const static double Eps = 1e-11; /* must be > accu */
const static double rEps= 1e-10; /* relative tolerance ... */
double ux, lx, ux0, nx, pp;
#ifdef IEEE_754
if (ISNAN(p) || ISNAN(df) || ISNAN(ncp))
return p + df + ncp;
#endif
if (!R_FINITE(df)) ML_ERR_return_NAN;
/* Was
* df = floor(df + 0.5);
* if (df < 1 || ncp < 0) ML_ERR_return_NAN;
*/
if (df < 0 || ncp < 0) ML_ERR_return_NAN;
R_Q_P01_boundaries(p, 0, ML_POSINF);
pp = R_D_qIv(p);
if(pp > 1 - DBL_EPSILON) return lower_tail ? ML_POSINF : 0.0;
/* Invert pnchisq(.) :
* 1. finding an upper and lower bound */
{
/* This is Pearson's (1959) approximation,
which is usually good to 4 figs or so. */
double b, c, ff;
b = (ncp*ncp)/(df + 3*ncp);
c = (df + 3*ncp)/(df + 2*ncp);
ff = (df + 2 * ncp)/(c*c);
ux = b + c * qchisq(p, ff, lower_tail, log_p);
if(ux < 0) ux = 1;
ux0 = ux;
}
if(!lower_tail && ncp >= 80) {
/* in this case, pnchisq() works via lower_tail = TRUE */
if(pp < 1e-10) ML_ERROR(ME_PRECISION, "qnchisq");
p = /* R_DT_qIv(p)*/ log_p ? -expm1(p) : (0.5 - (p) + 0.5);
lower_tail = TRUE;
} else {
p = pp;
}
pp = fmin2(1 - DBL_EPSILON, p * (1 + Eps));
if(lower_tail) {
for(; ux < DBL_MAX &&
pnchisq_raw(ux, df, ncp, Eps, rEps, 10000, TRUE, FALSE) < pp;
ux *= 2);
pp = p * (1 - Eps);
for(lx = fmin2(ux0, DBL_MAX);
lx > DBL_MIN &&
pnchisq_raw(lx, df, ncp, Eps, rEps, 10000, TRUE, FALSE) > pp;
lx *= 0.5);
}
else {
for(; ux < DBL_MAX &&
pnchisq_raw(ux, df, ncp, Eps, rEps, 10000, FALSE, FALSE) > pp;
ux *= 2);
pp = p * (1 - Eps);
for(lx = fmin2(ux0, DBL_MAX);
lx > DBL_MIN &&
pnchisq_raw(lx, df, ncp, Eps, rEps, 10000, FALSE, FALSE) < pp;
lx *= 0.5);
}
/* 2. interval (lx,ux) halving : */
if(lower_tail) {
do {
nx = 0.5 * (lx + ux);
if (pnchisq_raw(nx, df, ncp, accu, racc, 100000, TRUE, FALSE) > p)
ux = nx;
else
lx = nx;
}
while ((ux - lx) / nx > accu);
} else {
do {
nx = 0.5 * (lx + ux);
if (pnchisq_raw(nx, df, ncp, accu, racc, 100000, FALSE, FALSE) < p)
ux = nx;
else
lx = nx;
}
while ((ux - lx) / nx > accu);
}
return 0.5 * (ux + lx);
}
SEXP
KalmanSmooth(SEXP sy, SEXP mod, SEXP sUP)
{
SEXP sZ = getListElement(mod, "Z"), sa = getListElement(mod, "a"),
sP = getListElement(mod, "P"), sT = getListElement(mod, "T"),
sV = getListElement(mod, "V"), sh = getListElement(mod, "h"),
sPn = getListElement(mod, "Pn");
if (TYPEOF(sy) != REALSXP || TYPEOF(sZ) != REALSXP ||
TYPEOF(sa) != REALSXP || TYPEOF(sP) != REALSXP ||
TYPEOF(sT) != REALSXP || TYPEOF(sV) != REALSXP)
error(_("invalid argument type"));
SEXP ssa, ssP, ssPn, res, states = R_NilValue, sN;
int n = LENGTH(sy), p = LENGTH(sa);
double *y = REAL(sy), *Z = REAL(sZ), *a, *P,
*T = REAL(sT), *V = REAL(sV), h = asReal(sh), *Pnew;
double *at, *rt, *Pt, *gains, *resids, *Mt, *L, gn, *Nt;
Rboolean var = TRUE;
PROTECT(ssa = duplicate(sa)); a = REAL(ssa);
PROTECT(ssP = duplicate(sP)); P = REAL(ssP);
PROTECT(ssPn = duplicate(sPn)); Pnew = REAL(ssPn);
PROTECT(res = allocVector(VECSXP, 2));
SEXP nm = PROTECT(allocVector(STRSXP, 2));
SET_STRING_ELT(nm, 0, mkChar("smooth"));
SET_STRING_ELT(nm, 1, mkChar("var"));
setAttrib(res, R_NamesSymbol, nm);
UNPROTECT(1);
SET_VECTOR_ELT(res, 0, states = allocMatrix(REALSXP, n, p));
at = REAL(states);
SET_VECTOR_ELT(res, 1, sN = allocVector(REALSXP, n*p*p));
Nt = REAL(sN);
double *anew, *mm, *M;
anew = (double *) R_alloc(p, sizeof(double));
M = (double *) R_alloc(p, sizeof(double));
mm = (double *) R_alloc(p * p, sizeof(double));
Pt = (double *) R_alloc(n * p * p, sizeof(double));
gains = (double *) R_alloc(n, sizeof(double));
resids = (double *) R_alloc(n, sizeof(double));
Mt = (double *) R_alloc(n * p, sizeof(double));
L = (double *) R_alloc(p * p, sizeof(double));
for (int l = 0; l < n; l++) {
for (int i = 0; i < p; i++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += T[i + p * k] * a[k];
anew[i] = tmp;
}
if (l > asInteger(sUP)) {
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += T[i + p * k] * P[k + p * j];
mm[i + p * j] = tmp;
}
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = V[i + p * j];
for (int k = 0; k < p; k++)
tmp += mm[i + p * k] * T[j + p * k];
Pnew[i + p * j] = tmp;
}
}
for (int i = 0; i < p; i++) at[l + n*i] = anew[i];
for (int i = 0; i < p*p; i++) Pt[l + n*i] = Pnew[i];
if (!ISNAN(y[l])) {
double resid0 = y[l];
for (int i = 0; i < p; i++)
resid0 -= Z[i] * anew[i];
double gain = h;
for (int i = 0; i < p; i++) {
double tmp = 0.0;
for (int j = 0; j < p; j++)
tmp += Pnew[i + j * p] * Z[j];
Mt[l + n*i] = M[i] = tmp;
gain += Z[i] * M[i];
}
gains[l] = gain;
resids[l] = resid0;
for (int i = 0; i < p; i++)
a[i] = anew[i] + M[i] * resid0 / gain;
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++)
P[i + j * p] = Pnew[i + j * p] - M[i] * M[j] / gain;
} else {
for (int i = 0; i < p; i++) {
a[i] = anew[i];
Mt[l + n * i] = 0.0;
}
for (int i = 0; i < p * p; i++)
P[i] = Pnew[i];
gains[l] = NA_REAL;
resids[l] = NA_REAL;
}
}
/* rt stores r_{t-1} */
rt = (double *) R_alloc(n * p, sizeof(double));
for (int l = n - 1; l >= 0; l--) {
if (!ISNAN(gains[l])) {
gn = 1/gains[l];
for (int i = 0; i < p; i++)
rt[l + n * i] = Z[i] * resids[l] * gn;
} else {
for (int i = 0; i < p; i++) rt[l + n * i] = 0.0;
gn = 0.0;
}
if (var) {
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++)
Nt[l + n*i + n*p*j] = Z[i] * Z[j] * gn;
}
if (l < n - 1) {
/* compute r_{t-1} */
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++)
mm[i + p * j] = ((i==j) ? 1:0) - Mt[l + n * i] * Z[j] * gn;
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += T[i + p * k] * mm[k + p * j];
L[i + p * j] = tmp;
}
for (int i = 0; i < p; i++) {
double tmp = 0.0;
for (int j = 0; j < p; j++)
tmp += L[j + p * i] * rt[l + 1 + n * j];
rt[l + n * i] += tmp;
}
if(var) { /* compute N_{t-1} */
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += L[k + p * i] * Nt[l + 1 + n*k + n*p*j];
mm[i + p * j] = tmp;
}
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += mm[i + p * k] * L[k + p * j];
Nt[l + n*i + n*p*j] += tmp;
}
}
}
for (int i = 0; i < p; i++) {
double tmp = 0.0;
for (int j = 0; j < p; j++)
tmp += Pt[l + n*i + n*p*j] * rt[l + n * j];
at[l + n*i] += tmp;
}
}
if (var)
for (int l = 0; l < n; l++) {
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = 0.0;
for (int k = 0; k < p; k++)
tmp += Pt[l + n*i + n*p*k] * Nt[l + n*k + n*p*j];
mm[i + p * j] = tmp;
}
for (int i = 0; i < p; i++)
for (int j = 0; j < p; j++) {
double tmp = Pt[l + n*i + n*p*j];
for (int k = 0; k < p; k++)
tmp -= mm[i + p * k] * Pt[l + n*k + n*p*j];
Nt[l + n*i + n*p*j] = tmp;
}
}
UNPROTECT(4);
return res;
}
SEXP na_locf (SEXP x, SEXP fromLast, SEXP _maxgap, SEXP _limit)
{
/* only works on univariate data *
* of type LGLSXP, INTSXP and REALSXP. */
SEXP result;
int i, ii, nr, _first, P=0;
double gap, maxgap, limit;
_first = firstNonNA(x);
if(_first == nrows(x))
return(x);
int *int_x=NULL, *int_result=NULL;
double *real_x=NULL, *real_result=NULL;
if(ncols(x) > 1)
error("na.locf.xts only handles univariate, dimensioned data");
nr = nrows(x);
maxgap = asReal(coerceVector(_maxgap,REALSXP));
limit = asReal(coerceVector(_limit ,REALSXP));
gap = 0;
PROTECT(result = allocVector(TYPEOF(x), nrows(x))); P++;
switch(TYPEOF(x)) {
case LGLSXP:
int_x = LOGICAL(x);
int_result = LOGICAL(result);
if(!LOGICAL(fromLast)[0]) {
/* copy leading NAs */
for(i=0; i < (_first+1); i++) {
int_result[i] = int_x[i];
}
/* result[_first] now has first value fromLast=FALSE */
for(i=_first+1; i<nr; i++) {
int_result[i] = int_x[i];
if(int_result[i] == NA_LOGICAL && gap < maxgap) {
int_result[i] = int_result[i-1];
gap++;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have excessive trailing gap */
for(ii = i-1; ii > i-gap-1; ii--) {
int_result[ii] = NA_LOGICAL;
}
}
} else {
/* nr-2 is first position to fill fromLast=TRUE */
int_result[nr-1] = int_x[nr-1];
for(i=nr-2; i>=0; i--) {
int_result[i] = int_x[i];
if(int_result[i] == NA_LOGICAL && gap < maxgap) {
int_result[i] = int_result[i+1];
gap++;
}
}
}
break;
case INTSXP:
int_x = INTEGER(x);
int_result = INTEGER(result);
if(!LOGICAL(fromLast)[0]) {
/* copy leading NAs */
for(i=0; i < (_first+1); i++) {
int_result[i] = int_x[i];
}
/* result[_first] now has first value fromLast=FALSE */
for(i=_first+1; i<nr; i++) {
int_result[i] = int_x[i];
if(int_result[i] == NA_INTEGER) {
if(limit > gap)
int_result[i] = int_result[i-1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i-1; ii > i-gap-1; ii--) {
int_result[ii] = NA_INTEGER;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have excessive trailing gap */
for(ii = i-1; ii > i-gap-1; ii--) {
int_result[ii] = NA_INTEGER;
}
}
} else {
/* nr-2 is first position to fill fromLast=TRUE */
int_result[nr-1] = int_x[nr-1];
for(i=nr-2; i>=0; i--) {
int_result[i] = int_x[i];
if(int_result[i] == NA_INTEGER) {
if(limit > gap)
int_result[i] = int_result[i+1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i+1; ii < i+gap+1; ii++) {
int_result[ii] = NA_INTEGER;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have leading trailing gap */
for(ii = i+1; ii < i+gap+1; ii++) {
int_result[ii] = NA_INTEGER;
}
}
}
break;
case REALSXP:
real_x = REAL(x);
real_result = REAL(result);
if(!LOGICAL(fromLast)[0]) { /* fromLast=FALSE */
for(i=0; i < (_first+1); i++) {
real_result[i] = real_x[i];
}
for(i=_first+1; i<nr; i++) {
real_result[i] = real_x[i];
if( ISNA(real_result[i]) || ISNAN(real_result[i])) {
if(limit > gap)
real_result[i] = real_result[i-1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i-1; ii > i-gap-1; ii--) {
real_result[ii] = NA_REAL;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have excessive trailing gap */
for(ii = i-1; ii > i-gap-1; ii--) {
real_result[ii] = NA_REAL;
}
}
} else { /* fromLast=TRUE */
real_result[nr-1] = real_x[nr-1];
for(i=nr-2; i>=0; i--) {
real_result[i] = real_x[i];
if(ISNA(real_result[i]) || ISNAN(real_result[i])) {
if(limit > gap)
real_result[i] = real_result[i+1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i+1; ii < i+gap+1; ii++) {
real_result[ii] = NA_REAL;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have leading trailing gap */
for(ii = i+1; ii < i+gap+1; ii++) {
real_result[ii] = NA_REAL;
}
}
}
break;
default:
error("unsupported type");
break;
}
if(isXts(x)) {
setAttrib(result, R_DimSymbol, getAttrib(x, R_DimSymbol));
setAttrib(result, R_DimNamesSymbol, getAttrib(x, R_DimNamesSymbol));
setAttrib(result, xts_IndexSymbol, getAttrib(x, xts_IndexSymbol));
copy_xtsCoreAttributes(x, result);
copy_xtsAttributes(x, result);
}
UNPROTECT(P);
return(result);
}
void transitivity_R(double *mat, int *n, int *m, double *t, int *meas, int *checkna)
/*
Compute transitivity information for the (edgelist) network in mat. This is
stored in t, with t[0] being the number of ordered triads at risk for
transitivity, and t[1] being the number satisfying the condition. The
definition used is controlled by meas, with meas==1 implying the weak condition
(a->b->c => a->c), meas==0 implying the strong condition (a->b->c <=>a->c),
meas==2 implying the rank condition (a->c >= min(a->b,b->c)), and meas==3
implying Dekker's correlation measure (cor(a->c,a->b*b->c)).
If checkna==0, the measures are computed without missingness checks (i.e.,
treating NA edges as present). If checkna==1, any triad containing missing
edges is omitted from the total count. Finally, if checkna==2, missing edges
are treated as absent by the routine.
This routine may be called from R using .C.
*/
{
int i,j,k,sij,sjk,sik;
double ev;
snaNet *g;
slelement *jp,*kp,*ikp;
/*Form the snaNet and initialize t*/
GetRNGstate();
//Rprintf("Building network, %d vertices and %d edges\n",*n,*m);
g=elMatTosnaNet(mat,n,m);
//Rprintf("Build complete. Proceeding.\n");
PutRNGstate();
t[0]=t[1]=0.0;
/*Get the transitivity information*/
switch(*meas){
case 0: /*"Strong" form: i->j->k <=> i->k*/
for(i=0;i<g->n;i++)
for(j=0;j<g->n;j++)
if(i!=j){
for(k=0;k<g->n;k++)
if((j!=k)&&(i!=k)){
sij=snaIsAdjacent(i,j,g,*checkna);
sjk=snaIsAdjacent(j,k,g,*checkna);
sik=snaIsAdjacent(i,k,g,*checkna);
if(!(IISNA(sij)||IISNA(sjk)||IISNA(sik))){
t[0]+=sij*sjk*sik+(1-sij*sjk)*(1-sik);
t[1]++;
}
}
}
break;
case 1: /*"Weak" form: i->j->k => i->k*/
for(i=0;i<g->n;i++){
for(jp=snaFirstEdge(g,i,1);jp!=NULL;jp=jp->next[0]){
if((i!=(int)(jp->val))&&((*checkna==0)||(!ISNAN(*((double *)(jp->dp)))))){ /*Case 1 acts like case 2 here*/
for(kp=snaFirstEdge(g,(int)(jp->val),1);kp!=NULL;kp=kp->next[0]){
if(((int)(jp->val)!=(int)(kp->val))&&(i!=(int)(kp->val))&& ((*checkna==0)||(!ISNAN(*((double *)(kp->dp)))))){
sik=snaIsAdjacent(i,(int)(kp->val),g,*checkna);
if(!IISNA(sik)){ /*Not counting in case 1 (but am in case 2)*/
t[0]+=sik;
t[1]++;
}
}
}
}
}
}
break;
case 2: /*"Rank" form: i->k >= min(i->j,j->k)*/
for(i=0;i<g->n;i++){
for(jp=snaFirstEdge(g,i,1);jp!=NULL;jp=jp->next[0]){
if((i!=(int)(jp->val))&&((*checkna==0)||(!ISNAN(*((double *)(jp->dp)))))){ /*Case 1 acts like case 2 here*/
for(kp=snaFirstEdge(g,(int)(jp->val),1);kp!=NULL;kp=kp->next[0]){
if(((int)(jp->val)!=(int)(kp->val))&&(i!=(int)(kp->val))&& ((*checkna==0)||(!ISNAN(*((double *)(kp->dp)))))){
sik=snaIsAdjacent(i,(int)(kp->val),g,*checkna);
if(!IISNA(sik)){ /*Not counting in case 1 (but am in case 2)*/
if(sik){
ikp=slistSearch(g->oel[i],kp->val); /*We already verified that it is here*/
ev=*((double *)(ikp->dp));
}else{
ev=0.0;
}
if((*checkna==0)||(!ISNAN(ev))){
t[0]+=(ev>=MIN(*((double *)(kp->dp)),*((double *)(jp->dp))));
t[1]++;
}
}
}
}
}
}
}
break;
case 3: /*"Corr" form: corr(i->k, i->j * j->k)*/
error("Edgelist computation not currently supported for correlation measure in gtrans.\n");
break;
}
}
SEXP na_locf_col (SEXP x, SEXP fromLast, SEXP _maxgap, SEXP _limit)
{
/* version of na_locf that works on multivariate data
* of type LGLSXP, INTSXP and REALSXP. */
SEXP result;
int i, ii, j, nr, nc, _first=0, P=0;
double gap, maxgap, limit;
int *int_x=NULL, *int_result=NULL;
double *real_x=NULL, *real_result=NULL;
nr = nrows(x);
nc = ncols(x);
maxgap = asReal(_maxgap);
limit = asReal(_limit);
gap = 0;
if(firstNonNA(x) == nr)
return(x);
PROTECT(result = allocMatrix(TYPEOF(x), nr, nc)); P++;
switch(TYPEOF(x)) {
case LGLSXP:
int_x = LOGICAL(x);
int_result = LOGICAL(result);
if(!LOGICAL(fromLast)[0]) {
for(j=0; j < nc; j++) {
/* copy leading NAs */
_first = firstNonNACol(x, j);
//if(_first+1 == nr) continue;
for(i=0+j*nr; i < (_first+1); i++) {
int_result[i] = int_x[i];
}
/* result[_first] now has first value fromLast=FALSE */
for(i=_first+1; i<nr+j*nr; i++) {
int_result[i] = int_x[i];
if(int_result[i] == NA_LOGICAL && gap < maxgap) {
int_result[i] = int_result[i-1];
gap++;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have excessive trailing gap */
for(ii = i-1; ii > i-gap-1; ii--) {
int_result[ii] = NA_LOGICAL;
}
}
}
} else {
/* nr-2 is first position to fill fromLast=TRUE */
for(j=0; j < nc; j++) {
int_result[nr-1+j*nr] = int_x[nr-1+j*nr];
for(i=nr-2 + j*nr; i>=0+j*nr; i--) {
int_result[i] = int_x[i];
if(int_result[i] == NA_LOGICAL && gap < maxgap) {
int_result[i] = int_result[i+1];
gap++;
}
}
}
}
break;
case INTSXP:
int_x = INTEGER(x);
int_result = INTEGER(result);
if(!LOGICAL(fromLast)[0]) {
for(j=0; j < nc; j++) {
/* copy leading NAs */
_first = firstNonNACol(x, j);
//if(_first+1 == nr) continue;
for(i=0+j*nr; i < (_first+1); i++) {
int_result[i] = int_x[i];
}
/* result[_first] now has first value fromLast=FALSE */
for(i=_first+1; i<nr+j*nr; i++) {
int_result[i] = int_x[i];
if(int_result[i] == NA_INTEGER) {
if(limit > gap)
int_result[i] = int_result[i-1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i-1; ii > i-gap-1; ii--) {
int_result[ii] = NA_INTEGER;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have excessive trailing gap */
for(ii = i-1; ii > i-gap-1; ii--) {
int_result[ii] = NA_INTEGER;
}
}
}
} else {
/* nr-2 is first position to fill fromLast=TRUE */
for(j=0; j < nc; j++) {
int_result[nr-1+j*nr] = int_x[nr-1+j*nr];
for(i=nr-2 + j*nr; i>=0+j*nr; i--) {
int_result[i] = int_x[i];
if(int_result[i] == NA_INTEGER) {
if(limit > gap)
int_result[i] = int_result[i+1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i+1; ii < i+gap+1; ii++) {
int_result[ii] = NA_INTEGER;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have leading trailing gap */
for(ii = i+1; ii < i+gap+1; ii++) {
int_result[ii] = NA_INTEGER;
}
}
}
}
break;
case REALSXP:
real_x = REAL(x);
real_result = REAL(result);
if(!LOGICAL(fromLast)[0]) { /* fromLast=FALSE */
for(j=0; j < nc; j++) {
/* copy leading NAs */
_first = firstNonNACol(x, j);
//if(_first+1 == nr) continue;
for(i=0+j*nr; i < (_first+1); i++) {
real_result[i] = real_x[i];
}
/* result[_first] now has first value fromLast=FALSE */
for(i=_first+1; i<nr+j*nr; i++) {
real_result[i] = real_x[i];
if( ISNA(real_result[i]) || ISNAN(real_result[i])) {
if(limit > gap)
real_result[i] = real_result[i-1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i-1; ii > i-gap-1; ii--) {
real_result[ii] = NA_REAL;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have excessive trailing gap */
for(ii = i-1; ii > i-gap-1; ii--) {
real_result[ii] = NA_REAL;
}
}
}
} else { /* fromLast=TRUE */
for(j=0; j < nc; j++) {
real_result[nr-1+j*nr] = real_x[nr-1+j*nr];
for(i=nr-2 + j*nr; i>=0+j*nr; i--) {
real_result[i] = real_x[i];
if(ISNA(real_result[i]) || ISNAN(real_result[i])) {
if(limit > gap)
real_result[i] = real_result[i+1];
gap++;
} else {
if((int)gap > (int)maxgap) {
for(ii = i+1; ii < i+gap+1; ii++) {
real_result[ii] = NA_REAL;
}
}
gap=0;
}
}
if((int)gap > (int)maxgap) { /* check that we don't have leading trailing gap */
for(ii = i+1; ii < i+gap+1; ii++) {
real_result[ii] = NA_REAL;
}
}
}
}
break;
default:
error("unsupported type");
break;
}
if(isXts(x)) {
setAttrib(result, R_DimSymbol, getAttrib(x, R_DimSymbol));
setAttrib(result, R_DimNamesSymbol, getAttrib(x, R_DimNamesSymbol));
setAttrib(result, xts_IndexSymbol, getAttrib(x, xts_IndexSymbol));
copy_xtsCoreAttributes(x, result);
copy_xtsAttributes(x, result);
}
UNPROTECT(P);
return(result);
}
DOUBLE
FUNC (DOUBLE x, int *exp)
{
int sign;
int exponent;
DECL_ROUNDING
/* Test for NaN, infinity, and zero. */
if (ISNAN (x) || x + x == x)
{
*exp = 0;
return x;
}
sign = 0;
if (x < 0)
{
x = - x;
sign = -1;
}
BEGIN_ROUNDING ();
{
/* Since the exponent is an 'int', it fits in 64 bits. Therefore the
loops are executed no more than 64 times. */
DOUBLE pow2[64]; /* pow2[i] = 2^2^i */
DOUBLE powh[64]; /* powh[i] = 2^-2^i */
int i;
exponent = 0;
if (x >= L_(1.0))
{
/* A positive exponent. */
DOUBLE pow2_i; /* = pow2[i] */
DOUBLE powh_i; /* = powh[i] */
/* Invariants: pow2_i = 2^2^i, powh_i = 2^-2^i,
x * 2^exponent = argument, x >= 1.0. */
for (i = 0, pow2_i = L_(2.0), powh_i = L_(0.5);
;
i++, pow2_i = pow2_i * pow2_i, powh_i = powh_i * powh_i)
{
if (x >= pow2_i)
{
exponent += (1 << i);
x *= powh_i;
}
else
break;
pow2[i] = pow2_i;
powh[i] = powh_i;
}
/* Avoid making x too small, as it could become a denormalized
number and thus lose precision. */
while (i > 0 && x < pow2[i - 1])
{
i--;
powh_i = powh[i];
}
exponent += (1 << i);
x *= powh_i;
/* Here 2^-2^i <= x < 1.0. */
}
else
{
/* A negative or zero exponent. */
DOUBLE pow2_i; /* = pow2[i] */
DOUBLE powh_i; /* = powh[i] */
/* Invariants: pow2_i = 2^2^i, powh_i = 2^-2^i,
x * 2^exponent = argument, x < 1.0. */
for (i = 0, pow2_i = L_(2.0), powh_i = L_(0.5);
;
i++, pow2_i = pow2_i * pow2_i, powh_i = powh_i * powh_i)
{
if (x < powh_i)
{
exponent -= (1 << i);
x *= pow2_i;
}
else
break;
pow2[i] = pow2_i;
powh[i] = powh_i;
}
/* Here 2^-2^i <= x < 1.0. */
}
/* Invariants: x * 2^exponent = argument, and 2^-2^i <= x < 1.0. */
while (i > 0)
{
i--;
if (x < powh[i])
{
exponent -= (1 << i);
x *= pow2[i];
}
}
/* Here 0.5 <= x < 1.0. */
}
if (sign < 0)
x = - x;
END_ROUNDING ();
*exp = exponent;
return x;
}
SEXP na_omit_xts (SEXP x)
{
SEXP na_index, not_na_index, col_index, result;
int i, j, ij, nr, nc;
int not_NA, NA;
nr = nrows(x);
nc = ncols(x);
not_NA = nr;
int *int_x=NULL, *int_na_index=NULL, *int_not_na_index=NULL;
double *real_x=NULL;
switch(TYPEOF(x)) {
case LGLSXP:
for(i=0; i<nr; i++) {
for(j=0; j<nc; j++) {
ij = i + j*nr;
if(LOGICAL(x)[ij] == NA_LOGICAL) {
not_NA--;
break;
}
}
}
break;
case INTSXP:
int_x = INTEGER(x);
for(i=0; i<nr; i++) {
for(j=0; j<nc; j++) {
ij = i + j*nr;
if(int_x[ij] == NA_INTEGER) {
not_NA--;
break;
}
}
}
break;
case REALSXP:
real_x = REAL(x);
for(i=0; i<nr; i++) {
for(j=0; j<nc; j++) {
ij = i + j*nr;
if(ISNA(real_x[ij]) || ISNAN(real_x[ij])) {
not_NA--;
break;
}
}
}
break;
default:
error("unsupported type");
break;
}
if(not_NA==0) { /* all NAs */
return(allocVector(TYPEOF(x),0));
}
if(not_NA==0 || not_NA==nr)
return(x);
PROTECT(not_na_index = allocVector(INTSXP, not_NA));
PROTECT(na_index = allocVector(INTSXP, nr-not_NA));
/* pointers for efficiency as INTEGER in package code is a function call*/
int_not_na_index = INTEGER(not_na_index);
int_na_index = INTEGER(na_index);
not_NA = NA = 0;
switch(TYPEOF(x)) {
case LGLSXP:
for(i=0; i<nr; i++) {
for(j=0; j<nc; j++) {
ij = i + j*nr;
if(LOGICAL(x)[ij] == NA_LOGICAL) {
int_na_index[NA] = i+1;
NA++;
break;
}
if(j==(nc-1)) {
/* make it to end of column, OK*/
int_not_na_index[not_NA] = i+1;
not_NA++;
}
}
}
break;
case INTSXP:
for(i=0; i<nr; i++) {
for(j=0; j<nc; j++) {
ij = i + j*nr;
if(int_x[ij] == NA_INTEGER) {
int_na_index[NA] = i+1;
NA++;
break;
}
if(j==(nc-1)) {
/* make it to end of column, OK*/
int_not_na_index[not_NA] = i+1;
not_NA++;
}
}
}
break;
case REALSXP:
for(i=0; i<nr; i++) {
for(j=0; j<nc; j++) {
ij = i + j*nr;
if(ISNA(real_x[ij]) || ISNAN(real_x[ij])) {
int_na_index[NA] = i+1;
NA++;
break;
}
if(j==(nc-1)) {
/* make it to end of column, OK*/
int_not_na_index[not_NA] = i+1;
not_NA++;
}
}
}
break;
default:
error("unsupported type");
break;
}
PROTECT(col_index = allocVector(INTSXP, nc));
for(i=0; i<nc; i++)
INTEGER(col_index)[i] = i+1;
PROTECT(result = do_subset_xts(x, not_na_index, col_index, ScalarLogical(0)));
SEXP class;
PROTECT(class = allocVector(STRSXP, 1));
SET_STRING_ELT(class, 0, mkChar("omit"));
setAttrib(na_index, R_ClassSymbol, class);
UNPROTECT(1);
setAttrib(result, install("na.action"), na_index);
UNPROTECT(4);
return(result);
}
void kcores_R(double *mat, int *n, int *m, double *corevec, int *dtype, int *pdiag, int *pigeval)
/*Compute k-cores for an input graph. Cores to be computed can be based on
indegree (dtype=0), outdegree (dtype=1), or total degree (dtype=2). Algorithm
used is based on Batagelj and Zaversnik (2002), with some pieces filled in.
It's quite fast -- for large graphs, far more time is spent processing the
input than computing the k-cores! When processing edge values, igeval
determines whether edge values should be ignored (0) or used (1); missing edges
are not counted in either case. When diag=1, diagonals are used; else they are
also omitted.*/
{
int i,j,k,temp,*ord,*nod,diag,igev;
double *stat;
snaNet *g;
slelement *ep;
diag=*pdiag;
igev=*pigeval;
/*Initialize sna internal network*/
GetRNGstate();
g=elMatTosnaNet(mat,n,m);
PutRNGstate();
/*Calculate the sorting stat*/
stat=(double *)R_alloc(*n,sizeof(double));
switch(*dtype){
case 0: /*Indegree*/
for(i=0;i<*n;i++){
stat[i]=0.0;
for(ep=snaFirstEdge(g,i,0);ep!=NULL;ep=ep->next[0])
if(((diag)||(i!=(int)(ep->val)))&&((ep->dp!=NULL)&&(!ISNAN(*(double *)(ep->dp)))))
stat[i]+= igev ? *((double *)(ep->dp)) : 1.0;
}
break;
case 1: /*Outdegree*/
for(i=0;i<*n;i++){
stat[i]=0.0;
for(ep=snaFirstEdge(g,i,1);ep!=NULL;ep=ep->next[0])
if(((diag)||(i!=(int)(ep->val)))&&((ep->dp!=NULL)&&(!ISNAN(*(double *)(ep->dp)))))
stat[i]+= igev ? *((double *)(ep->dp)) : 1.0;
}
break;
case 2: /*Total degree*/
for(i=0;i<*n;i++){
stat[i]=0.0;
for(ep=snaFirstEdge(g,i,0);ep!=NULL;ep=ep->next[0])
if(((diag)||(i!=(int)(ep->val)))&&((ep->dp!=NULL)&&(!ISNAN(*(double *)(ep->dp)))))
stat[i]+= igev ? *((double *)(ep->dp)) : 1.0;
for(ep=snaFirstEdge(g,i,1);ep!=NULL;ep=ep->next[0])
if(((diag)||(i!=(int)(ep->val)))&&((ep->dp!=NULL)&&(!ISNAN(*(double *)(ep->dp)))))
stat[i]+= igev ? *((double *)(ep->dp)) : 1.0;
}
break;
}
/*Set initial core/order values*/
ord=(int *)R_alloc(*n,sizeof(int));
nod=(int *)R_alloc(*n,sizeof(int));
for(i=0;i<*n;i++){
corevec[i]=stat[i];
ord[i]=nod[i]=i;
}
/*Heap reminder: i->(2i+1, 2i+2); parent at floor((i-1)/2); root at 0*/
/*Build a heap, based on the stat vector*/
for(i=1;i<*n;i++){
j=i;
while(j>0){
k=(int)floor((j-1)/2); /*Parent node*/
if(stat[nod[k]]>stat[nod[j]]){ /*Out of order -- swap*/
temp=nod[k];
nod[k]=nod[j];
nod[j]=temp;
ord[nod[j]]=j;
ord[nod[k]]=k;
}
j=k; /*Move to parent*/
}
}
/*Heap test
for(i=0;i<*n;i++){
Rprintf("Pos %d (n=%d, s=%.0f, check=%d): ",i,nod[i],stat[nod[i]],ord[nod[i]]==i);
j=(int)floor((i-1)/2.0);
if(j>=0)
Rprintf("Parent %d (n=%d, s=%.0f), ",j,nod[j],stat[nod[j]]);
else
Rprintf("No Parent (root), ");
j=2*i+1;
if(j<*n)
Rprintf("Lchild %d (n=%d, s=%.0f), ",j,nod[j],stat[nod[j]]);
else
Rprintf("No Lchild, ");
j=2*i+2;
if(j<*n)
Rprintf("Rchild %d (n=%d, s=%.0f)\n",j,nod[j],stat[nod[j]]);
else
Rprintf("No Rchild\n");
}
*/
/*Now, find the k-cores*/
for(i=*n-1;i>=0;i--){
/*Rprintf("Stack currently spans positions 0 to %d.\n",i);*/
corevec[nod[0]]=stat[nod[0]]; /*Coreness of top element is fixed*/
/*Rprintf("Pulled min vertex (%d): coreness was %.0f\n",nod[0],corevec[nod[0]]);*/
/*Swap root w/last element (and re-heap) to remove it*/
temp=nod[0];
nod[0]=nod[i];
nod[i]=temp;
ord[nod[0]]=0;
ord[nod[i]]=i;
j=0;
while(2*j+1<i){
k=2*j+1; /*Get first child*/
if((k<i-1)&&(stat[nod[k+1]]<stat[nod[k]])) /*Use smaller child node*/
k++;
if(stat[nod[k]]<stat[nod[j]]){ /*If child smaller, swap*/
temp=nod[j];
nod[j]=nod[k];
nod[k]=temp;
ord[nod[j]]=j;
ord[nod[k]]=k;
}else
break;
j=k; /*Move to child, repeat*/
}
/*Having removed top element, adjust its neighbors downward*/
switch(*dtype){
case 0: /*Indegree -> update out-neighbors*/
/*Rprintf("Reducing indegree of %d outneighbors...\n",g->outdeg[nod[i]]);*/
for(ep=snaFirstEdge(g,nod[i],1);ep!=NULL;ep=ep->next[0]){
j=(int)ep->val;
if(ord[j]<i){ /*Don't mess with removed nodes!*/
/*Adjust stat*/
/*Rprintf("\t%d: %.0f ->",j,stat[j]);*/
stat[j]=MAX(stat[j]-*((double *)(ep->dp)),corevec[nod[i]]);
/*Rprintf(" %.0f\n",stat[j]);*/
/*Percolate heap upward (stat can only go down!)*/
j=ord[j];
while(floor((j-1)/2)>=0){
k=floor((j-1)/2); /*Parent node*/
if(stat[nod[k]]>stat[nod[j]]){ /*If parent greater, swap*/
temp=nod[j];
nod[j]=nod[k];
nod[k]=temp;
ord[nod[j]]=j;
ord[nod[k]]=k;
}else
break;
j=k; /*Repeat w/new parent*/
}
}
}
break;
case 1: /*Outdegree -> update in-neighbors*/
for(ep=snaFirstEdge(g,nod[i],0);ep!=NULL;ep=ep->next[0]){
j=(int)ep->val;
if(ord[j]<i){ /*Don't mess with removed nodes!*/
/*Adjust stat*/
/*Rprintf("\t%d: %.0f ->",j,stat[j]);*/
stat[j]=MAX(stat[j]-*((double *)(ep->dp)),corevec[nod[i]]);
/*Rprintf(" %.0f\n",stat[j]);*/
/*Percolate heap upward (stat can only go down!)*/
j=ord[j];
while(floor((j-1)/2)>=0){
k=floor((j-1)/2); /*Parent node*/
if(stat[nod[k]]>stat[nod[j]]){ /*If parent greater, swap*/
temp=nod[j];
nod[j]=nod[k];
nod[k]=temp;
ord[nod[j]]=j;
ord[nod[k]]=k;
}else
break;
j=k; /*Repeat w/new parent*/
}
}
}
break;
case 2: /*Total degree -> update all neighbors*/
for(ep=snaFirstEdge(g,nod[i],1);ep!=NULL;ep=ep->next[0]){
j=(int)ep->val;
if(ord[j]<i){ /*Don't mess with removed nodes!*/
/*Adjust stat*/
/*Rprintf("\t%d: %.0f ->",j,stat[j]);*/
stat[j]=MAX(stat[j]-*((double *)(ep->dp)),corevec[nod[i]]);
/*Rprintf(" %.0f\n",stat[j]);*/
/*Percolate heap upward (stat can only go down!)*/
j=ord[j];
while(floor((j-1)/2)>=0){
k=floor((j-1)/2); /*Parent node*/
if(stat[nod[k]]>stat[nod[j]]){ /*If parent greater, swap*/
temp=nod[j];
nod[j]=nod[k];
nod[k]=temp;
ord[nod[j]]=j;
ord[nod[k]]=k;
}else
break;
j=k; /*Repeat w/new parent*/
}
}
}
for(ep=snaFirstEdge(g,nod[i],0);ep!=NULL;ep=ep->next[0]){
j=(int)ep->val;
if(ord[j]<i){ /*Don't mess with removed nodes!*/
/*Adjust stat*/
/*Rprintf("\t%d: %.0f ->",j,stat[j]);*/
stat[j]=MAX(stat[j]-*((double *)(ep->dp)),corevec[nod[i]]);
/*Rprintf(" %.0f\n",stat[j]);*/
/*Percolate heap upward (stat can only go down!)*/
j=ord[j];
while(floor((j-1)/2)>=0){
k=floor((j-1)/2); /*Parent node*/
if(stat[nod[k]]>stat[nod[j]]){ /*If parent greater, swap*/
temp=nod[j];
nod[j]=nod[k];
nod[k]=temp;
ord[nod[j]]=j;
ord[nod[k]]=k;
}else
break;
j=k; /*Repeat w/new parent*/
}
}
}
break;
}
}
}
/*
* PURPOSE
* computes sqrt(a^2 + b^2) with accuracy and
* without spurious underflow / overflow problems
*
* MOTIVATION
* This work was motivated by the fact that the original Scilab
* PYTHAG, which implements Moler and Morrison's algorithm is slow.
* Some tests showed that the Kahan's algorithm, is superior in
* precision and moreover faster than the original PYTHAG. The speed
* gain partly comes from not calling DLAMCH.
*
* REFERENCE
* This is a Fortran-77 translation of an algorithm by William Kahan,
* which appears in his article "Branch cuts for complex elementary
* functions, or much ado about nothing's sign bit",
* Editors: Iserles, A. and Powell, M. J. D.
* in "States of the Art in Numerical Analysis"
* Oxford, Clarendon Press, 1987
* ISBN 0-19-853614-3
** AUTHOR
* Bruno Pincon <*****@*****.**>,
* Thanks to Lydia van Dijk <*****@*****.**>
*/
ELEMENTARY_FUNCTIONS_IMPEXP double dpythags(double _dblVal1, double _dblVal2)
{
double dblSqrt2 = 1.41421356237309504;
double dblSqrt2p1 = 2.41421356237309504;
double dblEsp = 1.25371671790502177E-16;
double dblRMax = getOverflowThreshold();
double dblAbs1 = 0;
double dblAbs2 = 0;
double dblTemp = 0;
if (ISNAN(_dblVal1) == 1)
{
return _dblVal2;
}
if (ISNAN(_dblVal2) == 1)
{
return _dblVal1;
}
dblAbs1 = dabss(_dblVal1);
dblAbs2 = dabss(_dblVal2);
//Order x and y such that 0 <= y <= x
if (dblAbs1 < dblAbs2)
{
dblTemp = dblAbs1;
dblAbs1 = dblAbs2;
dblAbs2 = dblTemp;
}
//Test for overflowing x
if ( dblAbs1 >= dblRMax)
{
return dblAbs1;
}
//Handle generic case
dblTemp = dblAbs1 - dblAbs2;
if (dblTemp != dblAbs1)
{
double dblS = 0;
if (dblTemp > dblAbs2)
{
dblS = dblAbs1 / dblAbs2;
dblS += dsqrts(1 + dblS * dblS);
}
else
{
dblS = dblTemp / dblAbs2;
dblTemp = (2 + dblS) * dblS;
dblS = ((dblEsp + dblTemp / (dblSqrt2 + dsqrts(2 + dblTemp))) + dblS) + dblSqrt2p1;
}
return dblAbs1 + dblAbs2 / dblS;
}
else
{
return dblAbs1;
}
}
/*
* cos(x):
*
* if (x < 0) {
* x = abs(x);
* }
* if (x > 2*pi) {
* x %= 2*pi;
* }
* if (x > pi) {
* x -= pi;
* sign inverse;
* }
* if (x > pi/2) {
* y = sin(x - pi/2);
* sign inverse;
* } else {
* y = cos(x);
* }
* if (sign) {
* y = -y;
* }
*/
struct fpn *
fpu_cos(struct fpemu *fe)
{
struct fpn x;
struct fpn p;
struct fpn *r;
int sign;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
/* x = abs(input) */
sign = 0;
CPYFPN(&x, &fe->fe_f2);
x.fp_sign = 0;
/* p <- 2*pi */
fpu_const(&p, FPU_CONST_PI);
p.fp_exp++;
/*
* if (x > 2*pi*N)
* cos(x) is cos(x - 2*pi*N)
*/
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
r = fpu_cmp(fe);
if (r->fp_sign == 0) {
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
r = fpu_mod(fe);
CPYFPN(&x, r);
}
/* p <- pi */
p.fp_exp--;
/*
* if (x > pi)
* cos(x) is -cos(x - pi)
*/
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
fe->fe_f2.fp_sign = 1;
r = fpu_add(fe);
if (r->fp_sign == 0) {
CPYFPN(&x, r);
sign ^= 1;
}
/* p <- pi/2 */
p.fp_exp--;
/*
* if (x > pi/2)
* cos(x) is -sin(x - pi/2)
* else
* cos(x)
*/
CPYFPN(&fe->fe_f1, &x);
CPYFPN(&fe->fe_f2, &p);
fe->fe_f2.fp_sign = 1;
r = fpu_add(fe);
if (r->fp_sign == 0) {
__fpu_sincos_cordic(fe, r);
r = &fe->fe_f1;
sign ^= 1;
} else {
__fpu_sincos_cordic(fe, &x);
r = &fe->fe_f2;
}
r->fp_sign = sign;
return r;
}
/*public static*/
double
LineIntersector::interpolateZ(const Coordinate &p,
const Coordinate &p1, const Coordinate &p2)
{
#if GEOS_DEBUG
cerr<<"LineIntersector::interpolateZ("<<p.toString()<<", "<<p1.toString()<<", "<<p2.toString()<<")"<<endl;
#endif
if ( ISNAN(p1.z) )
{
#if GEOS_DEBUG
cerr<<" p1 do not have a Z"<<endl;
#endif
return p2.z; // might be DoubleNotANumber again
}
if ( ISNAN(p2.z) )
{
#if GEOS_DEBUG
cerr<<" p2 do not have a Z"<<endl;
#endif
return p1.z; // might be DoubleNotANumber again
}
if (p==p1)
{
#if GEOS_DEBUG
cerr<<" p==p1, returning "<<p1.z<<endl;
#endif
return p1.z;
}
if (p==p2)
{
#if GEOS_DEBUG
cerr<<" p==p2, returning "<<p2.z<<endl;
#endif
return p2.z;
}
//double zgap = fabs(p2.z - p1.z);
double zgap = p2.z - p1.z;
if ( ! zgap )
{
#if GEOS_DEBUG
cerr<<" no zgap, returning "<<p2.z<<endl;
#endif
return p2.z;
}
double xoff = (p2.x-p1.x);
double yoff = (p2.y-p1.y);
double seglen = (xoff*xoff+yoff*yoff);
xoff = (p.x-p1.x);
yoff = (p.y-p1.y);
double pdist = (xoff*xoff+yoff*yoff);
double fract = sqrt(pdist/seglen);
double zoff = zgap*fract;
//double interpolated = p1.z < p2.z ? p1.z+zoff : p1.z-zoff;
double interpolated = p1.z+zoff;
#if GEOS_DEBUG
cerr<<" zgap:"<<zgap<<" seglen:"<<seglen<<" pdist:"<<pdist
<<" fract:"<<fract<<" z:"<<interpolated<<endl;
#endif
return interpolated;
}
