Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include "tutil.h"
Toshihiro Shimizu 890ddd
#include "tmathutil.h"
Toshihiro Shimizu 890ddd
#include "tconvert.h"
Toshihiro Shimizu 890ddd
#include <iterator></iterator>
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
using TConsts::epsilon;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TMathException::TMathException(std::string msg) : m_msg(::to_wstring(msg)) {}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
namespace {
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//! maximum order for a polynomial
Toshihiro Shimizu 890ddd
const int MAX_ORDER = 12;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//! smallest relative error we want
Toshihiro Shimizu 890ddd
const double RELERROR = 1.0e-14;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//! max power of 10 we wish to search to
Toshihiro Shimizu 890ddd
const int MAXPOW = 32;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
//! max number of iterations
Toshihiro Shimizu 890ddd
const int MAXIT = 800;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//! a coefficient smaller than SMALL_ENOUGH is considered to be zero (0.0).
Toshihiro Shimizu 890ddd
const double SMALL_ENOUGH = 1.0e-12;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
inline int getEl(int i, int j, int n) { return (j - 1) + (i - 1) * n; }
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//! structure type for representing a polynomial
Shinya Kitaoka 120a6e
typedef struct {
Shinya Kitaoka 120a6e
  int ord;
Shinya Kitaoka 120a6e
  double coef[MAX_ORDER + 1];
Toshihiro Shimizu 890ddd
} poly;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//  compute number of root in (min,max)
Toshihiro Shimizu 890ddd
double evalpoly(int ord, double *coef, double x);
Toshihiro Shimizu 890ddd
int numchanges(int np, poly *sseq, double a);
Toshihiro Shimizu 890ddd
int buildsturm(int ord, poly *sseq);
Toshihiro Shimizu 890ddd
int modrf(int ord, double *coef, double a, double b, double *val);
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void convert(const std::vector<double> &v, poly &p) {</double>
Shinya Kitaoka 120a6e
  assert((int)v.size() <= MAX_ORDER);
Shinya Kitaoka 120a6e
  if ((int)v.size() > MAX_ORDER) return;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  p.ord = v.size() - 1;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  std::copy(v.begin(), v.end(), p.coef);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void convert(const poly &p, std::vector<double> &v) {</double>
Shinya Kitaoka 120a6e
  v.resize(p.ord);
Shinya Kitaoka 120a6e
  std::copy(p.coef, p.coef + p.ord, v.begin());
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
  * modp
Toshihiro Shimizu 890ddd
  *
Toshihiro Shimizu 890ddd
  *  calculates the modulus of u(x) / v(x) leaving it in r, it
Toshihiro Shimizu 890ddd
  *  returns 0 if r(x) is a constant.
Shinya Kitaoka 120a6e
  *  note: this function assumes the leading coefficient of v
Toshihiro Shimizu 890ddd
  *  is 1 or -1
Toshihiro Shimizu 890ddd
  */
Shinya Kitaoka 120a6e
int modp(poly *u, poly *v, poly *r) {
Shinya Kitaoka 120a6e
  int k, j;
Shinya Kitaoka 120a6e
  double *nr, *end, *uc;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  nr  = r->coef;
Shinya Kitaoka 120a6e
  end = &u->coef[u->ord];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  uc                      = u->coef;
Shinya Kitaoka 120a6e
  while (uc <= end) *nr++ = *uc++;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (v->coef[v->ord] < 0.0) {
Shinya Kitaoka 120a6e
    for (k = u->ord - v->ord - 1; k >= 0; k -= 2) r->coef[k] = -r->coef[k];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    for (k = u->ord - v->ord; k >= 0; k--)
Shinya Kitaoka 120a6e
      for (j       = v->ord + k - 1; j >= k; j--)
Shinya Kitaoka 120a6e
        r->coef[j] = -r->coef[j] - r->coef[v->ord + k] * v->coef[j - k];
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    for (k = u->ord - v->ord; k >= 0; k--)
Shinya Kitaoka 120a6e
      for (j = v->ord + k - 1; j >= k; j--)
Shinya Kitaoka 120a6e
        r->coef[j] -= r->coef[v->ord + k] * v->coef[j - k];
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  k = v->ord - 1;
Shinya Kitaoka 120a6e
  while (k >= 0 && fabs(r->coef[k]) < SMALL_ENOUGH) {
Shinya Kitaoka 120a6e
    r->coef[k] = 0.0;
Shinya Kitaoka 120a6e
    k--;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  r->ord = (k < 0) ? 0 : k;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return (r->ord);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
  * buildsturm
Toshihiro Shimizu 890ddd
  *
Toshihiro Shimizu 890ddd
  *  build up a sturm sequence for a polynomial in sseq, returning
Toshihiro Shimizu 890ddd
  * the number of polynomials in the sequence
Toshihiro Shimizu 890ddd
  */
Shinya Kitaoka 120a6e
int buildsturm(int ord, poly *sseq) {
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  double f, *fp, *fc;
Shinya Kitaoka 120a6e
  poly *sp;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  sseq[0].ord = ord;
Shinya Kitaoka 120a6e
  sseq[1].ord = ord - 1;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  //  calculate the derivative and normalise the leadingcoefficient.
Shinya Kitaoka 120a6e
  f  = fabs(sseq[0].coef[ord] * ord);
Shinya Kitaoka 120a6e
  fp = sseq[1].coef;
Shinya Kitaoka 120a6e
  fc = sseq[0].coef + 1;
Shinya Kitaoka 120a6e
  for (i = 1; i <= ord; i++) *fp++ = *fc++ * i / f;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // construct the rest of the Sturm sequence
Shinya Kitaoka 120a6e
  for (sp = sseq + 2; modp(sp - 2, sp - 1, sp); sp++) {
Shinya Kitaoka 120a6e
    //  reverse the sign and normalise
Shinya Kitaoka 120a6e
    f = -fabs(sp->coef[sp->ord]);
Shinya Kitaoka 120a6e
    for (fp = &sp->coef[sp->ord]; fp >= sp->coef; fp--) *fp /= f;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  sp->coef[0] = -sp->coef[0];  // reverse the sign
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return (sp - sseq);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
  return the number of distinct real roots of the polynomial
Toshihiro Shimizu 890ddd
  described in sseq
Toshihiro Shimizu 890ddd
  */
Shinya Kitaoka 120a6e
int numroots(int np, poly *sseq, int &atneg, int &atpos) {
Shinya Kitaoka 120a6e
  int atposinf = 0, atneginf = 0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  poly *s;
Shinya Kitaoka 120a6e
  double f, lf;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // change at positive infinity
Shinya Kitaoka 120a6e
  lf = sseq[0].coef[sseq[0].ord];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (s = sseq + 1; s <= sseq + np; ++s) {
Shinya Kitaoka 120a6e
    f = s->coef[s->ord];
Shinya Kitaoka 120a6e
    if (0.0 == lf || lf * f < 0.0) ++atposinf;
Shinya Kitaoka 120a6e
    lf = f;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // change at negative infinity
Shinya Kitaoka 120a6e
  if (sseq[0].ord & 1)
Shinya Kitaoka 120a6e
    lf = -sseq[0].coef[sseq[0].ord];
Shinya Kitaoka 120a6e
  else
Shinya Kitaoka 120a6e
    lf = sseq[0].coef[sseq[0].ord];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (s = sseq + 1; s <= sseq + np; ++s) {
Shinya Kitaoka 120a6e
    if (s->ord & 1)
Shinya Kitaoka 120a6e
      f = -s->coef[s->ord];
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      f = s->coef[s->ord];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (0.0 == lf || lf * f < 0.0) ++atneginf;
Shinya Kitaoka 120a6e
    lf = f;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  atneg = atneginf;
Shinya Kitaoka 120a6e
  atpos = atposinf;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return atneginf - atposinf;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
  * numchanges
Toshihiro Shimizu 890ddd
  *
Toshihiro Shimizu 890ddd
  * return the number of sign changes in the Sturm sequence in
Toshihiro Shimizu 890ddd
  * sseq at the value a.
Toshihiro Shimizu 890ddd
  */
Shinya Kitaoka 120a6e
int numchanges(int np, poly *sseq, double a) {
Shinya Kitaoka 120a6e
  int changes;
Shinya Kitaoka 120a6e
  double f, lf;
Shinya Kitaoka 120a6e
  poly *s;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  changes = 0;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  lf = evalpoly(sseq[0].ord, sseq[0].coef, a);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  for (s = sseq + 1; s <= sseq + np; s++) {
Shinya Kitaoka 120a6e
    f = evalpoly(s->ord, s->coef, a);
Shinya Kitaoka 120a6e
    if (lf == 0.0 || lf * f < 0) changes++;
Shinya Kitaoka 120a6e
    lf = f;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return (changes);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
  * sbisect
Toshihiro Shimizu 890ddd
  *
Toshihiro Shimizu 890ddd
  * uses a bisection based on the sturm sequence for the polynomial
Toshihiro Shimizu 890ddd
  * described in sseq to isolate intervals in which roots occur,
Toshihiro Shimizu 890ddd
  * the roots are returned in the roots array in order of magnitude.
Toshihiro Shimizu 890ddd
  */
Shinya Kitaoka 120a6e
void sbisect(int np, poly *sseq, double min, double max, int atmin, int atmax,
Shinya Kitaoka 120a6e
             double *roots) {
Shinya Kitaoka 120a6e
  double mid;
Shinya Kitaoka 120a6e
  int n1 = 0, n2 = 0, its, atmid;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if ((atmin - atmax) == 1) {
Shinya Kitaoka 120a6e
    // first try a less expensive technique.
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (modrf(sseq->ord, sseq->coef, min, max, &roots[0])) return;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    /*
Shinya Kitaoka 120a6e
* if we get here we have to evaluate the root the hard
Shinya Kitaoka 120a6e
* way by using the Sturm sequence.
Shinya Kitaoka 120a6e
*/
Shinya Kitaoka 120a6e
    for (its = 0; its < MAXIT; its++) {
Shinya Kitaoka 120a6e
      mid = (min + max) / 2;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      atmid = numchanges(np, sseq, mid);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      if (fabs(mid) > RELERROR) {
Shinya Kitaoka 120a6e
        if (fabs((max - min) / mid) < RELERROR) {
Shinya Kitaoka 120a6e
          roots[0] = mid;
Shinya Kitaoka 120a6e
          return;
Shinya Kitaoka 120a6e
        }
Shinya Kitaoka 120a6e
      } else if (fabs(max - min) < RELERROR) {
Shinya Kitaoka 120a6e
        roots[0] = mid;
Shinya Kitaoka 120a6e
        return;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      if ((atmin - atmid) == 0)
Shinya Kitaoka 120a6e
        min = mid;
Shinya Kitaoka 120a6e
      else
Shinya Kitaoka 120a6e
        max = mid;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (its == MAXIT) {
Shinya Kitaoka 120a6e
      /*
Shinya Kitaoka 120a6e
fprintf(stderr, "sbisect: overflow min %f max %f\
Shinya Kitaoka 120a6e
diff %e nroot %d n1 %d n2 %d\n",
Shinya Kitaoka 120a6e
min, max, max - min, nroot, n1, n2);
Shinya Kitaoka 120a6e
*/
Shinya Kitaoka 120a6e
      roots[0] = mid;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  /*
Shinya Kitaoka 120a6e
* more than one root in the interval, we have to bisect...
Shinya Kitaoka 120a6e
*/
Shinya Kitaoka 120a6e
  for (its = 0; its < MAXIT; its++) {
Shinya Kitaoka 120a6e
    mid = (min + max) / 2;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    atmid = numchanges(np, sseq, mid);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    n1 = atmin - atmid;
Shinya Kitaoka 120a6e
    n2 = atmid - atmax;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (n1 != 0 && n2 != 0) {
Shinya Kitaoka 120a6e
      sbisect(np, sseq, min, mid, atmin, atmid, roots);
Shinya Kitaoka 120a6e
      sbisect(np, sseq, mid, max, atmid, atmax, &roots[n1]);
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (n1 == 0)
Shinya Kitaoka 120a6e
      min = mid;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      max = mid;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (its == MAXIT) {
Shinya Kitaoka 120a6e
    /*
Shinya Kitaoka 120a6e
fprintf(stderr, "sbisect: roots too close together\n");
Shinya Kitaoka 120a6e
fprintf(stderr, "sbisect: overflow min %f max %f diff %e\
Shinya Kitaoka 120a6e
nroot %d n1 %d n2 %d\n",
Shinya Kitaoka 120a6e
min, max, max - min, nroot, n1, n2);
Shinya Kitaoka 120a6e
*/
Shinya Kitaoka 120a6e
    for (n1 = atmax; n1 < atmin; n1++) roots[n1 - atmax] = mid;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
  * evalpoly
Toshihiro Shimizu 890ddd
  *
Toshihiro Shimizu 890ddd
  * evaluate polynomial defined in coef returning its value.
Toshihiro Shimizu 890ddd
    */
Shinya Kitaoka 120a6e
double evalpoly(int ord, double *coef, double x) {
Shinya Kitaoka 120a6e
  double *fp, f;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  fp = &coef[ord];
Shinya Kitaoka 120a6e
  f  = *fp;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  for (fp--; fp >= coef; fp--) f = x * f + *fp;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return (f);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
    * modrf
Toshihiro Shimizu 890ddd
    *
Toshihiro Shimizu 890ddd
    * uses the modified regula-falsi method to evaluate the root
Toshihiro Shimizu 890ddd
    * in interval [a,b] of the polynomial described in coef. The
Toshihiro Shimizu 890ddd
    * root is returned is returned in *val. The routine returns zero
Toshihiro Shimizu 890ddd
    * if it can't converge.
Toshihiro Shimizu 890ddd
    */
Shinya Kitaoka 120a6e
int modrf(int ord, double *coef, double a, double b, double *val) {
Shinya Kitaoka 120a6e
  int its;
Shinya Kitaoka 120a6e
  double fa, fb, x, fx, lfx;
Shinya Kitaoka 120a6e
  double *fp, *scoef, *ecoef;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  scoef = coef;
Shinya Kitaoka 120a6e
  ecoef = &coef[ord];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  fb = fa = *ecoef;
Shinya Kitaoka 120a6e
  for (fp = ecoef - 1; fp >= scoef; fp--) {
Shinya Kitaoka 120a6e
    fa = a * fa + *fp;
Shinya Kitaoka 120a6e
    fb = b * fb + *fp;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  //  if there is no sign difference the method won't work
Shinya Kitaoka 120a6e
  if (fa * fb > 0.0) return (0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (fabs(fa) < RELERROR) {
Shinya Kitaoka 120a6e
    *val = a;
Shinya Kitaoka 120a6e
    return (1);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (fabs(fb) < RELERROR) {
Shinya Kitaoka 120a6e
    *val = b;
Shinya Kitaoka 120a6e
    return (1);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  lfx = fa;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (its = 0; its < MAXIT; its++) {
Shinya Kitaoka 120a6e
    x = (fb * a - fa * b) / (fb - fa);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    fx = *ecoef;
Shinya Kitaoka 120a6e
    for (fp = ecoef - 1; fp >= scoef; fp--) fx = x * fx + *fp;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (fabs(x) > RELERROR) {
Shinya Kitaoka 120a6e
      if (fabs(fx / x) < RELERROR) {
Shinya Kitaoka 120a6e
        *val = x;
Shinya Kitaoka 120a6e
        return (1);
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    } else if (fabs(fx) < RELERROR) {
Shinya Kitaoka 120a6e
      *val = x;
Shinya Kitaoka 120a6e
      return (1);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if ((fa * fx) < 0) {
Shinya Kitaoka 120a6e
      b  = x;
Shinya Kitaoka 120a6e
      fb = fx;
Shinya Kitaoka 120a6e
      if ((lfx * fx) > 0) fa /= 2;
Shinya Kitaoka 120a6e
    } else {
Shinya Kitaoka 120a6e
      a  = x;
Shinya Kitaoka 120a6e
      fa = fx;
Shinya Kitaoka 120a6e
      if ((lfx * fx) > 0) fb /= 2;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    lfx = fx;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // fprintf(stderr, "modrf overflow %f %f %f\n", a, b, fx);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return (0);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*!
Toshihiro Shimizu 890ddd
    a x^2 + b x + c
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      Remark:
Shinya Kitaoka 120a6e
      poly[0] = c
Toshihiro Shimizu 890ddd
      poly[1] = b
Shinya Kitaoka 120a6e
      poly[2] = a
Toshihiro Shimizu 890ddd
    */
Shinya Kitaoka 120a6e
int rootForQuadraticEquation(const std::vector<double> &v,</double>
Shinya Kitaoka 120a6e
                             std::vector<double> &sol) {</double>
Shinya Kitaoka 120a6e
  double q, delta;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  /*
Shinya Kitaoka 120a6e
if( isAlmostZero(v[2]))
Toshihiro Shimizu 890ddd
{
Shinya Kitaoka 120a6e
if ( isAlmostZero(v[1]) )
Shinya Kitaoka 120a6e
return -1;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  sol.push_back(-v[0]/v[1]);
Shinya Kitaoka 120a6e
  return 1;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
*/
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (isAlmostZero(v[1])) {
Shinya Kitaoka 120a6e
    q = -v[0] / v[2];
Shinya Kitaoka 120a6e
    if (q < 0)
Shinya Kitaoka 120a6e
      return 0;
Shinya Kitaoka 120a6e
    else if (isAlmostZero(q)) {
Shinya Kitaoka 120a6e
      sol.push_back(0);
Shinya Kitaoka 120a6e
      return 1;
Shinya Kitaoka 120a6e
    }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
    q = sqrt(q);
Shinya Kitaoka 120a6e
    sol.push_back(-q);
Shinya Kitaoka 120a6e
    sol.push_back(q);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
    return 2;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  delta = sq(v[1]) - 4.0 * v[0] * v[2];
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (delta < 0.0) return 0;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  assert(v[2] != 0);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (isAlmostZero(delta)) {
Shinya Kitaoka 120a6e
    sol.push_back(-v[1] / (v[2] * 2.0));
Shinya Kitaoka 120a6e
    return 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  q = -0.5 * (v[1] + tsign(v[1]) * sqrt(delta));
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  assert(q != 0);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  sol.push_back(v[0] / q);
Shinya Kitaoka 120a6e
  sol.push_back(q / v[2]);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return 2;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
    a x^3+b x^2 + c x + d
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      Remark:
Shinya Kitaoka 120a6e
      poly[0] = d
Toshihiro Shimizu 890ddd
      poly[1] = c
Shinya Kitaoka 120a6e
      poly[2] = b
Shinya Kitaoka 120a6e
      poly[3] = a
Toshihiro Shimizu 890ddd
    */
Shinya Kitaoka 120a6e
int rootForCubicEquation(const std::vector<double> &p,</double>
Shinya Kitaoka 120a6e
                         std::vector<double> &sol) {</double>
Shinya Kitaoka 120a6e
  /*
Shinya Kitaoka 120a6e
if( isAlmostZero(p[3]) )
Shinya Kitaoka 120a6e
return rootForQuadraticEquation(p,sol);
Shinya Kitaoka 120a6e
*/
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (isAlmostZero(p[0])) {
Shinya Kitaoka 120a6e
    int numberOfSol;
Shinya Kitaoka 120a6e
    std::vector<double> redPol(3);</double>
Shinya Kitaoka 120a6e
    redPol[0] = p[1];
Shinya Kitaoka 120a6e
    redPol[1] = p[2];
Shinya Kitaoka 120a6e
    redPol[2] = p[3];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    numberOfSol = rootForQuadraticEquation(redPol, sol);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    for (int i = 0; i < numberOfSol; ++i)
Shinya Kitaoka 120a6e
      if (0.0 == sol[i]) return numberOfSol;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // altrimenti devo contare la soluzione nulla
Shinya Kitaoka 120a6e
    ++numberOfSol;
Shinya Kitaoka 120a6e
    sol.push_back(0);
Shinya Kitaoka 120a6e
    return numberOfSol;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double inv_v3 = 1.0 / p[3], a = p[2] * inv_v3, b = p[1] * inv_v3,
Shinya Kitaoka 120a6e
         c = p[0] * inv_v3;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  static const double inv_3  = 1.0 / 3.0;
Shinya Kitaoka 120a6e
  static const double inv_9  = 1.0 / 9.0;
Shinya Kitaoka 120a6e
  static const double inv_54 = 1.0 / 54.0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double Q = (sq(a) - 3.0 * b) * inv_9,
Shinya Kitaoka 120a6e
         R = (2.0 * sq(a) * a - 9.0 * a * b + 27.0 * c) * inv_54;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double R_2 = sq(R), Q_3 = sq(Q) * Q;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (R_2 < Q_3) {
Shinya Kitaoka 120a6e
    double Q_sqrt = sqrt(Q);
Shinya Kitaoka 120a6e
    double theta  = acos(R / (Q * Q_sqrt));
Shinya Kitaoka 120a6e
    sol.push_back(-2 * Q_sqrt * cos(theta * inv_3) - a * inv_3);
Shinya Kitaoka 120a6e
    sol.push_back(-2 * Q_sqrt * cos((theta - M_2PI) * inv_3) - a * inv_3);
Shinya Kitaoka 120a6e
    sol.push_back(-2 * Q_sqrt * cos((theta + M_2PI) * inv_3) - a * inv_3);
Shinya Kitaoka 120a6e
    std::sort(sol.begin(), sol.end());
Shinya Kitaoka 120a6e
    return 3;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double A = -tsign(R) * pow((fabs(R) + sqrt(R_2 - Q_3)), inv_3);
Shinya Kitaoka 120a6e
  double B = A != 0 ? Q / A : 0;
Shinya Kitaoka 120a6e
  sol.push_back((A + B) - a * inv_3);
Shinya Kitaoka 120a6e
  return 1;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
int rootForGreaterThanThreeEquation(const std::vector<double> &p,</double>
Shinya Kitaoka 120a6e
                                    std::vector<double> &sol) {</double>
Shinya Kitaoka 120a6e
  poly sseq[MAX_ORDER];
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  convert(p, sseq[0]);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  int np = buildsturm(p.size() - 1, sseq);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  int atmin, atmax;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  int nroot = numroots(np, sseq, atmin, atmax);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (nroot == 0) return 0;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  double minVal = -1.0;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  UINT i = 0;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  int nchanges = numchanges(np, sseq, minVal);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  for (i = 0; nchanges != atmin && i != (UINT)MAXPOW; ++i) {
Shinya Kitaoka 120a6e
    minVal *= 10.0;
Shinya Kitaoka 120a6e
    nchanges = numchanges(np, sseq, minVal);
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (nchanges != atmin) {
Shinya Kitaoka 120a6e
    atmin = nchanges;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  double maxVal = 1.0;
Shinya Kitaoka 120a6e
  nchanges      = numchanges(np, sseq, maxVal);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  for (i = 0; nchanges != atmax && i != (UINT)MAXPOW; ++i) {
Shinya Kitaoka 120a6e
    maxVal *= 10.0;
Shinya Kitaoka 120a6e
    nchanges = numchanges(np, sseq, maxVal);
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (nchanges != atmax) {
Shinya Kitaoka 120a6e
    atmax = nchanges;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  nroot = atmin - atmax;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  assert(nroot > 0);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  poly outPoly;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  outPoly.ord = nroot;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  sbisect(np, sseq, minVal, maxVal, atmin, atmax, outPoly.coef);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  convert(outPoly, sol);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return nroot < 0 ? -1 : nroot;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
}  // end of unnamed namespace
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void tLUDecomposition(double *a, int n, int *indx, double &d) {
Shinya Kitaoka 120a6e
  int i, imax, j, k;
Shinya Kitaoka 120a6e
  double big, dum, sum, temp;
Shinya Kitaoka 120a6e
  std::vector<double> vv(n);</double>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  d = 1.0;
Shinya Kitaoka 120a6e
  for (i = 1; i <= n; i++) {
Shinya Kitaoka 120a6e
    big = 0.0;
Shinya Kitaoka 120a6e
    for (j = 1; j <= n; j++)
Shinya Kitaoka 120a6e
      if ((temp = fabs(a[getEl(i, j, n)])) > big) big = temp;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (big == 0.0)
Shinya Kitaoka 120a6e
      throw TMathException("Singular matrix in routine tLUDecomposition\n");
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    vv[i - 1] = 1.0 / big;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (j = 1; j <= n; j++) {
Shinya Kitaoka 120a6e
    for (i = 1; i < j; i++) {
Shinya Kitaoka 120a6e
      sum = a[getEl(i, j, n)];
Shinya Kitaoka 120a6e
      for (k = 1; k < i; k++) sum -= a[getEl(i, k, n)] * a[getEl(k, j, n)];
Shinya Kitaoka 120a6e
      a[getEl(i, j, n)] = sum;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    big = 0.0;
Shinya Kitaoka 120a6e
    for (i = j; i <= n; i++) {
Shinya Kitaoka 120a6e
      sum = a[getEl(i, j, n)];
Shinya Kitaoka 120a6e
      for (k = 1; k < j; k++) sum -= a[getEl(i, k, n)] * a[getEl(k, j, n)];
Shinya Kitaoka 120a6e
      a[getEl(i, j, n)] = sum;
Shinya Kitaoka 120a6e
      if ((dum = vv[i - 1] * fabs(sum)) >= big) {
Shinya Kitaoka 120a6e
        big  = dum;
Shinya Kitaoka 120a6e
        imax = i;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (j != imax) {
Shinya Kitaoka 120a6e
      for (k = 1; k <= n; k++) {
Shinya Kitaoka 120a6e
        dum = a[getEl(imax, k, n)];
Shinya Kitaoka 120a6e
        a[getEl(imax, k, n)] = a[getEl(j, k, n)];
Shinya Kitaoka 120a6e
        a[getEl(j, k, n)]    = dum;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      d            = -(d);
Shinya Kitaoka 120a6e
      vv[imax - 1] = vv[j - 1];
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    indx[j - 1] = imax;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (a[getEl(j, j, n)] == 0.0) a[getEl(j, j, n)] = TConsts::epsilon;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (j != n) {
Shinya Kitaoka 120a6e
      dum = 1.0 / (a[getEl(j, j, n)]);
Shinya Kitaoka 120a6e
      for (i = j + 1; i <= n; i++) a[getEl(i, j, n)] *= dum;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void tbackSubstitution(double *a, int n, int *indx, double *b) {
Shinya Kitaoka 120a6e
  int i, ii = 0, ip, j;
Shinya Kitaoka 120a6e
  double sum;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = 1; i <= n; i++) {
Shinya Kitaoka 120a6e
    ip        = indx[i - 1];
Shinya Kitaoka 120a6e
    sum       = b[ip - 1];
Shinya Kitaoka 120a6e
    b[ip - 1] = b[i - 1];
Shinya Kitaoka 120a6e
    if (ii)
Shinya Kitaoka 120a6e
      for (j = ii; j <= i - 1; j++) sum -= a[getEl(i, j, n)] * b[j - 1];
Shinya Kitaoka 120a6e
    else if (sum)
Shinya Kitaoka 120a6e
      ii     = i;
Shinya Kitaoka 120a6e
    b[i - 1] = sum;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  for (i = n; i >= 1; i--) {
Shinya Kitaoka 120a6e
    sum = b[i - 1];
Shinya Kitaoka 120a6e
    for (j   = i + 1; j <= n; j++) sum -= a[getEl(i, j, n)] * b[j - 1];
Shinya Kitaoka 120a6e
    b[i - 1] = sum / a[getEl(i, i, n)];
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double tdet(double *LUa, int n, double d) {
Shinya Kitaoka 120a6e
  for (int i = 1; i <= n; ++i) d *= LUa[getEl(i, i, n)];
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return d;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double tdet(double *a, int n) {
Shinya Kitaoka 120a6e
  double d;
Shinya Kitaoka 120a6e
  std::vector<int> indx(n);</int>
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  tLUDecomposition(a, n, &indx[0], d);
Shinya Kitaoka 120a6e
  for (int i = 1; i <= n; ++i) d *= a[getEl(i, i, n)];
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return d;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void tsolveSistem(double *a, int n, double *res) {
Shinya Kitaoka 120a6e
  double d;
Shinya Kitaoka 120a6e
  std::vector<int> indx(n);</int>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  tLUDecomposition(a, n, &indx[0], d);
Shinya Kitaoka 120a6e
  assert(tdet(a, n, d) != 0);
Shinya Kitaoka 120a6e
  /*
Shinya Kitaoka 120a6e
if( isAlmostZero(tdet(a, n, d)) )
Shinya Kitaoka 120a6e
throw TMathException("Singular matrix in routine tLUDecomposition\n");
Shinya Kitaoka 120a6e
*/
Shinya Kitaoka 120a6e
  tbackSubstitution(a, n, &indx[0], res);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
int rootFinding(const std::vector<double> &in_poly, std::vector<double> &sol) {</double></double>
Shinya Kitaoka 120a6e
  // per ora risolvo solo i polinomi di grado al piu' pari a 3
Shinya Kitaoka 120a6e
  assert((int)in_poly.size() <= MAX_ORDER);
Shinya Kitaoka 120a6e
  if (in_poly.empty() || (int)in_poly.size() > MAX_ORDER) return -1;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  std::vector<double> p;</double>
Shinya Kitaoka 120a6e
  std::copy(in_poly.begin(), in_poly.end(), std::back_inserter(p));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // eat zero in poly
Shinya Kitaoka 120a6e
  while (!p.empty() && isAlmostZero(p.back())) p.pop_back();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  sol.clear();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  while (!p.empty() && p.front() == 0) {
Shinya Kitaoka 120a6e
    sol.push_back(0.0);
Shinya Kitaoka 120a6e
    p.erase(p.begin());  // se i coefficienti bassi sono zero, ci sono soluzioni
Shinya Kitaoka 120a6e
                         // pari a 0.0. le metto,
Shinya Kitaoka 38fd86
  }                      // e abbasso il grado del polinomio(piu' veloce)
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  switch (p.size()) {
Shinya Kitaoka 120a6e
  case 0:
Shinya Kitaoka 120a6e
    if (sol.empty()) return INFINITE_SOLUTIONS;
Shinya Kitaoka 120a6e
    break;
Shinya Kitaoka 120a6e
  case 1:  // no solutions
Shinya Kitaoka 120a6e
    break;
Shinya Kitaoka 120a6e
  case 2:
Shinya Kitaoka 120a6e
    sol.push_back(-p[0] / p[1]);
Shinya Kitaoka 120a6e
    break;
Shinya Kitaoka 120a6e
  case 3:
Shinya Kitaoka 120a6e
    rootForQuadraticEquation(p, sol);
Shinya Kitaoka 120a6e
    break;
Shinya Kitaoka 120a6e
  case 4:
Shinya Kitaoka 120a6e
    rootForCubicEquation(p, sol);
Shinya Kitaoka 120a6e
    break;
Shinya Kitaoka 120a6e
  default:
Shinya Kitaoka 120a6e
    rootForGreaterThanThreeEquation(p, sol);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  return sol.size();
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
f278a5
int solveEquation2(std::complex<double> *roots, double a, double b, double c) {</double>
f278a5
  if (isAlmostZero(a)) {
f278a5
    if (isAlmostZero(b)) return 0;
f278a5
    roots[0] = -c/b;
f278a5
    return 1;
f278a5
  }
f278a5
f278a5
  double D = b*b - a*c*4;
f278a5
  double k = 0.5/a;
f278a5
  std::complex<double> d = D < 0</double>
f278a5
                         ? std::complex<double>(0, sqrt(-D))</double>
f278a5
                         : std::complex<double>(sqrt(D));</double>
f278a5
  roots[0] = (-b - d)*k;
f278a5
  roots[1] = (-b + d)*k;
f278a5
  return 2;
f278a5
}
f278a5
f278a5
//-----------------------------------------------------------------------------
f278a5
f278a5
int solveEquation3(std::complex<double> *roots, double a, double b, double c, double d) {</double>
f278a5
  if (isAlmostZero(a))
f278a5
    return solveEquation2(roots, b, c, d);
f278a5
f278a5
  // x = y - b/(3*a)
f278a5
  // y*y*y + p*y + q = 0
f278a5
  double p = (3*a*c - b*b)/(3*a*a);
f278a5
  double q = (27*a*a*d - 9*a*b*c + 2*b*b*b)/(27*a*a*a);
f278a5
f278a5
  double Q = p*p*p/27 + q*q/4;
f278a5
  std::complex<double> Qs = Q < 0</double>
f278a5
                          ? std::complex<double>(0, sqrt(-Q))</double>
f278a5
                          : std::complex<double>(sqrt(Q));</double>
f278a5
  std::complex<double> A = pow(-q/2 + Qs, 1.0/3);</double>
f278a5
  std::complex<double> B = pow(-q/2 - Qs, 1.0/3);</double>
f278a5
f278a5
  // choose complimentary B for A (A*B must be equal -p/3)
f278a5
  std::complex<double> rot(-0.5, sqrt(3.0)/2);</double>
f278a5
  if (!isAlmostZero(A*B + p/3)) B *= rot;
f278a5
  if (!isAlmostZero(A*B + p/3)) B *= rot;
f278a5
f278a5
  std::complex<double> Y = (A - B)*std::complex<double>(0, sqrt(3.0));</double></double>
f278a5
  std::complex<double> y0 = A + B;</double>
f278a5
  std::complex<double> y1 = (-y0 - Y)/2.0;</double>
f278a5
  std::complex<double> y2 = (-y0 + Y)/2.0;</double>
f278a5
f278a5
  double dd = b/(3*a);
f278a5
  roots[0] = y0 - dd;
f278a5
  roots[1] = y1 - dd;
f278a5
  roots[2] = y2 - dd;
f278a5
  return 3;
f278a5
}
f278a5
f278a5
//-----------------------------------------------------------------------------
f278a5
f278a5
int solveEquation4(std::complex<double> *roots, double a, double b, double c, double d, double e) {</double>
f278a5
  if (isAlmostZero(a))
f278a5
    return solveEquation3(roots, b, c, d, e);
f278a5
f278a5
  // x = y - b/(4*a)
f278a5
  // y^4 + p*y^2 + q*y + r = 0
f278a5
  double dd = b/(4*a);
f278a5
  double p = (8*a*c - 3*b*b)/(8*a*a);
f278a5
  double q = (8*a*a*d - 4*a*b*c + b*b*b)/(8*a*a*a);
f278a5
  double r = (256*a*a*a*e - 64*a*a*b*d + 16*a*b*b*c - 3*b*b*b*b)/(256*a*a*a*a);
f278a5
f278a5
  if (isAlmostZero(q)) {
f278a5
    // biquadratic equation
f278a5
    // y^4 + p*y^2 + r = 0
f278a5
    // handling this special case will give us more accurate results
f278a5
    std::complex<double> y[2];</double>
f278a5
    solveEquation2(y, 1, p, r);
f278a5
    y[0] = sqrt(y[0]);
f278a5
    y[1] = sqrt(y[1]);
f278a5
    roots[0] = -y[0] - dd;
f278a5
    roots[1] =  y[0] - dd;
f278a5
    roots[2] = -y[1] - dd;
f278a5
    roots[3] =  y[1] - dd;
f278a5
    return 4;
f278a5
  }
f278a5
f278a5
  // solve cubic equation
f278a5
  // z*z*z + (p/2)*z*z + ((p*p - 4*r)/16)*z - q*q/64 = 0
f278a5
  double pp = p/2;
f278a5
  double qq = (p*p - 4*r)/16;
f278a5
  double rr = -q*q/64;
f278a5
  std::complex<double> z[3];</double>
f278a5
  solveEquation3(z, 1, pp, qq, rr);
f278a5
f278a5
  z[0] = sqrt(z[0]);
f278a5
  z[1] = sqrt(z[1]);
f278a5
  z[2] = sqrt(z[2]);
f278a5
f278a5
  // we need to find signs combination where following is valid:
f278a5
  // (+-z0)*(+-z1)*(+-z2) = -q/8
f278a5
  // magnitudes are always equals, we need to check signs only
f278a5
  std::complex<double> zzz = z[0]*z[1]*z[2];</double>
f278a5
  if ((zzz.real() > 0) == (q > 0))
f278a5
    z[0] = -z[0];
f278a5
f278a5
  roots[0] =  z[0] - z[1] - z[2] - dd;
f278a5
  roots[1] = -z[0] + z[1] - z[2] - dd;
f278a5
  roots[2] = -z[0] - z[1] + z[2] - dd;
f278a5
  roots[3] =  z[0] + z[1] + z[2] - dd;
f278a5
  return 4;
f278a5
}
f278a5
f278a5
//-----------------------------------------------------------------------------
f278a5
Toshihiro Shimizu 890ddd
/*
Toshihiro Shimizu 890ddd
*/
Shinya Kitaoka 120a6e
int numberOfRootsInInterval(int order, const double *polyH, double min,
Shinya Kitaoka 120a6e
                            double max) {
Shinya Kitaoka 120a6e
  poly sseq[MAX_ORDER];
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  int i, nchanges_0, nchanges_1, np;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  if (order > MAX_ORDER) return -1;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  while (polyH[order] == 0.0 && order > 0) --order;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // init a sturm's sequence with our polynomious
Shinya Kitaoka 120a6e
  for (i = order; i >= 0; --i) sseq[0].coef[i] = polyH[i];
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  //  build the Sturm sequence
Shinya Kitaoka 120a6e
  np = buildsturm(order, sseq);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // compute number of variation on 0.0
Shinya Kitaoka 120a6e
  nchanges_0 = numchanges(np, sseq, min);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  nchanges_1 = numchanges(np, sseq, max);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return (nchanges_0 - nchanges_1);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double quadraticRoot(double a, double b, double c) {
Shinya Kitaoka 120a6e
  double bb, q;
Shinya Kitaoka 120a6e
  // caso lineare
Shinya Kitaoka 120a6e
  if (fabs(a) < epsilon) {
Shinya Kitaoka 120a6e
    if (fabs(b) >= epsilon) return -c / b;
Shinya Kitaoka 120a6e
    return 1;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  bb = b * b;
Shinya Kitaoka 120a6e
  q  = bb - 4.0 * a * c;
Shinya Kitaoka 120a6e
  if (q < 0.0) return 1;
Shinya Kitaoka 120a6e
  q                             = sqrt(q);
Shinya Kitaoka 120a6e
  if (b < 0.0) q                = -q;
Shinya Kitaoka 120a6e
  q                             = -0.5 * (b + q);
Shinya Kitaoka 120a6e
  double root1                  = -1;
Shinya Kitaoka 120a6e
  double root2                  = -1;
Shinya Kitaoka 120a6e
  if (fabs(q) >= epsilon) root1 = c / q;
Shinya Kitaoka 120a6e
  if (fabs(a) >= epsilon) root2 = q / a;
Shinya Kitaoka 120a6e
  if (0.0 - epsilon <= root1 && root1 <= 1.0 + epsilon) return root1;
Shinya Kitaoka 120a6e
  if (0.0 - epsilon <= root2 && root2 <= 1.0 + epsilon) return root2;
Shinya Kitaoka 120a6e
  return 1;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double cubicRoot(double a, double b, double c, double d) {
Shinya Kitaoka 120a6e
  double A, Q, R, QQQ, RR;
Shinya Kitaoka 120a6e
  double theta;
Shinya Kitaoka 120a6e
  /* Test for a quadratic or linear degeneracy			*/
Shinya Kitaoka 120a6e
  if (fabs(a) < epsilon) return quadraticRoot(b, c, d);
Shinya Kitaoka 120a6e
  /* Normalize							*/
Shinya Kitaoka 120a6e
  b /= a;
Shinya Kitaoka 120a6e
  c /= a;
Shinya Kitaoka 120a6e
  d /= a;
Shinya Kitaoka 120a6e
  a = 1.0;
Shinya Kitaoka 120a6e
  /* Compute discriminants						*/
Shinya Kitaoka 120a6e
  Q   = (b * b - 3.0 * c) / 9.0;
Shinya Kitaoka 120a6e
  QQQ = Q * Q * Q;
Shinya Kitaoka 120a6e
  R   = (2.0 * b * b * b - 9.0 * b * c + 27.0 * d) / 54.0;
Shinya Kitaoka 120a6e
  RR  = R * R;
Shinya Kitaoka 120a6e
  /* Three real roots							*/
Shinya Kitaoka 120a6e
  if (RR < QQQ) {
Shinya Kitaoka 120a6e
    theta = acos(R / sqrt(QQQ));
Shinya Kitaoka 120a6e
    double root[3];
Shinya Kitaoka 120a6e
    root[0] = root[1] = root[2] = -2.0 * sqrt(Q);
Shinya Kitaoka 120a6e
    root[0] *= cos(theta / 3.0);
Shinya Kitaoka 120a6e
    root[1] *= cos((theta + M_2PI) / 3.0);
Shinya Kitaoka 120a6e
    root[2] *= cos((theta - M_2PI) / 3.0);
Shinya Kitaoka 120a6e
    root[0] -= b / 3.0;
Shinya Kitaoka 120a6e
    root[1] -= b / 3.0;
Shinya Kitaoka 120a6e
    root[2] -= b / 3.0;
Shinya Kitaoka 120a6e
    if (0.0 - epsilon < root[0] && root[0] < 1.0 + epsilon) return root[0];
Shinya Kitaoka 120a6e
    if (0.0 - epsilon < root[1] && root[1] < 1.0 + epsilon) return root[1];
Shinya Kitaoka 120a6e
    if (0.0 - epsilon < root[2] && root[2] < 1.0 + epsilon) return root[2];
Shinya Kitaoka 120a6e
    return 1;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  /* One real root							*/
Shinya Kitaoka 120a6e
  else {
Shinya Kitaoka 120a6e
    double root = 0;
Shinya Kitaoka 120a6e
    A           = -pow(fabs(R) + sqrt(RR - QQQ), 1.0 / 3.0);
Shinya Kitaoka 120a6e
    if (A != 0.0) {
Shinya Kitaoka 120a6e
      if (R < 0.0) A = -A;
Shinya Kitaoka 120a6e
      root           = A + Q / A;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
    root -= b / 3.0;
Shinya Kitaoka 120a6e
    if (0.0 - epsilon < root && root < 1.0 + epsilon) return root;
Shinya Kitaoka 120a6e
    return 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
//  End Of File
Toshihiro Shimizu 890ddd
//-----------------------------------------------------------------------------