|
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 |
3bfa54 |
TMathException::TMathException(std::string msg)
|
|
Toshihiro Shimizu |
890ddd |
: m_msg(toWideString(msg))
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
namespace
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
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 |
|
|
Toshihiro Shimizu |
890ddd |
//!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 |
|
|
Toshihiro Shimizu |
890ddd |
inline int getEl(int i, int j, int n)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
return (j - 1) + (i - 1) * n;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
//-------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//! structure type for representing a polynomial
|
|
Toshihiro Shimizu |
890ddd |
typedef struct
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int ord;
|
|
MichaĆ Janiszewski |
b82824 |
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 |
3bfa54 |
void convert(const std::vector<double> &v, poly &p)</double>
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
assert((int)v.size() <= MAX_ORDER);
|
|
Toshihiro Shimizu |
890ddd |
if ((int)v.size() > MAX_ORDER)
|
|
Toshihiro Shimizu |
890ddd |
return;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
p.ord = v.size() - 1;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
std::copy(v.begin(), v.end(), p.coef);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
//-------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Shinya Kitaoka |
3bfa54 |
void convert(const poly &p, std::vector<double> &v)</double>
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
v.resize(p.ord);
|
|
Toshihiro Shimizu |
890ddd |
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.
|
|
Toshihiro Shimizu |
890ddd |
* note: this function assumes the leading coefficient of v
|
|
Toshihiro Shimizu |
890ddd |
* is 1 or -1
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
int modp(poly *u, poly *v, poly *r)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int k, j;
|
|
Toshihiro Shimizu |
890ddd |
double *nr, *end, *uc;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
nr = r->coef;
|
|
Toshihiro Shimizu |
890ddd |
end = &u->coef[u->ord];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
uc = u->coef;
|
|
Toshihiro Shimizu |
890ddd |
while (uc <= end)
|
|
Toshihiro Shimizu |
890ddd |
*nr++ = *uc++;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (v->coef[v->ord] < 0.0) {
|
|
Toshihiro Shimizu |
890ddd |
for (k = u->ord - v->ord - 1; k >= 0; k -= 2)
|
|
Toshihiro Shimizu |
890ddd |
r->coef[k] = -r->coef[k];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (k = u->ord - v->ord; k >= 0; k--)
|
|
Toshihiro Shimizu |
890ddd |
for (j = v->ord + k - 1; j >= k; j--)
|
|
Toshihiro Shimizu |
890ddd |
r->coef[j] = -r->coef[j] - r->coef[v->ord + k] * v->coef[j - k];
|
|
Toshihiro Shimizu |
890ddd |
} else {
|
|
Toshihiro Shimizu |
890ddd |
for (k = u->ord - v->ord; k >= 0; k--)
|
|
Toshihiro Shimizu |
890ddd |
for (j = v->ord + k - 1; j >= k; j--)
|
|
Toshihiro Shimizu |
890ddd |
r->coef[j] -= r->coef[v->ord + k] * v->coef[j - k];
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
k = v->ord - 1;
|
|
Toshihiro Shimizu |
890ddd |
while (k >= 0 && fabs(r->coef[k]) < SMALL_ENOUGH) {
|
|
Toshihiro Shimizu |
890ddd |
r->coef[k] = 0.0;
|
|
Toshihiro Shimizu |
890ddd |
k--;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
r->ord = (k < 0) ? 0 : k;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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 |
*/
|
|
Toshihiro Shimizu |
890ddd |
int buildsturm(int ord, poly *sseq)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int i;
|
|
Toshihiro Shimizu |
890ddd |
double f, *fp, *fc;
|
|
Toshihiro Shimizu |
890ddd |
poly *sp;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
sseq[0].ord = ord;
|
|
Toshihiro Shimizu |
890ddd |
sseq[1].ord = ord - 1;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// calculate the derivative and normalise the leadingcoefficient.
|
|
Toshihiro Shimizu |
890ddd |
f = fabs(sseq[0].coef[ord] * ord);
|
|
Toshihiro Shimizu |
890ddd |
fp = sseq[1].coef;
|
|
Toshihiro Shimizu |
890ddd |
fc = sseq[0].coef + 1;
|
|
Toshihiro Shimizu |
890ddd |
for (i = 1; i <= ord; i++)
|
|
Toshihiro Shimizu |
890ddd |
*fp++ = *fc++ * i / f;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// construct the rest of the Sturm sequence
|
|
Toshihiro Shimizu |
890ddd |
for (sp = sseq + 2; modp(sp - 2, sp - 1, sp); sp++) {
|
|
Toshihiro Shimizu |
890ddd |
// reverse the sign and normalise
|
|
Toshihiro Shimizu |
890ddd |
f = -fabs(sp->coef[sp->ord]);
|
|
Toshihiro Shimizu |
890ddd |
for (fp = &sp->coef[sp->ord]; fp >= sp->coef; fp--)
|
|
Toshihiro Shimizu |
890ddd |
*fp /= f;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
sp->coef[0] = -sp->coef[0]; // reverse the sign
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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 |
*/
|
|
Toshihiro Shimizu |
890ddd |
int numroots(int np, poly *sseq, int &atneg, int &atpos)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int
|
|
Toshihiro Shimizu |
890ddd |
atposinf = 0,
|
|
Toshihiro Shimizu |
890ddd |
atneginf = 0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
poly *s;
|
|
Toshihiro Shimizu |
890ddd |
double f, lf;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// change at positive infinity
|
|
Toshihiro Shimizu |
890ddd |
lf = sseq[0].coef[sseq[0].ord];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (s = sseq + 1; s <= sseq + np; ++s) {
|
|
Toshihiro Shimizu |
890ddd |
f = s->coef[s->ord];
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 == lf || lf * f < 0.0)
|
|
Toshihiro Shimizu |
890ddd |
++atposinf;
|
|
Toshihiro Shimizu |
890ddd |
lf = f;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// change at negative infinity
|
|
Toshihiro Shimizu |
890ddd |
if (sseq[0].ord & 1)
|
|
Toshihiro Shimizu |
890ddd |
lf = -sseq[0].coef[sseq[0].ord];
|
|
Toshihiro Shimizu |
890ddd |
else
|
|
Toshihiro Shimizu |
890ddd |
lf = sseq[0].coef[sseq[0].ord];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (s = sseq + 1; s <= sseq + np; ++s) {
|
|
Toshihiro Shimizu |
890ddd |
if (s->ord & 1)
|
|
Toshihiro Shimizu |
890ddd |
f = -s->coef[s->ord];
|
|
Toshihiro Shimizu |
890ddd |
else
|
|
Toshihiro Shimizu |
890ddd |
f = s->coef[s->ord];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 == lf || lf * f < 0.0)
|
|
Toshihiro Shimizu |
890ddd |
++atneginf;
|
|
Toshihiro Shimizu |
890ddd |
lf = f;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
atneg = atneginf;
|
|
Toshihiro Shimizu |
890ddd |
atpos = atposinf;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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 |
*/
|
|
Toshihiro Shimizu |
890ddd |
int numchanges(int np, poly *sseq, double a)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int changes;
|
|
Toshihiro Shimizu |
890ddd |
double f, lf;
|
|
Toshihiro Shimizu |
890ddd |
poly *s;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
changes = 0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
lf = evalpoly(sseq[0].ord, sseq[0].coef, a);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (s = sseq + 1; s <= sseq + np; s++) {
|
|
Toshihiro Shimizu |
890ddd |
f = evalpoly(s->ord, s->coef, a);
|
|
Toshihiro Shimizu |
890ddd |
if (lf == 0.0 || lf * f < 0)
|
|
Toshihiro Shimizu |
890ddd |
changes++;
|
|
Toshihiro Shimizu |
890ddd |
lf = f;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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 |
*/
|
|
Toshihiro Shimizu |
890ddd |
void sbisect(int np,
|
|
Toshihiro Shimizu |
890ddd |
poly *sseq,
|
|
Toshihiro Shimizu |
890ddd |
double min,
|
|
Toshihiro Shimizu |
890ddd |
double max,
|
|
Toshihiro Shimizu |
890ddd |
int atmin,
|
|
Toshihiro Shimizu |
890ddd |
int atmax,
|
|
Toshihiro Shimizu |
890ddd |
double *roots)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double mid;
|
|
Toshihiro Shimizu |
890ddd |
int n1 = 0, n2 = 0, its, atmid;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if ((atmin - atmax) == 1) {
|
|
Toshihiro Shimizu |
890ddd |
//first try a less expensive technique.
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (modrf(sseq->ord, sseq->coef, min, max, &roots[0]))
|
|
Toshihiro Shimizu |
890ddd |
return;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
* if we get here we have to evaluate the root the hard
|
|
Toshihiro Shimizu |
890ddd |
* way by using the Sturm sequence.
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
for (its = 0; its < MAXIT; its++) {
|
|
Toshihiro Shimizu |
890ddd |
mid = (min + max) / 2;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
atmid = numchanges(np, sseq, mid);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(mid) > RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
if (fabs((max - min) / mid) < RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
roots[0] = mid;
|
|
Toshihiro Shimizu |
890ddd |
return;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
} else if (fabs(max - min) < RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
roots[0] = mid;
|
|
Toshihiro Shimizu |
890ddd |
return;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if ((atmin - atmid) == 0)
|
|
Toshihiro Shimizu |
890ddd |
min = mid;
|
|
Toshihiro Shimizu |
890ddd |
else
|
|
Toshihiro Shimizu |
890ddd |
max = mid;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (its == MAXIT) {
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
fprintf(stderr, "sbisect: overflow min %f max %f\
|
|
Toshihiro Shimizu |
890ddd |
diff %e nroot %d n1 %d n2 %d\n",
|
|
Toshihiro Shimizu |
890ddd |
min, max, max - min, nroot, n1, n2);
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
roots[0] = mid;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
return;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
* more than one root in the interval, we have to bisect...
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
for (its = 0; its < MAXIT; its++) {
|
|
Toshihiro Shimizu |
890ddd |
mid = (min + max) / 2;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
atmid = numchanges(np, sseq, mid);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
n1 = atmin - atmid;
|
|
Toshihiro Shimizu |
890ddd |
n2 = atmid - atmax;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (n1 != 0 && n2 != 0) {
|
|
Toshihiro Shimizu |
890ddd |
sbisect(np, sseq, min, mid, atmin, atmid, roots);
|
|
Toshihiro Shimizu |
890ddd |
sbisect(np, sseq, mid, max, atmid, atmax, &roots[n1]);
|
|
Toshihiro Shimizu |
890ddd |
break;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (n1 == 0)
|
|
Toshihiro Shimizu |
890ddd |
min = mid;
|
|
Toshihiro Shimizu |
890ddd |
else
|
|
Toshihiro Shimizu |
890ddd |
max = mid;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (its == MAXIT) {
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
fprintf(stderr, "sbisect: roots too close together\n");
|
|
Toshihiro Shimizu |
890ddd |
fprintf(stderr, "sbisect: overflow min %f max %f diff %e\
|
|
Toshihiro Shimizu |
890ddd |
nroot %d n1 %d n2 %d\n",
|
|
Toshihiro Shimizu |
890ddd |
min, max, max - min, nroot, n1, n2);
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
for (n1 = atmax; n1 < atmin; n1++)
|
|
Toshihiro Shimizu |
890ddd |
roots[n1 - atmax] = mid;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
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 |
*/
|
|
Toshihiro Shimizu |
890ddd |
double evalpoly(int ord, double *coef, double x)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double *fp, f;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
fp = &coef[ord];
|
|
Toshihiro Shimizu |
890ddd |
f = *fp;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (fp--; fp >= coef; fp--)
|
|
Toshihiro Shimizu |
890ddd |
f = x * f + *fp;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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 |
*/
|
|
Toshihiro Shimizu |
890ddd |
int modrf(int ord, double *coef, double a, double b, double *val)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int its;
|
|
Toshihiro Shimizu |
890ddd |
double fa, fb, x, fx, lfx;
|
|
Toshihiro Shimizu |
890ddd |
double *fp, *scoef, *ecoef;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
scoef = coef;
|
|
Toshihiro Shimizu |
890ddd |
ecoef = &coef[ord];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
fb = fa = *ecoef;
|
|
Toshihiro Shimizu |
890ddd |
for (fp = ecoef - 1; fp >= scoef; fp--) {
|
|
Toshihiro Shimizu |
890ddd |
fa = a * fa + *fp;
|
|
Toshihiro Shimizu |
890ddd |
fb = b * fb + *fp;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// if there is no sign difference the method won't work
|
|
Toshihiro Shimizu |
890ddd |
if (fa * fb > 0.0)
|
|
Toshihiro Shimizu |
890ddd |
return (0);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(fa) < RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
*val = a;
|
|
Toshihiro Shimizu |
890ddd |
return (1);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(fb) < RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
*val = b;
|
|
Toshihiro Shimizu |
890ddd |
return (1);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
lfx = fa;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (its = 0; its < MAXIT; its++) {
|
|
Toshihiro Shimizu |
890ddd |
x = (fb * a - fa * b) / (fb - fa);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
fx = *ecoef;
|
|
Toshihiro Shimizu |
890ddd |
for (fp = ecoef - 1; fp >= scoef; fp--)
|
|
Toshihiro Shimizu |
890ddd |
fx = x * fx + *fp;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(x) > RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(fx / x) < RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
*val = x;
|
|
Toshihiro Shimizu |
890ddd |
return (1);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
} else if (fabs(fx) < RELERROR) {
|
|
Toshihiro Shimizu |
890ddd |
*val = x;
|
|
Toshihiro Shimizu |
890ddd |
return (1);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if ((fa * fx) < 0) {
|
|
Toshihiro Shimizu |
890ddd |
b = x;
|
|
Toshihiro Shimizu |
890ddd |
fb = fx;
|
|
Toshihiro Shimizu |
890ddd |
if ((lfx * fx) > 0)
|
|
Toshihiro Shimizu |
890ddd |
fa /= 2;
|
|
Toshihiro Shimizu |
890ddd |
} else {
|
|
Toshihiro Shimizu |
890ddd |
a = x;
|
|
Toshihiro Shimizu |
890ddd |
fa = fx;
|
|
Toshihiro Shimizu |
890ddd |
if ((lfx * fx) > 0)
|
|
Toshihiro Shimizu |
890ddd |
fb /= 2;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
lfx = fx;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// fprintf(stderr, "modrf overflow %f %f %f\n", a, b, fx);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
Remark:
|
|
Toshihiro Shimizu |
890ddd |
poly[0] = c
|
|
Toshihiro Shimizu |
890ddd |
poly[1] = b
|
|
Toshihiro Shimizu |
890ddd |
poly[2] = a
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Shinya Kitaoka |
3bfa54 |
int rootForQuadraticEquation(const std::vector<double> &v, std::vector<double> &sol)</double></double>
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double q, delta;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
if( isAlmostZero(v[2]))
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
if ( isAlmostZero(v[1]) )
|
|
Toshihiro Shimizu |
890ddd |
return -1;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(-v[0]/v[1]);
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (isAlmostZero(v[1])) {
|
|
Toshihiro Shimizu |
890ddd |
q = -v[0] / v[2];
|
|
Toshihiro Shimizu |
890ddd |
if (q < 0)
|
|
Toshihiro Shimizu |
890ddd |
return 0;
|
|
Toshihiro Shimizu |
890ddd |
else if (isAlmostZero(q)) {
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(0);
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
q = sqrt(q);
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(-q);
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(q);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
return 2;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
delta = sq(v[1]) - 4.0 * v[0] * v[2];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (delta < 0.0)
|
|
Toshihiro Shimizu |
890ddd |
return 0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
assert(v[2] != 0);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (isAlmostZero(delta)) {
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(-v[1] / (v[2] * 2.0));
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
q = -0.5 * (v[1] + tsign(v[1]) * sqrt(delta));
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
assert(q != 0);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(v[0] / q);
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(q / v[2]);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
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
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
Remark:
|
|
Toshihiro Shimizu |
890ddd |
poly[0] = d
|
|
Toshihiro Shimizu |
890ddd |
poly[1] = c
|
|
Toshihiro Shimizu |
890ddd |
poly[2] = b
|
|
Toshihiro Shimizu |
890ddd |
poly[3] = a
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Shinya Kitaoka |
3bfa54 |
int rootForCubicEquation(const std::vector<double> &p, std::vector<double> &sol)</double></double>
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
if( isAlmostZero(p[3]) )
|
|
Toshihiro Shimizu |
890ddd |
return rootForQuadraticEquation(p,sol);
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (isAlmostZero(p[0])) {
|
|
Toshihiro Shimizu |
890ddd |
int numberOfSol;
|
|
Shinya Kitaoka |
3bfa54 |
std::vector<double> redPol(3);</double>
|
|
Toshihiro Shimizu |
890ddd |
redPol[0] = p[1];
|
|
Toshihiro Shimizu |
890ddd |
redPol[1] = p[2];
|
|
Toshihiro Shimizu |
890ddd |
redPol[2] = p[3];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
numberOfSol = rootForQuadraticEquation(redPol, sol);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (int i = 0; i < numberOfSol; ++i)
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 == sol[i])
|
|
Toshihiro Shimizu |
890ddd |
return numberOfSol;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// altrimenti devo contare la soluzione nulla
|
|
Toshihiro Shimizu |
890ddd |
++numberOfSol;
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(0);
|
|
Toshihiro Shimizu |
890ddd |
return numberOfSol;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double
|
|
Toshihiro Shimizu |
890ddd |
inv_v3 = 1.0 / p[3],
|
|
Toshihiro Shimizu |
890ddd |
a = p[2] * inv_v3,
|
|
Toshihiro Shimizu |
890ddd |
b = p[1] * inv_v3,
|
|
Toshihiro Shimizu |
890ddd |
c = p[0] * inv_v3;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
static const double inv_3 = 1.0 / 3.0;
|
|
Toshihiro Shimizu |
890ddd |
static const double inv_9 = 1.0 / 9.0;
|
|
Toshihiro Shimizu |
890ddd |
static const double inv_54 = 1.0 / 54.0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double
|
|
Toshihiro Shimizu |
890ddd |
Q = (sq(a) - 3.0 * b) * inv_9,
|
|
Toshihiro Shimizu |
890ddd |
R = (2.0 * sq(a) * a - 9.0 * a * b + 27.0 * c) * inv_54;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double
|
|
Toshihiro Shimizu |
890ddd |
R_2 = sq(R),
|
|
Toshihiro Shimizu |
890ddd |
Q_3 = sq(Q) * Q;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (R_2 < Q_3) {
|
|
Toshihiro Shimizu |
890ddd |
double Q_sqrt = sqrt(Q);
|
|
Toshihiro Shimizu |
890ddd |
double theta = acos(R / (Q * Q_sqrt));
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(-2 * Q_sqrt * cos(theta * inv_3) - a * inv_3);
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(-2 * Q_sqrt * cos((theta - TConsts::pi * 2.0) * inv_3) - a * inv_3);
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(-2 * Q_sqrt * cos((theta + TConsts::pi * 2.0) * inv_3) - a * inv_3);
|
|
Toshihiro Shimizu |
890ddd |
std::sort(sol.begin(), sol.end());
|
|
Toshihiro Shimizu |
890ddd |
return 3;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double A = -tsign(R) * pow((fabs(R) + sqrt(R_2 - Q_3)), inv_3);
|
|
Toshihiro Shimizu |
890ddd |
double B = A != 0 ? Q / A : 0;
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back((A + B) - a * inv_3);
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Shinya Kitaoka |
3bfa54 |
int rootForGreaterThanThreeEquation(const std::vector<double> &p, std::vector<double> &sol)</double></double>
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
poly sseq[MAX_ORDER];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
convert(p, sseq[0]);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
int np = buildsturm(p.size() - 1, sseq);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
int atmin, atmax;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
int nroot = numroots(np, sseq, atmin, atmax);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (nroot == 0)
|
|
Toshihiro Shimizu |
890ddd |
return 0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double minVal = -1.0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
UINT i = 0;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
int nchanges = numchanges(np, sseq, minVal);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (i = 0; nchanges != atmin && i != (UINT)MAXPOW; ++i) {
|
|
Toshihiro Shimizu |
890ddd |
minVal *= 10.0;
|
|
Toshihiro Shimizu |
890ddd |
nchanges = numchanges(np, sseq, minVal);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (nchanges != atmin) {
|
|
Toshihiro Shimizu |
890ddd |
atmin = nchanges;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double maxVal = 1.0;
|
|
Toshihiro Shimizu |
890ddd |
nchanges = numchanges(np, sseq, maxVal);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (i = 0; nchanges != atmax && i != (UINT)MAXPOW; ++i) {
|
|
Toshihiro Shimizu |
890ddd |
maxVal *= 10.0;
|
|
Toshihiro Shimizu |
890ddd |
nchanges = numchanges(np, sseq, maxVal);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (nchanges != atmax) {
|
|
Toshihiro Shimizu |
890ddd |
atmax = nchanges;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
nroot = atmin - atmax;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
assert(nroot > 0);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
poly outPoly;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
outPoly.ord = nroot;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
sbisect(np, sseq, minVal, maxVal, atmin, atmax, outPoly.coef);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
convert(outPoly, sol);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
return nroot < 0 ? -1 : nroot;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
} // end of unnamed namespace
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
void tLUDecomposition(double *a, int n, int *indx, double &d)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int i, imax, j, k;
|
|
Toshihiro Shimizu |
890ddd |
double big, dum, sum, temp;
|
|
Toshihiro Shimizu |
890ddd |
std::vector<double> vv(n);</double>
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
d = 1.0;
|
|
Toshihiro Shimizu |
890ddd |
for (i = 1; i <= n; i++) {
|
|
Toshihiro Shimizu |
890ddd |
big = 0.0;
|
|
Toshihiro Shimizu |
890ddd |
for (j = 1; j <= n; j++)
|
|
Toshihiro Shimizu |
890ddd |
if ((temp = fabs(a[getEl(i, j, n)])) > big)
|
|
Toshihiro Shimizu |
890ddd |
big = temp;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (big == 0.0)
|
|
Toshihiro Shimizu |
890ddd |
throw TMathException("Singular matrix in routine tLUDecomposition\n");
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
vv[i - 1] = 1.0 / big;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (j = 1; j <= n; j++) {
|
|
Toshihiro Shimizu |
890ddd |
for (i = 1; i < j; i++) {
|
|
Toshihiro Shimizu |
890ddd |
sum = a[getEl(i, j, n)];
|
|
Toshihiro Shimizu |
890ddd |
for (k = 1; k < i; k++)
|
|
Toshihiro Shimizu |
890ddd |
sum -= a[getEl(i, k, n)] * a[getEl(k, j, n)];
|
|
Toshihiro Shimizu |
890ddd |
a[getEl(i, j, n)] = sum;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
big = 0.0;
|
|
Toshihiro Shimizu |
890ddd |
for (i = j; i <= n; i++) {
|
|
Toshihiro Shimizu |
890ddd |
sum = a[getEl(i, j, n)];
|
|
Toshihiro Shimizu |
890ddd |
for (k = 1; k < j; k++)
|
|
Toshihiro Shimizu |
890ddd |
sum -= a[getEl(i, k, n)] * a[getEl(k, j, n)];
|
|
Toshihiro Shimizu |
890ddd |
a[getEl(i, j, n)] = sum;
|
|
Toshihiro Shimizu |
890ddd |
if ((dum = vv[i - 1] * fabs(sum)) >= big) {
|
|
Toshihiro Shimizu |
890ddd |
big = dum;
|
|
Toshihiro Shimizu |
890ddd |
imax = i;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (j != imax) {
|
|
Toshihiro Shimizu |
890ddd |
for (k = 1; k <= n; k++) {
|
|
Toshihiro Shimizu |
890ddd |
dum = a[getEl(imax, k, n)];
|
|
Toshihiro Shimizu |
890ddd |
a[getEl(imax, k, n)] = a[getEl(j, k, n)];
|
|
Toshihiro Shimizu |
890ddd |
a[getEl(j, k, n)] = dum;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
d = -(d);
|
|
Toshihiro Shimizu |
890ddd |
vv[imax - 1] = vv[j - 1];
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
indx[j - 1] = imax;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (a[getEl(j, j, n)] == 0.0)
|
|
Toshihiro Shimizu |
890ddd |
a[getEl(j, j, n)] = TConsts::epsilon;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (j != n) {
|
|
Toshihiro Shimizu |
890ddd |
dum = 1.0 / (a[getEl(j, j, n)]);
|
|
Toshihiro Shimizu |
890ddd |
for (i = j + 1; i <= n; i++)
|
|
Toshihiro Shimizu |
890ddd |
a[getEl(i, j, n)] *= dum;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
void tbackSubstitution(double *a, int n, int *indx, double *b)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
int i, ii = 0, ip, j;
|
|
Toshihiro Shimizu |
890ddd |
double sum;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
for (i = 1; i <= n; i++) {
|
|
Toshihiro Shimizu |
890ddd |
ip = indx[i - 1];
|
|
Toshihiro Shimizu |
890ddd |
sum = b[ip - 1];
|
|
Toshihiro Shimizu |
890ddd |
b[ip - 1] = b[i - 1];
|
|
Toshihiro Shimizu |
890ddd |
if (ii)
|
|
Toshihiro Shimizu |
890ddd |
for (j = ii; j <= i - 1; j++)
|
|
Toshihiro Shimizu |
890ddd |
sum -= a[getEl(i, j, n)] * b[j - 1];
|
|
Toshihiro Shimizu |
890ddd |
else if (sum)
|
|
Toshihiro Shimizu |
890ddd |
ii = i;
|
|
Toshihiro Shimizu |
890ddd |
b[i - 1] = sum;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
for (i = n; i >= 1; i--) {
|
|
Toshihiro Shimizu |
890ddd |
sum = b[i - 1];
|
|
Toshihiro Shimizu |
890ddd |
for (j = i + 1; j <= n; j++)
|
|
Toshihiro Shimizu |
890ddd |
sum -= a[getEl(i, j, n)] * b[j - 1];
|
|
Toshihiro Shimizu |
890ddd |
b[i - 1] = sum / a[getEl(i, i, n)];
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double tdet(double *LUa, int n, double d)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
for (int i = 1; i <= n; ++i)
|
|
Toshihiro Shimizu |
890ddd |
d *= LUa[getEl(i, i, n)];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
return d;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double tdet(double *a, int n)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double d;
|
|
Shinya Kitaoka |
3bfa54 |
std::vector<int> indx(n);</int>
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
tLUDecomposition(a, n, &indx[0], d);
|
|
Toshihiro Shimizu |
890ddd |
for (int i = 1; i <= n; ++i)
|
|
Toshihiro Shimizu |
890ddd |
d *= a[getEl(i, i, n)];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
return d;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
void tsolveSistem(double *a, int n, double *res)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double d;
|
|
Shinya Kitaoka |
3bfa54 |
std::vector<int> indx(n);</int>
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
tLUDecomposition(a, n, &indx[0], d);
|
|
Toshihiro Shimizu |
890ddd |
assert(tdet(a, n, d) != 0);
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
if( isAlmostZero(tdet(a, n, d)) )
|
|
Toshihiro Shimizu |
890ddd |
throw TMathException("Singular matrix in routine tLUDecomposition\n");
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
tbackSubstitution(a, n, &indx[0], res);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Shinya Kitaoka |
3bfa54 |
int rootFinding(const std::vector<double> &in_poly, std::vector<double> &sol)</double></double>
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
// per ora risolvo solo i polinomi di grado al piu' pari a 3
|
|
Toshihiro Shimizu |
890ddd |
assert((int)in_poly.size() <= MAX_ORDER);
|
|
Toshihiro Shimizu |
890ddd |
if (in_poly.empty() || (int)in_poly.size() > MAX_ORDER)
|
|
Toshihiro Shimizu |
890ddd |
return -1;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
std::vector<double> p;</double>
|
|
Toshihiro Shimizu |
890ddd |
std::copy(in_poly.begin(), in_poly.end(), std::back_inserter(p));
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// eat zero in poly
|
|
Toshihiro Shimizu |
890ddd |
while (!p.empty() && isAlmostZero(p.back()))
|
|
Toshihiro Shimizu |
890ddd |
p.pop_back();
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
sol.clear();
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
while (!p.empty() && p.front() == 0) {
|
|
Toshihiro Shimizu |
890ddd |
sol.push_back(0.0);
|
|
Toshihiro Shimizu |
890ddd |
p.erase(p.begin()); //se i coefficienti bassi sono zero, ci sono soluzioni pari a 0.0. le metto,
|
|
Toshihiro Shimizu |
890ddd |
} //e abbasso il grado del polinomio(piu' veloce)
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
switch (p.size()) {
|
|
Toshihiro Shimizu |
890ddd |
case 0:
|
|
Toshihiro Shimizu |
890ddd |
if (sol.empty())
|
|
Toshihiro Shimizu |
890ddd |
return INFINITE_SOLUTIONS;
|
|
Shinya Kitaoka |
d4642c |
break;
|
|
Shinya Kitaoka |
d4642c |
case 1: //no solutions
|
|
Shinya Kitaoka |
d4642c |
break;
|
|
Shinya Kitaoka |
d4642c |
case 2:
|
|
Shinya Kitaoka |
d4642c |
sol.push_back(-p[0] / p[1]);
|
|
Shinya Kitaoka |
d4642c |
break;
|
|
Shinya Kitaoka |
d4642c |
case 3:
|
|
Shinya Kitaoka |
d4642c |
rootForQuadraticEquation(p, sol);
|
|
Shinya Kitaoka |
d4642c |
break;
|
|
Shinya Kitaoka |
d4642c |
case 4:
|
|
Shinya Kitaoka |
d4642c |
rootForCubicEquation(p, sol);
|
|
Shinya Kitaoka |
d4642c |
break;
|
|
Shinya Kitaoka |
d4642c |
default:
|
|
Toshihiro Shimizu |
890ddd |
rootForGreaterThanThreeEquation(p, sol);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
return sol.size();
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
/*
|
|
Toshihiro Shimizu |
890ddd |
*/
|
|
Toshihiro Shimizu |
890ddd |
int numberOfRootsInInterval(int order, const double *polyH, double min, double max)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
poly sseq[MAX_ORDER];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
int
|
|
Toshihiro Shimizu |
890ddd |
i,
|
|
Toshihiro Shimizu |
890ddd |
nchanges_0,
|
|
Toshihiro Shimizu |
890ddd |
nchanges_1,
|
|
Toshihiro Shimizu |
890ddd |
np;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
if (order > MAX_ORDER)
|
|
Toshihiro Shimizu |
890ddd |
return -1;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
while (polyH[order] == 0.0 && order > 0)
|
|
Toshihiro Shimizu |
890ddd |
--order;
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// init a sturm's sequence with our polynomious
|
|
Toshihiro Shimizu |
890ddd |
for (i = order; i >= 0; --i)
|
|
Toshihiro Shimizu |
890ddd |
sseq[0].coef[i] = polyH[i];
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
// build the Sturm sequence
|
|
Toshihiro Shimizu |
890ddd |
np = buildsturm(order, sseq);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//compute number of variation on 0.0
|
|
Toshihiro Shimizu |
890ddd |
nchanges_0 = numchanges(np, sseq, min);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
nchanges_1 = numchanges(np, sseq, max);
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
return (nchanges_0 - nchanges_1);
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double quadraticRoot(double a, double b, double c)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double bb, q;
|
|
Toshihiro Shimizu |
890ddd |
// caso lineare
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(a) < epsilon) {
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(b) >= epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return -c / b;
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
bb = b * b;
|
|
Toshihiro Shimizu |
890ddd |
q = bb - 4.0 * a * c;
|
|
Toshihiro Shimizu |
890ddd |
if (q < 0.0)
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
q = sqrt(q);
|
|
Toshihiro Shimizu |
890ddd |
if (b < 0.0)
|
|
Toshihiro Shimizu |
890ddd |
q = -q;
|
|
Toshihiro Shimizu |
890ddd |
q = -0.5 * (b + q);
|
|
Toshihiro Shimizu |
890ddd |
double root1 = -1;
|
|
Toshihiro Shimizu |
890ddd |
double root2 = -1;
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(q) >= epsilon)
|
|
Toshihiro Shimizu |
890ddd |
root1 = c / q;
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(a) >= epsilon)
|
|
Toshihiro Shimizu |
890ddd |
root2 = q / a;
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 - epsilon <= root1 && root1 <= 1.0 + epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return root1;
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 - epsilon <= root2 && root2 <= 1.0 + epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return root2;
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
double cubicRoot(double a, double b, double c, double d)
|
|
Toshihiro Shimizu |
890ddd |
{
|
|
Toshihiro Shimizu |
890ddd |
double A, Q, R, QQQ, RR;
|
|
Toshihiro Shimizu |
890ddd |
double theta;
|
|
Toshihiro Shimizu |
890ddd |
/* Test for a quadratic or linear degeneracy */
|
|
Toshihiro Shimizu |
890ddd |
if (fabs(a) < epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return quadraticRoot(b, c, d);
|
|
Toshihiro Shimizu |
890ddd |
/* Normalize */
|
|
Toshihiro Shimizu |
890ddd |
b /= a;
|
|
Toshihiro Shimizu |
890ddd |
c /= a;
|
|
Toshihiro Shimizu |
890ddd |
d /= a;
|
|
Toshihiro Shimizu |
890ddd |
a = 1.0;
|
|
Toshihiro Shimizu |
890ddd |
/* Compute discriminants */
|
|
Toshihiro Shimizu |
890ddd |
Q = (b * b - 3.0 * c) / 9.0;
|
|
Toshihiro Shimizu |
890ddd |
QQQ = Q * Q * Q;
|
|
Toshihiro Shimizu |
890ddd |
R = (2.0 * b * b * b - 9.0 * b * c + 27.0 * d) / 54.0;
|
|
Toshihiro Shimizu |
890ddd |
RR = R * R;
|
|
Toshihiro Shimizu |
890ddd |
/* Three real roots */
|
|
Toshihiro Shimizu |
890ddd |
if (RR < QQQ) {
|
|
Toshihiro Shimizu |
890ddd |
theta = acos(R / sqrt(QQQ));
|
|
Toshihiro Shimizu |
890ddd |
double root[3];
|
|
Toshihiro Shimizu |
890ddd |
root[0] = root[1] = root[2] = -2.0 * sqrt(Q);
|
|
Toshihiro Shimizu |
890ddd |
root[0] *= cos(theta / 3.0);
|
|
Toshihiro Shimizu |
890ddd |
root[1] *= cos((theta + 2 * TConsts::pi) / 3.0);
|
|
Toshihiro Shimizu |
890ddd |
root[2] *= cos((theta - 2 * TConsts::pi) / 3.0);
|
|
Toshihiro Shimizu |
890ddd |
root[0] -= b / 3.0;
|
|
Toshihiro Shimizu |
890ddd |
root[1] -= b / 3.0;
|
|
Toshihiro Shimizu |
890ddd |
root[2] -= b / 3.0;
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 - epsilon < root[0] && root[0] < 1.0 + epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return root[0];
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 - epsilon < root[1] && root[1] < 1.0 + epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return root[1];
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 - epsilon < root[2] && root[2] < 1.0 + epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return root[2];
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
/* One real root */
|
|
Toshihiro Shimizu |
890ddd |
else {
|
|
Toshihiro Shimizu |
890ddd |
double root = 0;
|
|
Toshihiro Shimizu |
890ddd |
A = -pow(fabs(R) + sqrt(RR - QQQ), 1.0 / 3.0);
|
|
Toshihiro Shimizu |
890ddd |
if (A != 0.0) {
|
|
Toshihiro Shimizu |
890ddd |
if (R < 0.0)
|
|
Toshihiro Shimizu |
890ddd |
A = -A;
|
|
Toshihiro Shimizu |
890ddd |
root = A + Q / A;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
root -= b / 3.0;
|
|
Toshihiro Shimizu |
890ddd |
if (0.0 - epsilon < root && root < 1.0 + epsilon)
|
|
Toshihiro Shimizu |
890ddd |
return root;
|
|
Toshihiro Shimizu |
890ddd |
return 1;
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
}
|
|
Toshihiro Shimizu |
890ddd |
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|
|
Toshihiro Shimizu |
890ddd |
// End Of File
|
|
Toshihiro Shimizu |
890ddd |
//-----------------------------------------------------------------------------
|