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_
