starter/math/polynom_solver.cpp

   1 /*
   2  Copyright 2005, 2006 Computer Vision Lab,
   3  Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
   4  Modified by Damian Stewart <damian@frey.co.nz> 2009-2010;
   5  modifications Copyright 2009, 2010 Damian Stewart <damian@frey.co.nz>.
   6
   7  Distributed under the terms of the GNU General Public License v3.
   8
   9  This file is part of The Artvertiser.
  10
  11  The Artvertiser is free software: you can redistribute it and/or modify
  12  it under the terms of the GNU General Public License as published by
  13  the Free Software Foundation, either version 3 of the License, or
  14  (at your option) any later version.
  15
  16  The Artvertiser is distributed in the hope that it will be useful,
  17  but WITHOUT ANY WARRANTY; without even the implied warranty of
  18  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  19  GNU General Public License for more details.
  20
  21  You should have received a copy of the GNU Lesser General Public License
  22  along with The Artvertiser.  If not, see <http://www.gnu.org/licenses/>.
  23  */
  24
  25 /// \file polynom_solver.cpp
  26 /// \brief Functions to calculate roots of polynomials (Author: Mustafa Ozuysal, modified by Vincent Lepetit)
  27
  28 #include <math.h>
  29 #include <iostream>
  30
  31 #include "polynom_solver.h"
  32 #include <general/general.h>
  33
  34 using namespace std;
  35
  36 int solve_deg2(double a, double b, double c, double & x1, double & x2)
  37 {
  38   double delta = b * b - 4 * a * c;
  39
  40   if (delta < 0) return 0;
  41
  42   double inv_2a = 0.5 / a;
  43
  44   if (delta == 0)
  45   {
  46     x1 = -b * inv_2a;
  47     x2 = x1;
  48     return 1;
  49   }
  50
  51   double sqrt_delta = sqrt(delta);
  52   x1 = (-b + sqrt_delta) * inv_2a;
  53   x2 = (-b - sqrt_delta) * inv_2a;
  54   return 2;
  55 }
  56
  57
  58 /// Reference : Eric W. Weisstein. "Cubic Equation." From MathWorld--A Wolfram Web Resource.
  59 /// http://mathworld.wolfram.com/CubicEquation.html
  60 /// \return Number of real roots found.
  61 int solve_deg3(double a, double b, double c, double d,
  62                double & x0, double & x1, double & x2)
  63 {
  64   if (a == 0)
  65   {
  66     // Solve second order sytem
  67     if (b == 0)
  68     {
  69       // Solve first order system
  70       if (c == 0)
  71         return 0;
  72
  73       x0 = -d / c;
  74       return 1;
  75     }
  76
  77     x2 = 0;
  78     return solve_deg2(b, c, d, x0, x1);
  79   }
  80
  81   // Calculate the normalized form x^3 + a2 * x^2 + a1 * x + a0 = 0
  82   double inv_a = 1. / a;
  83   double b_a = inv_a * b, b_a2 = b_a * b_a;
  84   double c_a = inv_a * c;
  85   double d_a = inv_a * d;
  86
  87   // Solve the cubic equation
  88   double Q = (3 * c_a - b_a2) / 9;
  89   double R = (9 * b_a * c_a - 27 * d_a - 2 * b_a * b_a2) / 54;
  90   double Q3 = Q * Q * Q;
  91   double D = Q3 + R * R;
  92   double b_a_3 = (1. / 3.) * b_a;
  93
  94   if (Q == 0) {
  95     if(R == 0) {
  96       x0 = x1 = x2 = - b_a_3;
  97       return 3;
  98     }
  99     else
 100     {
 101       x0 = pow(2 * R, 1 / 3.0) - b_a_3;
 102       return 1;
 103     }
 104   }
 105
 106   if (D <= 0)
 107   {
 108     // Three real roots
 109     double theta = acos(R / sqrt(-Q3));
 110     double sqrt_Q = sqrt(-Q);
 111     x0 = 2 * sqrt_Q * cos(theta             / 3.0) - b_a_3;
 112     x1 = 2 * sqrt_Q * cos((theta + 2 * M_PI)/ 3.0) - b_a_3;
 113     x2 = 2 * sqrt_Q * cos((theta + 4 * M_PI)/ 3.0) - b_a_3;
 114
 115     return 3;
 116   }
 117
 118   // D > 0, only one real root
 119   double AD = pow(fabs(R) + sqrt(D), 1.0 / 3.0) * (R > 0 ? 1 : (R < 0 ? -1 : 0));
 120   double BD = (AD == 0) ? 0 : -Q / AD;
 121
 122   // Calculate the only real root
 123   x0 = AD + BD - b_a_3;
 124
 125   return 1;
 126 }
 127
 128 /// Reference : Eric W. Weisstein. "Quartic Equation." From MathWorld--A Wolfram Web Resource.
 129 /// http://mathworld.wolfram.com/QuarticEquation.html
 130 /// \return Number of real roots found.
 131 int solve_deg4(double a, double b, double c, double d, double e,
 132                double & x0, double & x1, double & x2, double & x3)
 133 {
 134   if (a == 0)
 135   {
 136     x3 = 0;
 137     return solve_deg3(b, c, d, e, x0, x1, x2);
 138   }
 139
 140   // Normalize coefficients
 141   double inv_a = 1. / a;
 142   b *= inv_a; c *= inv_a; d *= inv_a; e *= inv_a;
 143   double b2 = b * b, bc = b * c, b3 = b2 * b;
 144
 145   // Solve resultant cubic
 146   double r0, r1, r2;
 147   int n = solve_deg3(1, -c, d * b - 4 * e, 4 * c * e - d * d - b2 * e, r0, r1, r2);
 148   if (n == 0) return 0;
 149
 150   // Calculate R^2
 151   double R2 = 0.25 * b2 - c + r0, R;
 152   if (R2 < 0)
 153     return 0;
 154
 155   R = sqrt(R2);
 156   double inv_R = 1. / R;
 157
 158   int nb_real_roots = 0;
 159
 160   // Calculate D^2 and E^2
 161   double D2, E2;
 162   if (R < 10E-12)
 163   {
 164     double temp = r0 * r0 - 4 * e;
 165     if (temp < 0)
 166       D2 = E2 = -1;
 167     else
 168     {
 169       double sqrt_temp = sqrt(temp);
 170       D2 = 0.75 * b2 - 2 * c + 2 * sqrt_temp;
 171       E2 = D2 - 4 * sqrt_temp;
 172     }
 173   }
 174   else
 175   {
 176     double u = 0.75 * b2 - 2 * c - R2, v = 0.25 * inv_R * (4 * bc - 8 * d - b3);
 177     D2 = u + v;
 178     E2 = u - v;
 179   }
 180
 181   double b_4 = 0.25 * b, R_2 = 0.5 * R;
 182   if (D2 >= 0) {
 183     double D = sqrt(D2);
 184     nb_real_roots = 2;
 185     double D_2 = 0.5 * D;
 186     x0 = R_2 + D_2 - b_4;
 187     x1 = x0 - D;
 188   }
 189
 190   // Calculate E^2
 191   if (E2 >= 0) {
 192     double E = sqrt(E2);
 193     double E_2 = 0.5 * E;
 194     if (nb_real_roots == 0)
 195     {
 196       x0 = - R_2 + E_2 - b_4;
 197       x1 = x0 - E;
 198       nb_real_roots = 2;
 199     }
 200     else
 201     {
 202       x2 = - R_2 + E_2 - b_4;
 203       x3 = x2 - E;
 204       nb_real_roots = 4;
 205     }
 206   }
 207
 208   return nb_real_roots;
 209 }