rpp_quintic.cpp
Go to the documentation of this file.
00001 /* ========================================================================
00002  * PROJECT: ARToolKitPlus
00003  * ========================================================================
00004  *
00005  * Solution of a quintic equation by a hybrid method
00006  * Conversion from Fortran to C by Raoul Rausch (Oct 2004)
00007  *
00008  * Copyright of the derived and new portions of this work
00009  *     (C) 2006 Graz University of Technology
00010  *
00011  * This framework is free software; you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation; either version 2 of the License, or
00014  * (at your option) any later version.
00015  *
00016  * This framework is distributed in the hope that it will be useful,
00017  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  * GNU General Public License for more details.
00020  *
00021  * You should have received a copy of the GNU General Public License
00022  * along with this framework; if not, write to the Free Software
00023  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00024  *
00025  * For further information please contact 
00026  *   Dieter Schmalstieg
00027  *   <schmalstieg@icg.tu-graz.ac.at>
00028  *   Graz University of Technology, 
00029  *   Institut for Computer Graphics and Vision,
00030  *   Inffeldgasse 16a, 8010 Graz, Austria.
00031  * ========================================================================
00032  ** @author   Thomas Pintaric
00033  *
00034  * $Id: rpp_quintic.cpp 162 2006-04-19 21:28:10Z grabner $
00035  * @file
00036  * ======================================================================== */
00037 
00038 
00039 #ifndef _NO_LIBRPP_
00040 
00041 
00042 #include "math.h"
00043 #include "stdio.h"
00044 #define max(a,b) (a>b?a:b)
00045 #define min(a,b) (a<b?a:b)
00046 
00047 
00048 namespace rpp {
00049 
00050 
00051 int quintic(double [], double [], double [], int*, double);
00052 int quartic(double[], double[], double[], int* );
00053 int cubic(double[], double[], int*);
00054 int signR(double);
00055 double CBRT(double);
00056 
00057 /*-------------------- Global Function Description Block ----------------------
00058  *
00059  *     ***QUINTIC************************************************25.03.98
00060  *     Solution of a quintic equation by a hybrid method:
00061  *     first real solution is obtained numerically by the Newton method,
00062  *     the remaining four roots are obtained analytically by QUARTIC
00063  *     NO WARRANTY, ALWAYS TEST THIS SUBROUTINE AFTER DOWNLOADING
00064  *     ******************************************************************
00065  *     dd(0:4)     (i)  vector containing the polynomial coefficients
00066  *     sol(1:4)    (o)  results, real part
00067  *     soli(1:4)   (o)  results, imaginary part
00068  *     Nsol        (o)  number of real solutions 
00069  *
00070  *     17-Oct-2004 / Raoul Rausch
00071  *     Conversion from Fortran to C
00072  *
00073  *
00074  *-----------------------------------------------------------------------------
00075  */
00076 /*
00077 int quintic(double dd[6], double sol[5], double soli[5], int *Nsol, double xstart)
00078 {
00079         double  dd4[5], sol4[4], soli4[4], xnew, xs;//, soli4[4];dd[6], sol[5], soli[5],
00080         double sum, sum1, eC;
00081         const double eps = 1.e-8;
00082         int i, Nsol4;
00083 
00084         *Nsol = 0;
00085 
00086         //printf("\n Quintic!\n");
00087 
00088         if (dd[5] == 0.0)
00089         { 
00090                 //printf("\n ERROR: NOT A QUINTIC EQUATION");
00091                 return 0;
00092         }
00093 
00094         // Newton iteration of one real root
00095         xs= xstart;
00096         xnew = xstart;  //added rr
00097         do
00098         {
00099                 xs = xnew;      //added rr
00100                 sum = dd[0];
00101                 for (i=1;i<6;i++)       sum += dd[i]*pow(xs,i); // Don't know what ** means
00102                 sum1 = dd[1];
00103                 for (i=1;i<5;i++)       sum1 += (double)(i+1)*dd[i+1]*pow(xs,i);
00104                 xnew = xs - sum/sum1;
00105                 //if (fabs(xnew-xs) > eps)
00106                 //xs =xnew;
00107                 //printf("\n %f\t%f!", xs, xnew);
00108         }while (fabs(xnew-xs) > eps);
00109 
00110         eC = xnew;
00111         //
00112         // "eC" is one real root of quintic equation
00113         // reduce quintic to quartic equation using "eC"
00114         dd4[4] = dd[5];
00115         for (i=4;i>0;i--)       dd4[i-1] = dd[i] + eC*dd4[i];
00116 
00117         quartic(dd4, sol4, soli4, &Nsol4);
00118 
00119         
00120         sol[0] = eC;
00121         soli[0] = 0.0;
00122 
00123         for (i=0;i<4;i++)
00124         {
00125                 sol[i+1] =sol4[i];
00126                 soli[i+1] = soli4[i];
00127         }
00128         *Nsol = Nsol4 + 1;
00129 
00130         return 0;
00131 }
00132 */
00133 
00134 /*-------------------- Global Function Description Block ----------------------
00135  *
00136  *     ***QUARTIC************************************************25.03.98
00137  *     Solution of a quartic equation
00138  *     ref.: J. E. Hacke, Amer. Math. Monthly, Vol. 48, 327-328, (1941)
00139  *     NO WARRANTY, ALWAYS TEST THIS SUBROUTINE AFTER DOWNLOADING
00140  *     ******************************************************************
00141  *     dd(0:4)     (i)  vector containing the polynomial coefficients
00142  *     sol(1:4)    (o)  results, real part
00143  *     soli(1:4)   (o)  results, imaginary part
00144  *     Nsol        (o)  number of real solutions 
00145  *     ==================================================================
00146  *      17-Oct-2004 / Raoul Rausch
00147  *              Conversion from Fortran to C
00148  *
00149  *
00150  *-----------------------------------------------------------------------------
00151  */
00152 
00153  int quartic(double dd[5], double sol[4], double soli[4], int* Nsol)
00154  {
00155         double AA[4], z[3];
00156         double a, b, c, d, f, p, q, r, zsol, xK2, xL, xK, sqp, sqm;
00157         int ncube, i;
00158         *Nsol = 0;
00159 
00160         if (dd[4] == 0.0)
00161         {
00162                 //printf("\n ERROR: NOT A QUARTIC EQUATION");
00163                 return 0;
00164         }
00165 
00166         a = dd[4];
00167         b = dd[3];
00168         c = dd[2];
00169         d = dd[1];
00170         f = dd[0];
00171 
00172         p = (-3.0*pow(b,2) + 8.0 *a*c)/(8.0*pow(a,2));
00173         q = (pow(b,3) - 4.0*a*b*c + 8.0 *d*pow(a,2)) / (8.0*pow(a,3));
00174         r = (-3.0*pow(b,4) + 16.0 *a*pow(b,2)*c - 64.0 *pow(a,2)*b*d + 256.0 *pow(a,3)*f)/(256.0*pow(a,4));
00175         
00176         // Solve cubic resolvent
00177         AA[3] = 8.0;
00178         AA[2] = -4.0*p;
00179         AA[1] = -8.0*r;
00180         AA[0] = 4.0*p*r - pow(q,2);
00181 
00182         //printf("\n bcubic %.4e\t%.4e\t%.4e\t%.4e ", AA[0], AA[1], AA[2], AA[3]);
00183         cubic(AA, z, &ncube);
00184         //printf("\n acubic %.4e\t%.4e\t%.4e ", z[0], z[1], z[2]);
00185         
00186         zsol = - 1.e99;
00187         for (i=0;i<ncube;i++)   zsol = max(zsol, z[i]); //Not sure C has max fct
00188         z[0] =zsol;
00189         xK2 = 2.0*z[0] -p;
00190         xK = sqrt(xK2);
00191         xL = q/(2.0*xK);
00192         sqp = xK2 - 4.0 * (z[0] + xL);
00193         sqm = xK2 - 4.0 * (z[0] - xL);
00194 
00195         for (i=0;i<4;i++)       soli[i] = 0.0;
00196         if ( (sqp >= 0.0) && (sqm >= 0.0))
00197         {
00198                 //printf("\n case 1 ");
00199                 sol[0] = 0.5 * (xK + sqrt(sqp));
00200                 sol[1] = 0.5 * (xK - sqrt(sqp));
00201                 sol[2] = 0.5 * (-xK + sqrt(sqm));
00202                 sol[3] = 0.5 * (-xK - sqrt(sqm));
00203                 *Nsol = 4;
00204         }
00205         else if ( (sqp >= 0.0) && (sqm < 0.0))
00206         {
00207                 //printf("\n case 2 ");
00208                 sol[0] = 0.5 * (xK + sqrt(sqp));
00209                 sol[1] = 0.5 * (xK - sqrt(sqp));
00210                 sol[2] = -0.5 * xK;
00211                 sol[3] = -0.5 * xK;
00212                 soli[2] =  sqrt(-.25 * sqm);
00213                 soli[3] = -sqrt(-.25 * sqm);
00214                 *Nsol = 2;
00215         }
00216         else if ( (sqp < 0.0) && (sqm >= 0.0))
00217         {
00218                 //printf("\n case 3 ");
00219                 sol[0] = 0.5 * (-xK + sqrt(sqm));
00220                 sol[1] = 0.5 * (-xK - sqrt(sqm));
00221                 sol[2] = 0.5 * xK;
00222                 sol[3] = 0.5 * xK;
00223                 soli[2] =  sqrt(-0.25 * sqp);
00224                 soli[3] = -sqrt(-0.25 * sqp);
00225                 *Nsol = 2;
00226         }
00227         else if ( (sqp < 0.0) && (sqm < 0.0))
00228         {
00229                 //printf("\n case 4 ");
00230                 sol[0] = -0.5 * xK;
00231                 sol[1] = -0.5 * xK;
00232                 soli[0] =  sqrt(-0.25 * sqm);
00233                 soli[1] = -sqrt(-0.25 * sqm);
00234                 sol[2] = 0.5 * xK;
00235                 sol[3] = 0.5 * xK;
00236                 soli[2] =  sqrt(-0.25 * sqp);
00237                 soli[3] = -sqrt(-0.25 * sqp);
00238                 *Nsol = 0;
00239         }
00240         
00241         for (i=0;i<4;i++)       sol[i] -= b/(4.0*a);
00242         return 0;
00243  }
00244 
00245 
00246  /*-------------------- Global Function Description Block ----------------------
00247   *
00248   *     ***CUBIC************************************************08.11.1986
00249   *     Solution of a cubic equation
00250   *     Equations of lesser degree are solved by the appropriate formulas.
00251   *     The solutions are arranged in ascending order.
00252   *     NO WARRANTY, ALWAYS TEST THIS SUBROUTINE AFTER DOWNLOADING
00253   *     ******************************************************************
00254   *     A(0:3)      (i)  vector containing the polynomial coefficients
00255   *     X(1:L)      (o)  results
00256   *     L           (o)  number of valid solutions (beginning with X(1))
00257   *     ==================================================================
00258   *     17-Oct-2004 / Raoul Rausch
00259   *             Conversion from Fortran to C
00260   *
00261   *
00262   *-----------------------------------------------------------------------------
00263   */
00264 
00265 int cubic(double A[4], double X[3], int* L)
00266 {
00267         const double PI = 3.1415926535897932;
00268         const double THIRD = 1./3.;
00269         double U[3],W, P, Q, DIS, PHI;
00270         int i;
00271 
00272         //define cubic root as statement function
00273         // In C, the function is defined outside of the cubic fct
00274 
00275         // ====determine the degree of the polynomial ====
00276 
00277         if (A[3] != 0.0)
00278         {
00279                 //cubic problem
00280                 W = A[2]/A[3]*THIRD;
00281                 P = pow((A[1]/A[3]*THIRD - pow(W,2)),3);
00282                 Q = -.5*(2.0*pow(W,3)-(A[1]*W-A[0])/A[3] );
00283                 DIS = pow(Q,2)+P;
00284                 if ( DIS < 0.0 )
00285                 {
00286                         //three real solutions!
00287                         //Confine the argument of ACOS to the interval [-1;1]!
00288                         PHI = acos(min(1.0,max(-1.0,Q/sqrt(-P))));
00289                         P=2.0*pow((-P),(5.e-1*THIRD));
00290                         for (i=0;i<3;i++)       U[i] = P*cos((PHI+2*((double)i)*PI)*THIRD)-W;
00291                         X[0] = min(U[0], min(U[1], U[2]));
00292                         X[1] = max(min(U[0], U[1]),max( min(U[0], U[2]), min(U[1], U[2])));
00293                         X[2] = max(U[0], max(U[1], U[2]));
00294                         *L = 3;
00295                 }
00296                 else
00297                 {
00298                         // only one real solution!
00299                         DIS = sqrt(DIS);
00300                         X[0] = CBRT(Q+DIS)+CBRT(Q-DIS)-W;
00301                         *L=1;
00302                 }
00303         }
00304         else if (A[2] != 0.0)
00305         {
00306                 // quadratic problem
00307                 P = 0.5*A[1]/A[2];
00308                 DIS = pow(P,2)-A[0]/A[2];
00309                 if (DIS > 0.0)
00310                 {
00311                         // 2 real solutions
00312                         X[0] = -P - sqrt(DIS);
00313                         X[1] = -P + sqrt(DIS);
00314                         *L=2;
00315                 }
00316                 else
00317                 {
00318                         // no real solution
00319                         *L=0;
00320                 }
00321         }
00322         else if (A[1] != 0.0)
00323         {
00324                 //linear equation
00325                 X[0] =A[0]/A[1];
00326                 *L=1;
00327         }
00328         else
00329         {
00330                 //no equation
00331                 *L=0;
00332         }
00333  //
00334  //     ==== perform one step of a newton iteration in order to minimize
00335  //          round-off errors ====
00336  //
00337         for (i=0;i<*L;i++)
00338         {
00339                 X[i] = X[i] - (A[0]+X[i]*(A[1]+X[i]*(A[2]+X[i]*A[3])))/(A[1]+X[i]*(2.0*A[2]+X[i]*3.0*A[3]));
00340         //      printf("\n X inside cubic %.15e\n", X[i]);
00341         }
00342 
00343         return 0;
00344 }
00345 
00346 
00347 int signR(double Z)
00348 {
00349         int ret = 0;
00350         if (Z > 0.0)    ret = 1;
00351         if (Z < 0.0)    ret = -1;
00352         if (Z == 0.0)   ret =0;
00353 
00354         return ret;
00355 }
00356 
00357 double CBRT(double Z)
00358 {
00359         double ret;
00360         const double THIRD = 1./3.;
00361         //define cubic root as statement function
00362         //SIGN has different meanings in both C and Fortran
00363         // Was unable to use the sign command of C, so wrote my own
00364         // that why a new variable needs to be introduced that keeps track of the sign of
00365         // SIGN is supposed to return a 1, -1 or 0 depending on what the sign of the argument is
00366         ret = fabs(pow(fabs(Z),THIRD)) * signR(Z);
00367         return ret;
00368 }
00369 
00370 
00371 }  // namespace rpp
00372 
00373 
00374 #endif //_NO_LIBRPP_


aruco_pose
Author(s): Julian Brunner
autogenerated on Mon Oct 6 2014 08:32:34