home *** CD-ROM | disk | FTP | other *** search
/ Frozen Fish 1: Amiga / FrozenFish-Apr94.iso / bbs / alib / d5xx / d513 / dkbtrace.lha / DKBTrace / dkb212sr.lzh / vect.c < prev    next >
C/C++ Source or Header  |  1991-06-08  |  16KB  |  628 lines
  1. /*****************************************************************************
  2. *
  3. *                                   vect.c
  4. *
  5. *   from DKBTrace (c) 1990  David Buck
  6. *
  7. *  This module implements additional quartic math-helping functions.
  8. *
  9. *  This file was written by Alexander Enzmann.  He wrote the code for DKB 2.11
  10. *  QUARTICS (4th order shapes) and generously provided us these enhancements.
  11. *
  12. * This software is freely distributable. The source and/or object code may be
  13. * copied or uploaded to communications services so long as this notice remains
  14. * at the top of each file.  If any changes are made to the program, you must
  15. * clearly indicate in the documentation and in the programs startup message
  16. * who it was who made the changes. The documentation should also describe what
  17. * those changes were. This software may not be included in whole or in
  18. * part into any commercial package without the express written consent of the
  19. * author.  It may, however, be included in other public domain or freely
  20. * distributed software so long as the proper credit for the software is given.
  21. *
  22. * This software is provided as is without any guarantees or warranty. Although
  23. * the author has attempted to find and correct any bugs in the software, he
  24. * is not responsible for any damage caused by the use of the software.  The
  25. * author is under no obligation to provide service, corrections, or upgrades
  26. * to this package.
  27. *
  28. * Despite all the legal stuff above, if you do find bugs, I would like to hear
  29. * about them.  Also, if you have any comments or questions, you may contact me
  30. * at the following address:
  31. *
  32. *     David Buck
  33. *     22C Sonnet Cres.
  34. *     Nepean Ontario
  35. *     Canada, K2H 8W7
  36. *
  37. *  I can also be reached on the following bulleton boards:
  38. *
  39. *     OMX              (613) 731-3419
  40. *     Mystic           (613) 596-4249  or  (613) 596-4772
  41. *
  42. *  Fidonet:   1:163/109.9
  43. *  Internet:  dbuck@ccs.carleton.ca
  44. *  The "You Can Call Me RAY" BBS    (708) 358-5611
  45. *
  46. *  IBM Port by Aaron A. Collins. Aaron may be reached on the following BBS'es:
  47. *
  48. *     The "You Can Call Me RAY" BBS (708) 358-5611
  49. *     The Information Exchange BBS  (708) 945-5575
  50. *
  51. *****************************************************************************/
  52.  
  53. #include <stdio.h>
  54. #include <math.h>
  55. #include "frame.h"
  56. #include "dkbproto.h"
  57.  
  58. #ifdef ABS
  59. #undef ABS
  60. #endif
  61.  
  62. #define ABS(x) ((x) < 0.0 ? (0.0 - x) : (x))
  63.  
  64. #ifndef PI
  65. #define PI   3.141592653589793238462643383280
  66. #endif
  67.  
  68. #ifndef TWO_PI
  69. #define TWO_PI 6.283185207179586476925286766560
  70. #endif
  71.  
  72. #define TWO_PI_3  2.0943951023931954923084
  73. #define TWO_PI_43 4.1887902047863909846168
  74. #define MINUS_INFINITY -1.0e10
  75. #define POSITIVE_INFINITY 1.0e10
  76.  
  77. #define EPSILON 1.0e-5
  78.  
  79. #ifdef TEST
  80. void
  81. show_poly(label, order, Coeffs)
  82.    char *label;
  83.    int order;
  84.    DBL *Coeffs;
  85. {
  86.    int i;
  87.    printf("%s", label);
  88.    for (i=0;i<=order;i++) {
  89.       printf("%.2lf x^%d", Coeffs[i], order-i);
  90.       if (i==order) printf("\n");
  91.       else printf(" + ");
  92.       }
  93. }
  94.  
  95. void
  96. show_points(label, count, point_list)
  97.    char *label;
  98.    int count;
  99.    DBL *point_list;
  100. {
  101.    int i;
  102.    printf("%s", label);
  103.    for (i=0;i<count;i++) {
  104.       printf("%lf", point_list[i]);
  105.       if (i==(count-1)) printf("\n");
  106.       else printf(", ");
  107.       }
  108. }
  109.  
  110. /* Deflate a polynomial by the factor (x - a). */
  111. void
  112. deflate(n, coeffs, a)
  113.    int n;
  114.    DBL *coeffs;
  115.    DBL a;
  116. {
  117.    register int i;
  118.    DBL temp, rem = coeffs[0];
  119.    coeffs[0] = 0.0;
  120.    for (i=1;i<=n;i++) {
  121.       temp = coeffs[i];
  122.       coeffs[i] = rem;
  123.       rem = temp + rem * a;
  124.       }
  125. }
  126.  
  127. /* find the coefficients of the derivative of a polynomial */
  128. void
  129. dydx(order, coeffs, dcoeffs)
  130.    int order;
  131.    DBL *coeffs;
  132.    DBL *dcoeffs;
  133. {
  134.    register int i;
  135.    for (i=0;i<order;i++)
  136.       dcoeffs[i] = (order - i) * coeffs[i];
  137. }
  138.  
  139. #define MAX_ITERATIONS 100
  140. typedef void (*polyfunction) PARAMS((DBL x, int n, DBL *Coeffs, DBL *val, DBL *dx));
  141.  
  142. void
  143. polyeval(x, n, Coeffs, val, dx)
  144.    DBL x
  145.    int n;
  146.    DBL *Coeffs, *val, *dx;
  147. {
  148.    register int i;
  149.    *val = Coeffs[0];
  150.    *dx = 0.0;
  151.    for (i=1;i<=n;i++) {
  152.       *dx = *dx * x + *val;
  153.       *val = *val * x + Coeffs[i];
  154.       }
  155. }
  156.  
  157. /* Solve for a single root of a polynomial, using a bracketed
  158.    Newton-Raphson technique. 1 = root found, 0 = failure. */
  159. int
  160. rtsafe(funcd, n, Coeffs, x1, x2, epsilon, result)
  161.    polyfunction funcd;
  162.    int n;
  163.    DBL *Coeffs, x1, x2, epsilon, *result;
  164. {
  165.    int j;
  166.    DBL df, dx, dxold, f, fh, fl;
  167.    DBL swap, temp, xh, xl, rts;
  168.    (*funcd)(x1, n, Coeffs, &fl, &df);
  169.    (*funcd)(x2, n, Coeffs, &fh, &df);
  170.    if (ABS(fl) < epsilon) {
  171.       /* Root is on the lower bound */
  172.       *result = x1;
  173.       return 1;
  174.       }
  175.    else if (ABS(fh) < epsilon) {
  176.       /* Root is on the upper bound */
  177.       *result = x2;
  178.       return 1;
  179.       }
  180.    else if (fl * fh >= 0.0) {
  181.       /* Bad bounds - function should change signs over the given interval */
  182.       *result = 0.0;
  183.       return 0;
  184.       }
  185.    if (fl < 0.0) {
  186.       xl = x1;
  187.       xh = x2;
  188.       }
  189.    else {
  190.       xh = x1;
  191.       xl = x2;
  192.       swap = fl;
  193.       fl = fh;
  194.       fh = swap;
  195.       }
  196.    rts = 0.5 * (x1 + x2);
  197.    dxold = ABS(x2-x1);
  198.    dx = dxold;
  199.    (*funcd)(rts, n, Coeffs, &f, &df);
  200.    for (j=1;j<=MAX_ITERATIONS;j++) {
  201.       if ((((rts-xh)*df-f)*((rts-xl)*df-f) >= 0.0) ||
  202.      /* Newton out of range, bisect instead */
  203.       (ABS(2.0*f) > ABS(dxold*df))) {
  204.      /* Newton not converging fast enough, bisect instead */
  205.      dxold = dx;
  206.      dx = 0.5 * (xh-xl);
  207.      rts = xl + dx;
  208.      if (xl == rts) {
  209.         *result = rts;
  210.         return 1;
  211.         }
  212.      }
  213.       else {
  214.      dxold = dx;
  215.      dx = f / df;
  216.      temp = rts;
  217.      rts -= dx;
  218.      if (temp == rts) {
  219.         *result = rts;
  220.         return 1;
  221.         }
  222.      }
  223.       if (ABS(dx) < epsilon) {
  224.      *result = rts;
  225.      return 1;
  226.      }
  227.       (*funcd)(rts, n, Coeffs, &f, &df);
  228.       if (f < 0.0) {
  229.      xl = rts;
  230.      fl = f;
  231.      }
  232.       else {
  233.      xh = rts;
  234.      fh = f;
  235.      }
  236.       }
  237.    *result = 0.0;
  238.    return 0;
  239. }
  240. #endif
  241.  
  242. /*
  243.    Solve the quadratic equation:
  244.       x[0] * x^2 + x[1] * x + x[2] = 0.
  245.  
  246.    The value returned by this function is the number of real roots.
  247.    The roots themselves are returned in y[0], y[1].
  248. */
  249. solve_quadratic(x, y)
  250.    DBL *x, *y;
  251. {
  252.    DBL d, t, a, b, c;
  253.    a = x[0];
  254.    b = -x[1];
  255.    c = x[2];
  256.    if (a == 0.0) {
  257.       if (b == 0.0)
  258.          return 0;
  259.       y[0] = -c / b;
  260.       return 1;
  261.       }
  262.    d = b * b - 4.0 * a * c;
  263.    if (d < 0.0) return 0;
  264.    d = sqrt(d);
  265.    t = 2.0 * a;
  266.    y[0] = (b + d) / t;
  267.    y[1] = (b - d) / t;
  268.    return 2;
  269. }
  270.  
  271. /*
  272.    Solve the cubic equation:
  273.  
  274.       x[0] * x^3 + x[1] * x^2 + x[2] * x + x[3] = 0.
  275.  
  276.    The result of this function is an integer that tells how many real
  277.    roots exist.  Determination of how many are distinct is up to the
  278.    process that calls this routine.  The roots that exist are stored
  279.    in (y[0], y[1], y[2]).
  280.  
  281.    Note: this function relies very heavily on trigonometric functions and
  282.    the square root function.  If an alternative solution is found that does
  283.    not rely on transcendentals this code will be replaced.
  284. */
  285. int
  286. solve_cubic(x, y)
  287.    DBL *x, *y;
  288. {
  289.    DBL Q, R, Q3, R2, sQ, d, an, theta;
  290.    DBL A2, a0, a1, a2, a3;
  291.    a0 = x[0];
  292.    if (a0 == 0.0) return solve_quadratic(&x[1], y);
  293.    else if (a0 != 1.0) {
  294.       a1 = x[1] / a0;
  295.       a2 = x[2] / a0;
  296.       a3 = x[3] / a0;
  297.       }
  298.    else {
  299.       a1 = x[1];
  300.       a2 = x[2];
  301.       a3 = x[3];
  302.       }
  303.    A2 = a1 * a1;
  304.    Q = (A2 - 3.0 * a2) / 9.0;
  305.    R = (2.0 * A2 * a1 - 9.0 * a1 * a2 + 27.0 * a3) / 54.0;
  306.    Q3 = Q * Q * Q;
  307.    R2 = R * R;
  308.    d = Q3 - R2;
  309.    an = a1 / 3.0;
  310.    if (d >= 0.0) {
  311.       /* Three real roots. */
  312.       d = R / sqrt(Q3);
  313.       theta = acos(d) / 3.0;
  314.       sQ = -2.0 * sqrt(Q);
  315.       y[0] = sQ * cos(theta) - an;
  316.       y[1] = sQ * cos(theta + TWO_PI_3) - an;
  317.       y[2] = sQ * cos(theta + TWO_PI_43) - an;
  318.       return 3;
  319.       }
  320.    else {
  321.       sQ = pow(sqrt(R2 - Q3) + ABS(R), 1.0 / 3.0);
  322.       if (R < 0)
  323.      y[0] = (sQ + Q / sQ) - an;
  324.       else
  325.      y[0] = -(sQ + Q / sQ) - an;
  326.       return 1;
  327.       }
  328. }
  329.  
  330. /*
  331.    Solve the quartic equation:
  332.       x[0] x^4 + x[1] x^3 + x[2] x^2 + x[3] x + x[4] = 0
  333.    The value of this function is the number of real roots. The roots
  334.    themselves are returned in results[0-3].
  335.  
  336.    Two methods of implementation are given, the first is faster by
  337.    than the second by approximately 7-8% (with both in unoptimized form).
  338.    Hand optimization of the first function has been implemented, resulting
  339.    in a boost of around 15% of its speed.
  340. */
  341.  
  342. /* Solve a quartic using the method of Lodovico Ferrari (Circa 1731) */
  343. int
  344. solve_quartic(x, results)
  345.    DBL *x, *results;
  346. {
  347.    DBL cubic[4], roots[3];
  348.    DBL a0, a1, y, d1, x1, t1, t2;
  349.    DBL c0, c1, c2, c3, c4, d2, q1, q2;
  350.    int i, j;
  351.    c0 = x[0];
  352.    if (c0 == 0.0)
  353.       return solve_cubic(&x[1], results);
  354.    else if (x[4] == 0.0) {
  355.       results[0] = 0.0;
  356.       return 1 + solve_cubic(x, &results[1]);
  357.       }
  358.    else if (c0 != 1.0) {
  359.       c1 = x[1] / c0;
  360.       c2 = x[2] / c0;
  361.       c3 = x[3] / c0;
  362.       c4 = x[4] / c0;
  363.       }
  364.    else {
  365.       c1 = x[1];
  366.       c2 = x[2];
  367.       c3 = x[3];
  368.       c4 = x[4];
  369.       }
  370.  
  371.    /* The first step is to take the original equation:
  372.  
  373.      x^4 + b*x^3 + c*x^2 + d*x + e = 0
  374.  
  375.       and rewrite it as:
  376.  
  377.      x^4 + b*x^3 = -c*x^2 - d*x - e,
  378.  
  379.       adding (b*x/2)^2 + (x^2 + b*x/2)y + y^2/4 to each side gives a
  380.       perfect square on the lhs:
  381.  
  382.      (x^2 + b*x/2 + y/2)^2 = (b^2/4 - c + y)x^2 + (b*y/2 - d)x + y^2/4 - e
  383.  
  384.       By choosing the appropriate value for y, the rhs can be made a perfect
  385.       square also.  This value is found when the rhs is treated as a quadratic
  386.       in x with the discriminant equal to 0.  This will be true when:
  387.  
  388.      (b*y/2 - d)^2 - 4.0 * (b^2/4 - c*y)*(y^2/4 - e) = 0, or
  389.  
  390.      y^3 - c*y^2 + (b*d - 4*e)*y - b^2*e + 4*c*e - d^2 = 0.
  391.  
  392.       This is called the resolvent of the quartic equation.  */
  393.  
  394.    a0 = 4.0 * c4;
  395.    cubic[0] = 1.0;
  396.    cubic[1] = -1.0 * c2;
  397.    cubic[2] = c1 * c3 - a0;
  398.    cubic[3] = a0 * c2 - c1 * c1 * c4 - c3 * c3;
  399.    i = solve_cubic(&cubic[0], &roots[0]);
  400.    if (i == 3)
  401.       y = max(roots[0], max(roots[1], roots[2]));
  402.    else
  403.       y = roots[0];
  404.  
  405.    /* What we are left with is a quadratic squared on the lhs and a
  406.       linear term on the right.  The linear term has one of two signs,
  407.       take each and add it to the lhs.  The form of the quartic is now:
  408.  
  409.      a' = b^2/4 - c + y,    b' = b*y/2 - d, (from rhs quadritic above)
  410.  
  411.      (x^2 + b*x/2 + y/2) = +sqrt(a'*(x + 1/2 * b'/a')^2), and
  412.      (x^2 + b*x/2 + y/2) = -sqrt(a'*(x + 1/2 * b'/a')^2).
  413.  
  414.       By taking the linear term from each of the right hand sides and
  415.       adding to the appropriate part of the left hand side, two quadratic
  416.       formulas are created.  By solving each of these the four roots of
  417.       the quartic are determined.
  418.    */
  419.    i = 0;
  420.    a0 = c1 / 2.0;
  421.    a1 = y / 2.0;
  422.  
  423.    t1 = a0 * a0 - c2 + y;
  424.    if (t1 < 0.0) {
  425.       /* First Special case, a' < 0 means all roots are complex. */
  426.     if (t1 > -EPSILON)
  427.         t1 = 0;
  428.     else return 0;
  429.     }
  430.    if (t1 == 0) {
  431.       /* Second special case, the "x" term on the right hand side above
  432.      has vanished.  In this case:
  433.         (x^2 + b*x/2 + y/2) = +sqrt(y^2/4 - e), and
  434.         (x^2 + b*x/2 + y/2) = -sqrt(y^2/4 - e).  */
  435.       t2 = a1 * a1 - c4;
  436.       if (t2 < 0.0) return 0;
  437.       x1 = 0.0;
  438.       d1 = sqrt(t2);
  439.       }
  440.    else {
  441.       x1 = sqrt(t1);
  442.       d1 = 0.5 * (a0 * y - c3) / x1;
  443.       }
  444.    /* Solve the first quadratic */
  445.    q1 = -a0 - x1;
  446.    q2 = a1 + d1;
  447.    d2 = q1 * q1 - 4.0 * q2;
  448.    if (d2 >= 0.0) {
  449.       d2 = sqrt(d2);
  450.       results[0] = 0.5 * (q1 + d2);
  451.       results[1] = 0.5 * (q1 - d2);
  452.       i = 2;
  453.       }
  454.    /* Solve the second quadratic */
  455.    q1 = q1 + x1 + x1;
  456.    q2 = a1 - d1;
  457.    d2 = q1 * q1 - 4.0 * q2;
  458.    if (d2 >= 0.0) {
  459.       d2 = sqrt(d2);
  460.       results[i++] = 0.5 * (q1 + d2);
  461.       results[i++] = 0.5 * (q1 - d2);
  462.       }
  463.    return i;
  464. }
  465.  
  466. #ifdef TEST
  467. /* Solve a quartic using the method of Francois Vieta (Circa 1735) */
  468. int
  469. solve_quartic1(x, results)
  470.    DBL *x, *results;
  471. {
  472.    DBL cubic[4], roots[3];
  473.    DBL A2, y, z, p, q, r, d1, d2;
  474.    int i, j, k;
  475.    if (x[0] == 0.0)
  476.       return solve_cubic(&x[1], results);
  477.    else if (x[4] == 0.0) {
  478.       results[0] = 0.0;
  479.       return 1 + solve_cubic(x, &results[1]);
  480.       }
  481.    else if (x[0] != 1.0)
  482.       for (i=1;i<5;i++) x[i] /= x[0];
  483.  
  484.    /* Compute the cubic resolvant */
  485.    A2 = x[1] * x[1];
  486.    p = -3.0 * A2 / 8.0 + x[2];
  487.    q = A2 * x[1] / 8.0 - x[1] * x[2] / 2.0 + x[3];
  488.    r = -3.0 * A2 * A2 / 256.0 + A2 * x[2] / 16.0 - x[1] * x[3] / 4.0 + x[4];
  489.  
  490.    cubic[0] = 1.0;
  491.    cubic[1] = -1.0 * p / 2.0;
  492.    cubic[2] = -1.0 * r;
  493.    cubic[3] = r * p / 2.0 - q * q / 8.0;
  494.    k = solve_cubic(&cubic[0], &roots[0]);
  495.    z = roots[0];
  496.    if (k == 3)
  497.       z = max(max(roots[0], roots[1]), roots[2]);
  498.    else
  499.       z = roots[0];
  500.    d1 = 2.0 * z - p;
  501.    if (d1 < 0.0)
  502.       return 0;
  503.    else if (d1 == 0.0) {
  504.       d2 = z * z - r;
  505.       if (d2 < 0.0) return 0;
  506.       d2 = sqrt(d2);
  507.       }
  508.    else {
  509.       d1 = sqrt(d1);
  510.       d2 = 0.5 * q / d1;
  511.       }
  512.    i = 0;
  513.    cubic[0] = 1.0;
  514.    cubic[1] = d1;
  515.    cubic[2] = z - d2;
  516.    j = solve_quadratic(&cubic[0], &roots[0]);
  517.    if (j==1)
  518.       results[i++] = roots[0] - x[1] / 4.0;
  519.    else if (j == 2) {
  520.       results[i++] = roots[0] - x[1] / 4.0;
  521.       results[i++] = roots[1] - x[1] / 4.0;
  522.       }
  523.    cubic[1] = -d1;
  524.    cubic[2] = z + d2;
  525.    j = solve_quadratic(&cubic[0], &roots[0]);
  526.    if (j == 0)
  527.       return i;
  528.    else if (j==1)
  529.       results[i++] = roots[0] - x[1] / 4.0;
  530.    else {
  531.       results[i++] = roots[0] - x[1] / 4.0;
  532.       results[i++] = roots[1] - x[1] / 4.0;
  533.       }
  534.    return i;
  535. }
  536.  
  537. #ifndef __TURBOC__
  538. int _FAR_ _cdecl
  539. dbl_sort_func(const void _FAR_ *a, const void _FAR_ *b)
  540. #else
  541. int
  542. dbl_sort_func(a, b)
  543.    void *a, *b;
  544. #endif
  545. {
  546.    return *((DBL *)a) - *((DBL *)b);
  547. }
  548.  
  549. /*
  550.    Solve the nth order equation:
  551.       x[0] x^n + x[1] x^(n-1) + ... + x[n-1] x + x[n] = 0
  552.    The value of this function is the number of real roots. The roots
  553.    themselves are returned in roots[0-(n-1)]. The roots will be in sorted
  554.    order when they are returned.
  555. */
  556. int
  557. polysolve(order, Coeffs, roots)
  558.    int order;
  559.    DBL *Coeffs;
  560.    DBL *roots;
  561. {
  562.    DBL val, dx;
  563.    DBL *inflections, *derivative, root;
  564.    register int i, j;
  565.    int ipoints, rootcount;
  566.    if (order == 2) {
  567.       rootcount = solve_quadratic(Coeffs, roots);  
  568.       if (rootcount == 2 && roots[0] > roots[1]) {
  569.      root = roots[0];
  570.      roots[0] = roots[1];
  571.      roots[1] = root;
  572.      }
  573.       }
  574.    else if (order == 3) {
  575.       rootcount = solve_cubic(Coeffs, roots);
  576. #ifndef __TURBOC__
  577.       qsort((void _FAR_ *)roots, rootcount, sizeof(DBL), dbl_sort_func);
  578. #else
  579.       qsort((void *)roots, rootcount, sizeof(DBL), dbl_sort_func);
  580. #endif
  581.       }
  582.    else if (order == 4) {
  583.       rootcount = solve_quartic(Coeffs, roots);
  584.       qsort((void *)roots, (size_t)rootcount, sizeof(DBL), dbl_sort_func);
  585.       }
  586.    else {
  587.       inflections = (DBL *)malloc((order+1) * sizeof(DBL));
  588.       if (!inflections) {
  589.      printf("Failed to allocate memory for inflections in polysolve\n");
  590.      exit(1);
  591.      }
  592.       derivative = (DBL *)malloc(order * sizeof(DBL));
  593.       if (!derivative) {
  594.      printf("Failed to allocate memory for derivative in polysolve\n");
  595.      exit(1);
  596.      }
  597.       dydx(order, Coeffs, derivative);
  598.       /* show_poly("Derivs  : ", order-1, derivative); */
  599.       ipoints = polysolve(order-1, derivative, &inflections[1]);
  600.       /* show_points("Inflect : ", ipoints, &inflections[1]); */
  601.       /* Strip out redundant inflection points */
  602.       for (i=1;i<ipoints;)
  603.      if (inflections[i] == inflections[i+1]) {
  604.         for (j=i;j<ipoints;j++)
  605.            inflections[j] = inflections[j+1];
  606.         ipoints--;
  607.         }
  608.      else
  609.         i++;
  610.       /* show_points("Stripped: ", ipoints, &inflections[1]); */
  611.       /* Add negative and positive infinity to the list */
  612.       inflections[0] = MINUS_INFINITY;
  613.       inflections[ipoints+1] = POSITIVE_INFINITY;
  614.       /* Use bounded Newton-Raphson to solve for roots between points of
  615.          inflection. (When their values differ in sign.) */
  616.       rootcount = 0;
  617.       for (i=0;i<=ipoints;i++)
  618.      if (rtsafe(polyeval, order, Coeffs, inflections[i],
  619.             inflections[i+1], EPSILON, &root))
  620.         roots[rootcount++] = root;
  621.       /* Free up the temporary memory */
  622.       free(derivative);
  623.       free(inflections);
  624.       }
  625.    return rootcount;
  626. }
  627. #endif
  628.