rpp_quintic.cpp
Go to the documentation of this file.
1 /* ========================================================================
2  * PROJECT: ARToolKitPlus
3  * ========================================================================
4  *
5  * Solution of a quintic equation by a hybrid method
6  * Conversion from Fortran to C by Raoul Rausch (Oct 2004)
7  *
8  * Copyright of the derived and new portions of this work
9  * (C) 2006 Graz University of Technology
10  *
11  * This framework 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 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This framework 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 General Public License
22  * along with this framework; if not, write to the Free Software
23  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
24  *
25  * For further information please contact
26  * Dieter Schmalstieg
27  * <schmalstieg@icg.tu-graz.ac.at>
28  * Graz University of Technology,
29  * Institut for Computer Graphics and Vision,
30  * Inffeldgasse 16a, 8010 Graz, Austria.
31  * ========================================================================
32  ** @author Thomas Pintaric
33  *
34  * $Id: rpp_quintic.cpp 162 2006-04-19 21:28:10Z grabner $
35  * @file
36  * ======================================================================== */
37 
38 
39 #ifndef _NO_LIBRPP_
40 
41 
42 #include "math.h"
43 #include "stdio.h"
44 #define max(a,b) (a>b?a:b)
45 #define min(a,b) (a<b?a:b)
46 
47 
48 namespace rpp {
49 
50 
51 int quintic(double [], double [], double [], int*, double);
52 int quartic(double[], double[], double[], int* );
53 int cubic(double[], double[], int*);
54 int signR(double);
55 double CBRT(double);
56 
57 /*-------------------- Global Function Description Block ----------------------
58  *
59  * ***QUINTIC************************************************25.03.98
60  * Solution of a quintic equation by a hybrid method:
61  * first real solution is obtained numerically by the Newton method,
62  * the remaining four roots are obtained analytically by QUARTIC
63  * NO WARRANTY, ALWAYS TEST THIS SUBROUTINE AFTER DOWNLOADING
64  * ******************************************************************
65  * dd(0:4) (i) vector containing the polynomial coefficients
66  * sol(1:4) (o) results, real part
67  * soli(1:4) (o) results, imaginary part
68  * Nsol (o) number of real solutions
69  *
70  * 17-Oct-2004 / Raoul Rausch
71  * Conversion from Fortran to C
72  *
73  *
74  *-----------------------------------------------------------------------------
75  */
76 /*
77 int quintic(double dd[6], double sol[5], double soli[5], int *Nsol, double xstart)
78 {
79  double dd4[5], sol4[4], soli4[4], xnew, xs;//, soli4[4];dd[6], sol[5], soli[5],
80  double sum, sum1, eC;
81  const double eps = 1.e-8;
82  int i, Nsol4;
83 
84  *Nsol = 0;
85 
86  //printf("\n Quintic!\n");
87 
88  if (dd[5] == 0.0)
89  {
90  //printf("\n ERROR: NOT A QUINTIC EQUATION");
91  return 0;
92  }
93 
94  // Newton iteration of one real root
95  xs= xstart;
96  xnew = xstart; //added rr
97  do
98  {
99  xs = xnew; //added rr
100  sum = dd[0];
101  for (i=1;i<6;i++) sum += dd[i]*pow(xs,i); // Don't know what ** means
102  sum1 = dd[1];
103  for (i=1;i<5;i++) sum1 += (double)(i+1)*dd[i+1]*pow(xs,i);
104  xnew = xs - sum/sum1;
105  //if (fabs(xnew-xs) > eps)
106  //xs =xnew;
107  //printf("\n %f\t%f!", xs, xnew);
108  }while (fabs(xnew-xs) > eps);
109 
110  eC = xnew;
111  //
112  // "eC" is one real root of quintic equation
113  // reduce quintic to quartic equation using "eC"
114  dd4[4] = dd[5];
115  for (i=4;i>0;i--) dd4[i-1] = dd[i] + eC*dd4[i];
116 
117  quartic(dd4, sol4, soli4, &Nsol4);
118 
119 
120  sol[0] = eC;
121  soli[0] = 0.0;
122 
123  for (i=0;i<4;i++)
124  {
125  sol[i+1] =sol4[i];
126  soli[i+1] = soli4[i];
127  }
128  *Nsol = Nsol4 + 1;
129 
130  return 0;
131 }
132 */
133 
134 /*-------------------- Global Function Description Block ----------------------
135  *
136  * ***QUARTIC************************************************25.03.98
137  * Solution of a quartic equation
138  * ref.: J. E. Hacke, Amer. Math. Monthly, Vol. 48, 327-328, (1941)
139  * NO WARRANTY, ALWAYS TEST THIS SUBROUTINE AFTER DOWNLOADING
140  * ******************************************************************
141  * dd(0:4) (i) vector containing the polynomial coefficients
142  * sol(1:4) (o) results, real part
143  * soli(1:4) (o) results, imaginary part
144  * Nsol (o) number of real solutions
145  * ==================================================================
146  * 17-Oct-2004 / Raoul Rausch
147  * Conversion from Fortran to C
148  *
149  *
150  *-----------------------------------------------------------------------------
151  */
152 
153  int quartic(double dd[5], double sol[4], double soli[4], int* Nsol)
154  {
155  double AA[4], z[3];
156  double a, b, c, d, f, p, q, r, zsol, xK2, xL, xK, sqp, sqm;
157  int ncube, i;
158  *Nsol = 0;
159 
160  if (dd[4] == 0.0)
161  {
162  //printf("\n ERROR: NOT A QUARTIC EQUATION");
163  return 0;
164  }
165 
166  a = dd[4];
167  b = dd[3];
168  c = dd[2];
169  d = dd[1];
170  f = dd[0];
171 
172  p = (-3.0*pow(b,2) + 8.0 *a*c)/(8.0*pow(a,2));
173  q = (pow(b,3) - 4.0*a*b*c + 8.0 *d*pow(a,2)) / (8.0*pow(a,3));
174  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));
175 
176  // Solve cubic resolvent
177  AA[3] = 8.0;
178  AA[2] = -4.0*p;
179  AA[1] = -8.0*r;
180  AA[0] = 4.0*p*r - pow(q,2);
181 
182  //printf("\n bcubic %.4e\t%.4e\t%.4e\t%.4e ", AA[0], AA[1], AA[2], AA[3]);
183  cubic(AA, z, &ncube);
184  //printf("\n acubic %.4e\t%.4e\t%.4e ", z[0], z[1], z[2]);
185 
186  zsol = - 1.e99;
187  for (i=0;i<ncube;i++) zsol = max(zsol, z[i]); //Not sure C has max fct
188  z[0] =zsol;
189  xK2 = 2.0*z[0] -p;
190  xK = sqrt(xK2);
191  xL = q/(2.0*xK);
192  sqp = xK2 - 4.0 * (z[0] + xL);
193  sqm = xK2 - 4.0 * (z[0] - xL);
194 
195  for (i=0;i<4;i++) soli[i] = 0.0;
196  if ( (sqp >= 0.0) && (sqm >= 0.0))
197  {
198  //printf("\n case 1 ");
199  sol[0] = 0.5 * (xK + sqrt(sqp));
200  sol[1] = 0.5 * (xK - sqrt(sqp));
201  sol[2] = 0.5 * (-xK + sqrt(sqm));
202  sol[3] = 0.5 * (-xK - sqrt(sqm));
203  *Nsol = 4;
204  }
205  else if ( (sqp >= 0.0) && (sqm < 0.0))
206  {
207  //printf("\n case 2 ");
208  sol[0] = 0.5 * (xK + sqrt(sqp));
209  sol[1] = 0.5 * (xK - sqrt(sqp));
210  sol[2] = -0.5 * xK;
211  sol[3] = -0.5 * xK;
212  soli[2] = sqrt(-.25 * sqm);
213  soli[3] = -sqrt(-.25 * sqm);
214  *Nsol = 2;
215  }
216  else if ( (sqp < 0.0) && (sqm >= 0.0))
217  {
218  //printf("\n case 3 ");
219  sol[0] = 0.5 * (-xK + sqrt(sqm));
220  sol[1] = 0.5 * (-xK - sqrt(sqm));
221  sol[2] = 0.5 * xK;
222  sol[3] = 0.5 * xK;
223  soli[2] = sqrt(-0.25 * sqp);
224  soli[3] = -sqrt(-0.25 * sqp);
225  *Nsol = 2;
226  }
227  else if ( (sqp < 0.0) && (sqm < 0.0))
228  {
229  //printf("\n case 4 ");
230  sol[0] = -0.5 * xK;
231  sol[1] = -0.5 * xK;
232  soli[0] = sqrt(-0.25 * sqm);
233  soli[1] = -sqrt(-0.25 * sqm);
234  sol[2] = 0.5 * xK;
235  sol[3] = 0.5 * xK;
236  soli[2] = sqrt(-0.25 * sqp);
237  soli[3] = -sqrt(-0.25 * sqp);
238  *Nsol = 0;
239  }
240 
241  for (i=0;i<4;i++) sol[i] -= b/(4.0*a);
242  return 0;
243  }
244 
245 
246  /*-------------------- Global Function Description Block ----------------------
247  *
248  * ***CUBIC************************************************08.11.1986
249  * Solution of a cubic equation
250  * Equations of lesser degree are solved by the appropriate formulas.
251  * The solutions are arranged in ascending order.
252  * NO WARRANTY, ALWAYS TEST THIS SUBROUTINE AFTER DOWNLOADING
253  * ******************************************************************
254  * A(0:3) (i) vector containing the polynomial coefficients
255  * X(1:L) (o) results
256  * L (o) number of valid solutions (beginning with X(1))
257  * ==================================================================
258  * 17-Oct-2004 / Raoul Rausch
259  * Conversion from Fortran to C
260  *
261  *
262  *-----------------------------------------------------------------------------
263  */
264 
265 int cubic(double A[4], double X[3], int* L)
266 {
267  const double PI = 3.1415926535897932;
268  const double THIRD = 1./3.;
269  double U[3],W, P, Q, DIS, PHI;
270  int i;
271 
272  //define cubic root as statement function
273  // In C, the function is defined outside of the cubic fct
274 
275  // ====determine the degree of the polynomial ====
276 
277  if (A[3] != 0.0)
278  {
279  //cubic problem
280  W = A[2]/A[3]*THIRD;
281  P = pow((A[1]/A[3]*THIRD - pow(W,2)),3);
282  Q = -.5*(2.0*pow(W,3)-(A[1]*W-A[0])/A[3] );
283  DIS = pow(Q,2)+P;
284  if ( DIS < 0.0 )
285  {
286  //three real solutions!
287  //Confine the argument of ACOS to the interval [-1;1]!
288  PHI = acos(min(1.0,max(-1.0,Q/sqrt(-P))));
289  P=2.0*pow((-P),(5.e-1*THIRD));
290  for (i=0;i<3;i++) U[i] = P*cos((PHI+2*((double)i)*PI)*THIRD)-W;
291  X[0] = min(U[0], min(U[1], U[2]));
292  X[1] = max(min(U[0], U[1]),max( min(U[0], U[2]), min(U[1], U[2])));
293  X[2] = max(U[0], max(U[1], U[2]));
294  *L = 3;
295  }
296  else
297  {
298  // only one real solution!
299  DIS = sqrt(DIS);
300  X[0] = CBRT(Q+DIS)+CBRT(Q-DIS)-W;
301  *L=1;
302  }
303  }
304  else if (A[2] != 0.0)
305  {
306  // quadratic problem
307  P = 0.5*A[1]/A[2];
308  DIS = pow(P,2)-A[0]/A[2];
309  if (DIS > 0.0)
310  {
311  // 2 real solutions
312  X[0] = -P - sqrt(DIS);
313  X[1] = -P + sqrt(DIS);
314  *L=2;
315  }
316  else
317  {
318  // no real solution
319  *L=0;
320  }
321  }
322  else if (A[1] != 0.0)
323  {
324  //linear equation
325  X[0] =A[0]/A[1];
326  *L=1;
327  }
328  else
329  {
330  //no equation
331  *L=0;
332  }
333  //
334  // ==== perform one step of a newton iteration in order to minimize
335  // round-off errors ====
336  //
337  for (i=0;i<*L;i++)
338  {
339  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]));
340  // printf("\n X inside cubic %.15e\n", X[i]);
341  }
342 
343  return 0;
344 }
345 
346 
347 int signR(double Z)
348 {
349  int ret = 0;
350  if (Z > 0.0) ret = 1;
351  if (Z < 0.0) ret = -1;
352  if (Z == 0.0) ret =0;
353 
354  return ret;
355 }
356 
357 double CBRT(double Z)
358 {
359  double ret;
360  const double THIRD = 1./3.;
361  //define cubic root as statement function
362  //SIGN has different meanings in both C and Fortran
363  // Was unable to use the sign command of C, so wrote my own
364  // that why a new variable needs to be introduced that keeps track of the sign of
365  // SIGN is supposed to return a 1, -1 or 0 depending on what the sign of the argument is
366  ret = fabs(pow(fabs(Z),THIRD)) * signR(Z);
367  return ret;
368 }
369 
370 
371 } // namespace rpp
372 
373 
374 #endif //_NO_LIBRPP_
d
d
f
f
rpp
Definition: rpp.cpp:55
rpp::quartic
int quartic(double[], double[], double[], int *)
rpp::quintic
int quintic(double[], double[], double[], int *, double)
rpp::cubic
int cubic(double[], double[], int *)
z
TFSIMD_FORCE_INLINE const tfScalar & z() const
rpp::signR
int signR(double)
Definition: rpp_quintic.cpp:347
min
#define min(a, b)
Definition: rpp_quintic.cpp:45
max
#define max(a, b)
Definition: rpp_quintic.cpp:44
rpp::CBRT
double CBRT(double)
Definition: rpp_quintic.cpp:357

tuw_artoolkitplus
Author(s): Markus Bader
autogenerated on Sun Sep 4 2016 03:24:33