00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifdef HAVE_CONFIG_H
00019 #include "config.h"
00020 #endif
00021 #include <math.h>
00022 #include "solvers.h"
00023
00024 #ifdef DMALLOC
00025 #include "dmalloc.h"
00026 #endif
00027
00028 #ifndef HAVE_CBRT
00029 #define cbrt(x) ((x < 0) ? (-1*pow(-x, 1.0/3.0)) : pow (x, 1.0/3.0))
00030 #endif
00031 #ifndef M_PI
00032 #define M_PI 3.14159265358979323846
00033 #endif
00034
00035 #define EPS 1E-7
00036 #define AEQ0(x) (((x) < EPS) && ((x) > -EPS))
00037
00038 int solve3(double *coeff, double *roots)
00039 {
00040 double a, b, c, d;
00041 int rootn, i;
00042 double p, q, disc, b_over_3a, c_over_a, d_over_a;
00043 double r, theta, temp, alpha, beta;
00044
00045 a = coeff[3], b = coeff[2], c = coeff[1], d = coeff[0];
00046 if (AEQ0(a))
00047 return solve2(coeff, roots);
00048 b_over_3a = b / (3 * a);
00049 c_over_a = c / a;
00050 d_over_a = d / a;
00051
00052 p = b_over_3a * b_over_3a;
00053 q = 2 * b_over_3a * p - b_over_3a * c_over_a + d_over_a;
00054 p = c_over_a / 3 - p;
00055 disc = q * q + 4 * p * p * p;
00056
00057 if (disc < 0) {
00058 r = .5 * sqrt(-disc + q * q);
00059 theta = atan2(sqrt(-disc), -q);
00060 temp = 2 * cbrt(r);
00061 roots[0] = temp * cos(theta / 3);
00062 roots[1] = temp * cos((theta + M_PI + M_PI) / 3);
00063 roots[2] = temp * cos((theta - M_PI - M_PI) / 3);
00064 rootn = 3;
00065 } else {
00066 alpha = .5 * (sqrt(disc) - q);
00067 beta = -q - alpha;
00068 roots[0] = cbrt(alpha) + cbrt(beta);
00069 if (disc > 0)
00070 rootn = 1;
00071 else
00072 roots[1] = roots[2] = -.5 * roots[0], rootn = 3;
00073 }
00074
00075 for (i = 0; i < rootn; i++)
00076 roots[i] -= b_over_3a;
00077
00078 return rootn;
00079 }
00080
00081 int solve2(double *coeff, double *roots)
00082 {
00083 double a, b, c;
00084 double disc, b_over_2a, c_over_a;
00085
00086 a = coeff[2], b = coeff[1], c = coeff[0];
00087 if (AEQ0(a))
00088 return solve1(coeff, roots);
00089 b_over_2a = b / (2 * a);
00090 c_over_a = c / a;
00091
00092 disc = b_over_2a * b_over_2a - c_over_a;
00093 if (disc < 0)
00094 return 0;
00095 else if (disc == 0) {
00096 roots[0] = -b_over_2a;
00097 return 1;
00098 } else {
00099 roots[0] = -b_over_2a + sqrt(disc);
00100 roots[1] = -2 * b_over_2a - roots[0];
00101 return 2;
00102 }
00103 }
00104
00105 int solve1(double *coeff, double *roots)
00106 {
00107 double a, b;
00108
00109 a = coeff[1], b = coeff[0];
00110 if (AEQ0(a)) {
00111 if (AEQ0(b))
00112 return 4;
00113 else
00114 return 0;
00115 }
00116 roots[0] = -b / a;
00117 return 1;
00118 }