int SolveCubic(double *c, double *s)
{
int i, num;
double sub;
double A, B, C;
double sq_A, p, q;
double cb_p, D;
if (IsZero(c[3])) {
return SolveQuadric(c, s);
}
/* normal form: x^3 + Ax^2 + Bx + C = 0 */
A = c[ 2 ] / c[ 3 ];
B = c[ 1 ] / c[ 3 ];
C = c[ 0 ] / c[ 3 ];
/* substitute x = y - A/3 to eliminate quadric term:
x^3 +px + q = 0 */
sq_A = A * A;
p = 1.0/3 * (- 1.0/3 * sq_A + B);
q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
/* use Cardano's formula */
cb_p = p * p * p;
D = q * q + cb_p;
if (IsZero(D))
{
if (IsZero(q)) /* one triple solution */
{
s[ 0 ] = 0;
num = 1;
}
else /* one single and one double solution */
{
double u = cbrt(-q);
s[ 0 ] = 2 * u;
s[ 1 ] = - u;
num = 2;
}
}
else if (D < 0) /* Casus irreducibilis: three real solutions */
{
double phi = 1.0/3 * acos(-q / sqrt(-cb_p));
double t = 2 * sqrt(-p);
s[ 0 ] = t * cos(phi);
s[ 1 ] = - t * cos(phi + M_PI / 3);
s[ 2 ] = - t * cos(phi - M_PI / 3);
num = 3;
}
else /* one real solution */
{
double sqrt_D = sqrt(D);
double u = cbrt(sqrt_D - q);
double v = - cbrt(sqrt_D + q);
s[ 0 ] = u + v;
num = 1;
}
/* resubstitute */
sub = 1.0/3 * A;
for (i = 0; i < num; ++i)
s[ i ] -= sub;
return num;
}
int SolveQuartic( double c[5], double s[4] )
{
double coeffs[ 4 ];
double z, u, v, sub;
double A, B, C, D;
double sq_A, p, q, r;
int i, num;
/* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
A = c[ 3 ] / c[ 4 ];
B = c[ 2 ] / c[ 4 ];
C = c[ 1 ] / c[ 4 ];
D = c[ 0 ] / c[ 4 ];
/* substitute x = y - A/4 to eliminate cubic term:
x^4 + px^2 + qx + r = 0 */
sq_A = A * A;
p = - 3.0/8 * sq_A + B;
q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
if (IsZero(r))
{
/* no absolute term: y(y^3 + py + q) = 0 */
coeffs[ 0 ] = q;
coeffs[ 1 ] = p;
coeffs[ 2 ] = 0;
coeffs[ 3 ] = 1;
num = SolveCubic(coeffs, s);
s[ num++ ] = 0;
}
else
{
/* solve the resolvent cubic ... */
coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
coeffs[ 1 ] = - r;
coeffs[ 2 ] = - 1.0/2 * p;
coeffs[ 3 ] = 1;
(void) SolveCubic(coeffs, s);
/* ... and take the one real solution ... */
z = s[ 0 ];
/* ... to build two quadric equations */
u = z * z - r;
v = 2 * z - p;
if (IsZero(u))
u = 0;
else if (u > 0)
u = sqrt(u);
else
return 0;
if (IsZero(v))
v = 0;
else if (v > 0)
v = sqrt(v);
else
return 0;
coeffs[ 0 ] = z - u;
coeffs[ 1 ] = q < 0 ? -v : v;
coeffs[ 2 ] = 1;
num = SolveQuadric(coeffs, s);
coeffs[ 0 ]= z + u;
coeffs[ 1 ] = q < 0 ? v : -v;
coeffs[ 2 ] = 1;
num += SolveQuadric(coeffs, s + num);
}
/* resubstitute */
sub = 1.0/4 * A;
for (i = 0; i < num; ++i)
s[ i ] -= sub;
return num;
}
Result SolveQuartic(double c[5], double out[4])
{
double A = c[3] / c[4];
double B = c[2] / c[4];
double C = c[1] / c[4];
double D = c[0] / c[4];
double p = B - 3.0/8.0*A*A;
double q = A*A*A / 8.0 - A*B/2.0 + C;
double r = -3.0/256.0*A*A*A*A + A*A*B/16.0 - A*C/4.0 + D;
double c0[] = {r*p/2.0 - q*q/8.0, -r, -p/2.0, 1.0};
double out0[4];
Result res = SolveCubic(c0, out0);
if (res == RESULT_NO_SOLUTION)
return RESULT_NO_SOLUTION;
double z = out0[0];
double out1[4];
double out2[4];
Result res1, res2;
if (q >= 0)
{
double c1[] = {z - sqrt(z*z-r), sqrt(2.0*z-p), 1};
double c2[] = {z + sqrt(z*z-r),-sqrt(2.0*z-p), 1};
res1 = SolveQuadric(c1, out1);
res2 = SolveQuadric(c2, out2);
}
else
{
double c1[] = {z + sqrt(z*z-r), sqrt(2.0*z-p), 1};
double c2[] = {z - sqrt(z*z-r),-sqrt(2.0*z-p), 1};
res1 = SolveQuadric(c1, out1);
res2 = SolveQuadric(c2, out2);
}
if (res1 == RESULT_NO_SOLUTION && res2 == RESULT_NO_SOLUTION)
return RESULT_NO_SOLUTION;
if (res1 == RESULT_ONE_SOLUTION && res2 == RESULT_NO_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
return RESULT_ONE_SOLUTION;
}
if (res1 == RESULT_NO_SOLUTION && res2 == RESULT_ONE_SOLUTION)
{
out[0] = out2[0] - A / 4.0;
return RESULT_ONE_SOLUTION;
}
if (res1 == RESULT_ONE_SOLUTION && res2 == RESULT_ONE_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
out[1] = out2[0] - A / 4.0;
return RESULT_TWO_SOLUTIONS;
}
if (res1 == RESULT_TWO_SOLUTIONS && res2 == RESULT_NO_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
out[1] = out1[1] - A / 4.0;
return RESULT_TWO_SOLUTIONS;
}
if (res1 == RESULT_NO_SOLUTION && res2 == RESULT_TWO_SOLUTIONS)
{
out[0] = out2[0] - A / 4.0;
out[1] = out2[1] - A / 4.0;
return RESULT_TWO_SOLUTIONS;
}
if (res1 == RESULT_ONE_SOLUTION && res2 == RESULT_TWO_SOLUTIONS)
{
out[0] = out1[0] - A / 4.0;
out[1] = out2[0] - A / 4.0;
out[2] = out2[1] - A / 4.0;
return RESULT_THREE_SOLUTIONS;
}
if (res1 == RESULT_TWO_SOLUTIONS && res2 == RESULT_ONE_SOLUTION)
{
out[0] = out1[0] - A / 4.0;
out[1] = out1[1] - A / 4.0;
out[2] = out2[0] - A / 4.0;
return RESULT_THREE_SOLUTIONS;
}
if (res1 == RESULT_TWO_SOLUTIONS && res2 == RESULT_TWO_SOLUTIONS)
{
out[0] = out1[0] - A / 4.0;
out[1] = out1[1] - A / 4.0;
out[2] = out2[0] - A / 4.0;
out[3] = out2[1] - A / 4.0;
return RESULT_FOUR_SOLUTIONS;
}
std::cout << "Something wrong in "__FUNCSIG__ << std::endl;
return RESULT_NO_SOLUTION;
}
