starter/math/polynom_solver.cpp

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