home *** CD-ROM | disk | FTP | other *** search
/ Usenet 1994 October / usenetsourcesnewsgroupsinfomagicoctober1994disk2.iso / misc / volume15 / ggems / part03 / Roots3And4.c < prev   
C/C++ Source or Header  |  1990-10-14  |  4KB  |  231 lines
  1. /*
  2.  *  Roots3And4.c
  3.  *
  4.  *  Utility functions to find cubic and quartic roots,
  5.  *  coefficients are passed like this:
  6.  *
  7.  *      c[0] + c[1]*x + c[2]*x^2 + c[3]*x^3 + c[4]*x^4 = 0
  8.  *
  9.  *  The functions return the number of non-complex roots and
  10.  *  put the values into the s array.
  11.  *
  12.  *  Author:         Jochen Schwarze (schwarze@isa.de)
  13.  *
  14.  *  Jan 26, 1990    Version for Graphics Gems
  15.  *  Oct 11, 1990    Fixed sign problem for negative q's in SolveQuartic
  16.  *                  (reported by Mark Podlipec),
  17.  *                  Old-style function definitions,
  18.  *                  IsZero() as a macro
  19.  */
  20.  
  21. #include    <math.h>
  22.  
  23. /* epsilon surrounding for near zero values */
  24.  
  25. #define     EQN_EPS     1e-9
  26. #define        IsZero(x)    ((x) > -EQN_EPS && (x) < EQN_EPS)
  27.  
  28.  
  29. int SolveQuadric(c, s)
  30.     double c[ 3 ];
  31.     double s[ 2 ];
  32. {
  33.     double p, q, D;
  34.  
  35.     /* normal form: x^2 + px + q = 0 */
  36.  
  37.     p = c[ 1 ] / (2 * c[ 2 ]);
  38.     q = c[ 0 ] / c[ 2 ];
  39.  
  40.     D = p * p - q;
  41.  
  42.     if (IsZero(D))
  43.     {
  44.     s[ 0 ] = - p;
  45.     return 1;
  46.     }
  47.     else if (D < 0)
  48.     {
  49.     return 0;
  50.     }
  51.     else if (D > 0)
  52.     {
  53.     double sqrt_D = sqrt(D);
  54.  
  55.     s[ 0 ] =   sqrt_D - p;
  56.     s[ 1 ] = - sqrt_D - p;
  57.     return 2;
  58.     }
  59. }
  60.  
  61.  
  62. int SolveCubic(c, s)
  63.     double c[ 4 ];
  64.     double s[ 3 ];
  65. {
  66.     int     i, num;
  67.     double  sub;
  68.     double  A, B, C;
  69.     double  sq_A, p, q;
  70.     double  cb_p, D;
  71.  
  72.     /* normal form: x^3 + Ax^2 + Bx + C = 0 */
  73.  
  74.     A = c[ 2 ] / c[ 3 ];
  75.     B = c[ 1 ] / c[ 3 ];
  76.     C = c[ 0 ] / c[ 3 ];
  77.  
  78.     /*  substitute x = y - A/3 to eliminate quadric term:
  79.     x^3 +px + q = 0 */
  80.  
  81.     sq_A = A * A;
  82.     p = 1.0/3 * (- 1.0/3 * sq_A + B);
  83.     q = 1.0/2 * (2.0/27 * A * sq_A - 1.0/3 * A * B + C);
  84.  
  85.     /* use Cardano's formula */
  86.  
  87.     cb_p = p * p * p;
  88.     D = q * q + cb_p;
  89.  
  90.     if (IsZero(D))
  91.     {
  92.     if (IsZero(q)) /* one triple solution */
  93.     {
  94.         s[ 0 ] = 0;
  95.         num = 1;
  96.     }
  97.     else /* one single and one double solution */
  98.     {
  99.         double u = cbrt(-q);
  100.         s[ 0 ] = 2 * u;
  101.         s[ 1 ] = - u;
  102.         num = 2;
  103.     }
  104.     }
  105.     else if (D < 0) /* Casus irreducibilis: three real solutions */
  106.     {
  107.     double phi = 1.0/3 * acos(-q / sqrt(-cb_p));
  108.     double t = 2 * sqrt(-p);
  109.  
  110.     s[ 0 ] =   t * cos(phi);
  111.     s[ 1 ] = - t * cos(phi + M_PI / 3);
  112.     s[ 2 ] = - t * cos(phi - M_PI / 3);
  113.     num = 3;
  114.     }
  115.     else /* one real solution */
  116.     {
  117.     double sqrt_D = sqrt(D);
  118.     double u = cbrt(sqrt_D - q);
  119.     double v = - cbrt(sqrt_D + q);
  120.  
  121.     s[ 0 ] = u + v;
  122.     num = 1;
  123.     }
  124.  
  125.     /* resubstitute */
  126.  
  127.     sub = 1.0/3 * A;
  128.  
  129.     for (i = 0; i < num; ++i)
  130.     s[ i ] -= sub;
  131.  
  132.     return num;
  133. }
  134.  
  135.  
  136. int SolveQuartic(c, s)
  137.     double c[ 5 ]; 
  138.     double s[ 4 ];
  139. {
  140.     double  coeffs[ 4 ];
  141.     double  z, u, v, sub;
  142.     double  A, B, C, D;
  143.     double  sq_A, p, q, r;
  144.     int     i, num;
  145.  
  146.     /* normal form: x^4 + Ax^3 + Bx^2 + Cx + D = 0 */
  147.  
  148.     A = c[ 3 ] / c[ 4 ];
  149.     B = c[ 2 ] / c[ 4 ];
  150.     C = c[ 1 ] / c[ 4 ];
  151.     D = c[ 0 ] / c[ 4 ];
  152.  
  153.     /*  substitute x = y - A/4 to eliminate cubic term:
  154.     x^4 + px^2 + qx + r = 0 */
  155.  
  156.     sq_A = A * A;
  157.     p = - 3.0/8 * sq_A + B;
  158.     q = 1.0/8 * sq_A * A - 1.0/2 * A * B + C;
  159.     r = - 3.0/256*sq_A*sq_A + 1.0/16*sq_A*B - 1.0/4*A*C + D;
  160.  
  161.     if (IsZero(r))
  162.     {
  163.     /* no absolute term: y(y^3 + py + q) = 0 */
  164.  
  165.     coeffs[ 0 ] = q;
  166.     coeffs[ 1 ] = p;
  167.     coeffs[ 2 ] = 0;
  168.     coeffs[ 3 ] = 1;
  169.  
  170.     num = SolveCubic(coeffs, s);
  171.  
  172.     s[ num++ ] = 0;
  173.     }
  174.     else
  175.     {
  176.     /* solve the resolvent cubic ... */
  177.  
  178.     coeffs[ 0 ] = 1.0/2 * r * p - 1.0/8 * q * q;
  179.     coeffs[ 1 ] = - r;
  180.     coeffs[ 2 ] = - 1.0/2 * p;
  181.     coeffs[ 3 ] = 1;
  182.  
  183.     (void) SolveCubic(coeffs, s);
  184.  
  185.     /* ... and take the one real solution ... */
  186.  
  187.     z = s[ 0 ];
  188.  
  189.     /* ... to build two quadric equations */
  190.  
  191.     u = z * z - r;
  192.     v = 2 * z - p;
  193.  
  194.     if (IsZero(u))
  195.         u = 0;
  196.     else if (u > 0)
  197.         u = sqrt(u);
  198.     else
  199.         return 0;
  200.  
  201.     if (IsZero(v))
  202.         v = 0;
  203.     else if (v > 0)
  204.         v = sqrt(v);
  205.     else
  206.         return 0;
  207.  
  208.     coeffs[ 0 ] = z - u;
  209.     coeffs[ 1 ] = q < 0 ? -v : v;
  210.     coeffs[ 2 ] = 1;
  211.  
  212.     num = SolveQuadric(coeffs, s);
  213.  
  214.     coeffs[ 0 ]= z + u;
  215.     coeffs[ 1 ] = q < 0 ? v : -v;
  216.     coeffs[ 2 ] = 1;
  217.  
  218.     num += SolveQuadric(coeffs, s + num);
  219.     }
  220.  
  221.     /* resubstitute */
  222.  
  223.     sub = 1.0/4 * A;
  224.  
  225.     for (i = 0; i < num; ++i)
  226.     s[ i ] -= sub;
  227.  
  228.     return num;
  229. }
  230.  
  231.