inline size_t
descartesQuarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
double rts[4];
double worst3[3];
double qrts[4][3]; /* quartic roots for each cubic root */
if (d == 0.0)
{
root1 = 0.0;
return cubicSolve(a,b,c,root2,root3,root4) + 1;
}
int j, n4[4];
double v1[4] = {0,0,0,0},
v2[4] = {0,0,0,0},v3[4]={0,0,0,0};
double k,y;
double p,q,r;
double e0,e1,e2;
double g,h;
double asq;
double ainv4;
double e1invk;
asq = a*a;
e2 = b - asq * (3.0/8.0);
e1 = c + a*(asq*0.125 - b*0.5);
e0 = d + asq*(b*0.0625 - asq*(3.0/256.0)) - a*c*0.25;
p = 2.0*e2;
q = e2*e2 - 4.0*e0;
r = -e1*e1;
size_t n3 = cubicSolve(p,q,r,v3[0],v3[1],v3[2]);
for (size_t j3 = 0; j3 < n3; ++j3)
{
y = v3[j3];
if (y <= 0.0)
n4[j3] = 0;
else
{
k = std::sqrt(y);
ainv4 = a*0.25;
e1invk = e1/k;
g = (y + e2 + e1invk)*0.5;
h = (y + e2 - e1invk)*0.5 ;
bool n1 = quadSolve( g, -k, 1.0, v1[0], v1[1]);
bool n2 = quadSolve( h, k, 1.0, v2[0], v2[1]);
qrts[0][j3] = v1[0] - ainv4;
qrts[1][j3] = v1[1] - ainv4;
qrts[n1*2][j3] = v2[0] - ainv4;
qrts[n1*2+1][j3] = v2[1] - ainv4;
n4[j3]= n1*2 + n2*2;
} /* y>=0 */
for (j = 0; j < n4[j3]; ++j)
rts[j] = qrts[j][j3];
worst3[j3] = quarticError(a, b, c, d, rts, n4[j3]);
} /* j3 loop */
size_t j3 = 0;
if (n3 != 1)
{
if ((n4[1] > n4[j3]) ||
((worst3[1] < worst3[j3] ) && (n4[1] == n4[j3]))) j3 = 1;
if ((n4[2] > n4[j3]) ||
((worst3[2] < worst3[j3] ) && (n4[2] == n4[j3]))) j3 = 2;
}
root1 = qrts[0][j3];
root2 = qrts[1][j3];
root3 = qrts[2][j3];
root4 = qrts[3][j3];
return (n4[j3]);
}
//Solves quartics of the form x^4 + a x^3 + b x^2 + c x + d ==0
inline size_t quarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
static const double maxSqrt = std::sqrt(std::numeric_limits<double>::max());
if (std::abs(a) > maxSqrt)
yacfraidQuarticSolve(a,b,c,d,root1,root2,root3,root4);
if (d == 0)
{//Solve a cubic with a trivial root of 0
root1 = 0;
return 1 + cubicSolve(a, b, c, root2, root3, root4);
}
if ((a == 0) && (c== 0))
{//We have a biquadratic
double quadRoot1,quadRoot2;
if (quadSolve(d,b,1, quadRoot1, quadRoot2))
{
if (quadRoot1 < quadRoot2) std::swap(quadRoot1,quadRoot2);
if (quadRoot1 < 0)
return 0;
root1 = std::sqrt(quadRoot1);
root2 = -std::sqrt(quadRoot1);
if (quadRoot2 < 0)
return 2;
root3 = std::sqrt(quadRoot2);
root4 = -std::sqrt(quadRoot2);
return 4;
}
else
return 0;
}
//Now we have to resort to some dodgy formulae!
size_t k = 0, nr;
if (a < 0.0) k += 2;
if (b < 0.0) k += 1;
if (c < 0.0) k += 8;
if (d < 0.0) k += 4;
switch (k)
{
case 3 :
case 9 :
nr = ferrariQuarticSolve(a,b,c,d,root1,root2,root3,root4);
break;
case 5 :
nr = descartesQuarticSolve(a,b,c,d,root1,root2,root3,root4);
break;
case 15 :
//This algorithm is stable if we flip the sign of the roots
nr = descartesQuarticSolve(-a,b,-c,d,root1,root2,root3,root4);
root1 *=-1; root2 *=-1; root3 *=-1; root4 *=-1;
break;
default:
nr = neumarkQuarticSolve(a,b,c,d,root1,root2,root3,root4);
//nr = ferrariQuarticSolve(a,b,c,d,root1,root2,root3,root4);
//nr = yacfraidQuarticSolve(a,b,c,d,root1,root2,root3,root4);
break;
}
if (nr) detail::quarticNewtonRootPolish(a, b, c, d, root1, 15);
if (nr>1) detail::quarticNewtonRootPolish(a, b, c, d, root2, 15);
if (nr>2) detail::quarticNewtonRootPolish(a, b, c, d, root3, 15);
if (nr>3) detail::quarticNewtonRootPolish(a, b, c, d, root4, 15);
return nr;
}
inline size_t
ferrariQuarticSolve(const double& a, const double& b, const double& c, const double& d,
double& root1, double& root2, double& root3, double& root4)
{
double rts[4];
double worst3[3];
double qrts[4][3]; /* quartic roots for each cubic root */
if (d == 0.0)
{
root1 = 0.0;
return cubicSolve(a,b,c,root2,root3,root4) + 1;
}
int j;
int n4[4];
double asqinv4;
double ainv2;
double d4;
double yinv2;
double v1[4] = {0};
double v2[4] = {0};
double v3[4] = {0};
double p,q,r;
double y;
double e,f,esq,fsq,ef;
double g,gg,h,hh;
ainv2 = a*0.5;
asqinv4 = ainv2*ainv2;
d4 = d*4.0 ;
p = b;
q = a*c-d4;
r = (asqinv4 - b)*d4 + c*c;
size_t n3 = cubicSolve(p,q,r,v3[0],v3[1],v3[2]);
for (size_t j3 = 0; j3 < n3; ++j3)
{
y = v3[j3];
yinv2 = y*0.5;
esq = asqinv4 - b - y;
fsq = yinv2*yinv2 - d;
if ((esq < 0.0) && (fsq < 0.0))
n4[j3] = 0;
else
{
ef = -(0.25*a*y + 0.5*c);
if ( ((a > 0.0)&&(y > 0.0)&&(c > 0.0))
|| ((a > 0.0)&&(y < 0.0)&&(c < 0.0))
|| ((a < 0.0)&&(y > 0.0)&&(c < 0.0))
|| ((a < 0.0)&&(y < 0.0)&&(c > 0.0))
|| (a == 0.0)||(y == 0.0)||(c == 0.0))
/* use ef - */
{
if ((b < 0.0)&&(y < 0.0))
{
e = sqrt(esq);
f = ef/e;
}
else if (d < 0.0)
{
f = sqrt(fsq);
e = ef/f;
}
else
{
if (esq > 0.0)
e = sqrt(esq);
else
e = 0.0;
if (fsq > 0.0)
f = sqrt(fsq);
else
f = 0.0;
if (ef < 0.0)
f = -f;
}
}
else
/* use esq and fsq - */
{
if (esq > 0.0)
e = sqrt(esq);
else
e = 0.0;
if (fsq > 0.0)
f = sqrt(fsq);
else
f = 0.0;
if (ef < 0.0)
f = -f;
}
/* note that e >= 0.0 */
g = ainv2 - e;
gg = ainv2 + e;
if ( ((b > 0.0)&&(y > 0.0))
|| ((b < 0.0)&&(y < 0.0)) )
{
if (((a > 0.0) && (e > 0.0))
|| ((a < 0.0) && (e < 0.0)) )
g = (b + y)/gg;
else
if (((a > 0.0) && (e < 0.0))
|| ((a < 0.0) && (e > 0.0)) )
gg = (b + y)/g;
}
hh = -yinv2 + f;
h = -yinv2 - f;
if ( ((f > 0.0)&&(y < 0.0))
|| ((f < 0.0)&&(y > 0.0)) )
h = d/hh;
else
if ( ((f < 0.0)&&(y < 0.0))
|| ((f > 0.0)&&(y > 0.0)) )
hh = d/h;
bool n1 = quadSolve(hh,gg,1.0,v1[0],v1[1]);
bool n2 = quadSolve(h,g,1.0,v2[0],v2[1]);
n4[j3] = n1*2+n2*2;
qrts[0][j3] = v1[0];
qrts[1][j3] = v1[1];
qrts[n1*2+0][j3] = v2[0];
qrts[n1*2+1][j3] = v2[1];
}
for (j = 0; j < n4[j3]; ++j)
rts[j] = qrts[j][j3];
worst3[j3] = quarticError(a, b, c, d, rts, n4[j3]);
} /* j3 loop */
size_t j3 = 0;
if (n3 != 1)
{
if ((n4[1] > n4[j3]) ||
((worst3[1] < worst3[j3] ) && (n4[1] == n4[j3]))) j3 = 1;
if ((n4[2] > n4[j3]) ||
((worst3[2] < worst3[j3] ) && (n4[2] == n4[j3]))) j3 = 2;
}
root1 = qrts[0][j3];
root2 = qrts[1][j3];
root3 = qrts[2][j3];
root4 = qrts[3][j3];
return (n4[j3]);
}
