home *** CD-ROM | disk | FTP | other *** search
/ RISC DISC 3 / RISC_DISC_3.iso / resources / etexts / gems / gemsi / fitcurves.c < prev    next >
C/C++ Source or Header  |  1993-05-26  |  15KB  |  544 lines
  1. /*
  2. An Algorithm for Automatically Fitting Digitized Curves
  3. by Philip J. Schneider
  4. from "Graphics Gems", Academic Press, 1990
  5. */
  6.  
  7. #define TESTMODE
  8.  
  9. /*  fit_cubic.c    */                                    
  10. /*    Piecewise cubic fitting code    */
  11.  
  12. #include "GraphicsGems.h"                    
  13. #include <stdio.h>
  14. #include <malloc.h>
  15. #include <math.h>
  16.  
  17. typedef Point2 *BezierCurve;
  18.  
  19. /* Forward declarations */
  20. void        FitCurve();
  21. static    void        FitCubic();
  22. static    double        *Reparameterize();
  23. static    double        NewtonRaphsonRootFind();
  24. static    Point2        BezierII();
  25. static    double         B0(), B1(), B2(), B3();
  26. static    Vector2        ComputeLeftTangent();
  27. static    Vector2        ComputeRightTangent();
  28. static    Vector2        ComputeCenterTangent();
  29. static    double        ComputeMaxError();
  30. static    double        *ChordLengthParameterize();
  31. static    BezierCurve    GenerateBezier();
  32. static    Vector2        V2AddII();
  33. static    Vector2        V2ScaleIII();
  34. static    Vector2        V2SubII();
  35.  
  36. #define MAXPOINTS    1000        /* The most points you can have */
  37.  
  38. #ifdef TESTMODE
  39.  
  40. DrawBezierCurve(n, curve)
  41. int n;
  42. BezierCurve curve;
  43. {
  44.     /* You'll have to write this yourself. */
  45. }
  46.  
  47. /*
  48.  *  main:
  49.  *    Example of how to use the curve-fitting code.  Given an array
  50.  *   of points and a tolerance (squared error between points and 
  51.  *    fitted curve), the algorithm will generate a piecewise
  52.  *    cubic Bezier representation that approximates the points.
  53.  *    When a cubic is generated, the routine "DrawBezierCurve"
  54.  *    is called, which outputs the Bezier curve just created
  55.  *    (arguments are the degree and the control points, respectively).
  56.  *    Users will have to implement this function themselves     
  57.  *   ascii output, etc. 
  58.  *
  59.  */
  60. main()
  61. {
  62.     static Point2 d[7] = {    /*  Digitized points */
  63.     { 0.0, 0.0 },
  64.     { 0.0, 0.5 },
  65.     { 1.1, 1.4 },
  66.     { 2.1, 1.6 },
  67.     { 3.2, 1.1 },
  68.     { 4.0, 0.2 },
  69.     { 4.0, 0.0 },
  70.     };
  71.     double    error = 4.0;        /*  Squared error */
  72.     FitCurve(d, 7, error);        /*  Fit the Bezier curves */
  73. }
  74. #endif                         /* TESTMODE */
  75.  
  76. /*
  77.  *  FitCurve :
  78.  *      Fit a Bezier curve to a set of digitized points 
  79.  */
  80. void FitCurve(d, nPts, error)
  81.     Point2    *d;            /*  Array of digitized points    */
  82.     int        nPts;        /*  Number of digitized points    */
  83.     double    error;        /*  User-defined error squared    */
  84. {
  85.     Vector2    tHat1, tHat2;    /*  Unit tangent vectors at endpoints */
  86.  
  87.     tHat1 = ComputeLeftTangent(d, 0);
  88.     tHat2 = ComputeRightTangent(d, nPts - 1);
  89.     FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
  90. }
  91.  
  92.  
  93.  
  94. /*
  95.  *  FitCubic :
  96.  *      Fit a Bezier curve to a (sub)set of digitized points
  97.  */
  98. static void FitCubic(d, first, last, tHat1, tHat2, error)
  99.     Point2    *d;            /*  Array of digitized points */
  100.     int        first, last;    /* Indices of first and last pts in region */
  101.     Vector2    tHat1, tHat2;    /* Unit tangent vectors at endpoints */
  102.     double    error;        /*  User-defined error squared       */
  103. {
  104.     BezierCurve    bezCurve; /*Control points of fitted Bezier curve*/
  105.     double    *u;        /*  Parameter values for point  */
  106.     double    *uPrime;    /*  Improved parameter values */
  107.     double    maxError;    /*  Maximum fitting error     */
  108.     int        splitPoint;    /*  Point to split point set at     */
  109.     int        nPts;        /*  Number of points in subset  */
  110.     double    iterationError; /*Error below which you try iterating  */
  111.     int        maxIterations = 4; /*  Max times to try iterating  */
  112.     Vector2    tHatCenter;       /* Unit tangent vector at splitPoint */
  113.     int        i;        
  114.  
  115.     iterationError = error * error;
  116.     nPts = last - first + 1;
  117.  
  118.     /*  Use heuristic if region only has two points in it */
  119.     if (nPts == 2) {
  120.         double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;
  121.  
  122.         bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
  123.         bezCurve[0] = d[first];
  124.         bezCurve[3] = d[last];
  125.         V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
  126.         V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
  127.         DrawBezierCurve(3, bezCurve);
  128.         return;
  129.     }
  130.  
  131.     /*  Parameterize points, and attempt to fit curve */
  132.     u = ChordLengthParameterize(d, first, last);
  133.     bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
  134.  
  135.     /*  Find max deviation of points to fitted curve */
  136.     maxError = ComputeMaxError(d, first, last, bezCurve, u, &splitPoint);
  137.     if (maxError < error) {
  138.         DrawBezierCurve(3, bezCurve);
  139.         free((void *)u);
  140.         free((void *)bezCurve);
  141.         return;
  142.     }
  143.  
  144.  
  145.     /*  If error not too large, try some reparameterization  */
  146.     /*  and iteration */
  147.     if (maxError < iterationError) {
  148.         for (i = 0; i < maxIterations; i++) {
  149.             uPrime = Reparameterize(d, first, last, u, bezCurve);
  150.             bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
  151.             maxError = ComputeMaxError(d, first, last,
  152.                        bezCurve, uPrime, &splitPoint);
  153.             if (maxError < error) {
  154.             DrawBezierCurve(3, bezCurve);
  155.             free((void *)u);
  156.             free((void *)bezCurve);
  157.             return;
  158.         }
  159.         free((void *)u);
  160.         u = uPrime;
  161.     }
  162.     }
  163.  
  164.     /* Fitting failed -- split at max error point and fit recursively */
  165.     free((void *)u);
  166.     free((void *)bezCurve);
  167.     tHatCenter = ComputeCenterTangent(d, splitPoint);
  168.     FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
  169.     V2Negate(&tHatCenter);
  170.     FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
  171. }
  172.  
  173.  
  174. /*
  175.  *  GenerateBezier :
  176.  *  Use least-squares method to find Bezier control points for region.
  177.  *
  178.  */
  179. static BezierCurve  GenerateBezier(d, first, last, uPrime, tHat1, tHat2)
  180.     Point2    *d;            /*  Array of digitized points    */
  181.     int        first, last;        /*  Indices defining region    */
  182.     double    *uPrime;        /*  Parameter values for region */
  183.     Vector2    tHat1, tHat2;    /*  Unit tangents at endpoints    */
  184. {
  185.     int     i;
  186.     Vector2     A[MAXPOINTS][2];    /* Precomputed rhs for eqn    */
  187.     int     nPts;            /* Number of pts in sub-curve */
  188.     double     C[2][2];            /* Matrix C        */
  189.     double     X[2];            /* Matrix X            */
  190.     double     det_C0_C1,        /* Determinants of matrices    */
  191.                det_C0_X,
  192.                det_X_C1;
  193.     double     alpha_l,        /* Alpha values, left and right    */
  194.                alpha_r;
  195.     Vector2     tmp;            /* Utility variable        */
  196.     BezierCurve    bezCurve;    /* RETURN bezier curve ctl pts    */
  197.  
  198.     bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
  199.     nPts = last - first + 1;
  200.  
  201.  
  202.     /* Compute the A's    */
  203.     for (i = 0; i < nPts; i++) {
  204.         Vector2        v1, v2;
  205.         v1 = tHat1;
  206.         v2 = tHat2;
  207.         V2Scale(&v1, B1(uPrime[i]));
  208.         V2Scale(&v2, B2(uPrime[i]));
  209.         A[i][0] = v1;
  210.         A[i][1] = v2;
  211.     }
  212.  
  213.     /* Create the C and X matrices    */
  214.     C[0][0] = 0.0;
  215.     C[0][1] = 0.0;
  216.     C[1][0] = 0.0;
  217.     C[1][1] = 0.0;
  218.     X[0]    = 0.0;
  219.     X[1]    = 0.0;
  220.  
  221.     for (i = 0; i < nPts; i++) {
  222.         C[0][0] += V2Dot(&A[i][0], &A[i][0]);
  223.         C[0][1] += V2Dot(&A[i][0], &A[i][1]);
  224. /*                    C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/    
  225.         C[1][0] = C[0][1];
  226.         C[1][1] += V2Dot(&A[i][1], &A[i][1]);
  227.  
  228.         tmp = V2SubII(d[first + i],
  229.             V2AddII(
  230.               V2ScaleIII(d[first], B0(uPrime[i])),
  231.                 V2AddII(
  232.                       V2ScaleIII(d[first], B1(uPrime[i])),
  233.                             V2AddII(
  234.                               V2ScaleIII(d[last], B2(uPrime[i])),
  235.                                 V2ScaleIII(d[last], B3(uPrime[i]))))));
  236.     
  237.  
  238.     X[0] += V2Dot(&A[i][0], &tmp);
  239.     X[1] += V2Dot(&A[i][1], &tmp);
  240.     }
  241.  
  242.     /* Compute the determinants of C and X    */
  243.     det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
  244.     det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
  245.     det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];
  246.  
  247.     /* Finally, derive alpha values    */
  248.     if (det_C0_C1 == 0.0) {
  249.         det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
  250.     }
  251.     alpha_l = det_X_C1 / det_C0_C1;
  252.     alpha_r = det_C0_X / det_C0_C1;
  253.  
  254.  
  255.     /*  If alpha negative, use the Wu/Barsky heuristic (see text) */
  256.     if (alpha_l < 0.0 || alpha_r < 0.0) {
  257.         double    dist = V2DistanceBetween2Points(&d[last], &d[first]) /
  258.                     3.0;
  259.  
  260.         bezCurve[0] = d[first];
  261.         bezCurve[3] = d[last];
  262.         V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
  263.         V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
  264.         return (bezCurve);
  265.     }
  266.  
  267.     /*  First and last control points of the Bezier curve are */
  268.     /*  positioned exactly at the first and last data points */
  269.     /*  Control points 1 and 2 are positioned an alpha distance out */
  270.     /*  on the tangent vectors, left and right, respectively */
  271.     bezCurve[0] = d[first];
  272.     bezCurve[3] = d[last];
  273.     V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
  274.     V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
  275.     return (bezCurve);
  276. }
  277.  
  278.  
  279. /*
  280.  *  Reparameterize:
  281.  *    Given set of points and their parameterization, try to find
  282.  *   a better parameterization.
  283.  *
  284.  */
  285. static double *Reparameterize(d, first, last, u, bezCurve)
  286.     Point2    *d;            /*  Array of digitized points    */
  287.     int        first, last;        /*  Indices defining region    */
  288.     double    *u;            /*  Current parameter values    */
  289.     BezierCurve    bezCurve;    /*  Current fitted curve    */
  290. {
  291.     int     nPts = last-first+1;    
  292.     int     i;
  293.     double    *uPrime;        /*  New parameter values    */
  294.  
  295.     uPrime = (double *)malloc(nPts * sizeof(double));
  296.     for (i = first; i <= last; i++) {
  297.         uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-
  298.                     first]);
  299.     }
  300.     return (uPrime);
  301. }
  302.  
  303.  
  304.  
  305. /*
  306.  *  NewtonRaphsonRootFind :
  307.  *    Use Newton-Raphson iteration to find better root.
  308.  */
  309. static double NewtonRaphsonRootFind(Q, P, u)
  310.     BezierCurve    Q;            /*  Current fitted curve    */
  311.     Point2         P;        /*  Digitized point        */
  312.     double         u;        /*  Parameter value for "P"    */
  313. {
  314.     double         numerator, denominator;
  315.     Point2         Q1[3], Q2[2];    /*  Q' and Q''            */
  316.     Point2        Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q''    */
  317.     double         uPrime;        /*  Improved u            */
  318.     int         i;
  319.     
  320.     /* Compute Q(u)    */
  321.     Q_u = BezierII(3, Q, u);
  322.     
  323.     /* Generate control vertices for Q'    */
  324.     for (i = 0; i <= 2; i++) {
  325.         Q1[i].x = (Q[i+1].x - Q[i].x) * 3.0;
  326.         Q1[i].y = (Q[i+1].y - Q[i].y) * 3.0;
  327.     }
  328.     
  329.     /* Generate control vertices for Q'' */
  330.     for (i = 0; i <= 1; i++) {
  331.         Q2[i].x = (Q1[i+1].x - Q1[i].x) * 2.0;
  332.         Q2[i].y = (Q1[i+1].y - Q1[i].y) * 2.0;
  333.     }
  334.     
  335.     /* Compute Q'(u) and Q''(u)    */
  336.     Q1_u = BezierII(2, Q1, u);
  337.     Q2_u = BezierII(1, Q2, u);
  338.     
  339.     /* Compute f(u)/f'(u) */
  340.     numerator = (Q_u.x - P.x) * (Q1_u.x) + (Q_u.y - P.y) * (Q1_u.y);
  341.     denominator = (Q1_u.x) * (Q1_u.x) + (Q1_u.y) * (Q1_u.y) +
  342.                     (Q_u.x - P.x) * (Q2_u.x) + (Q_u.y - P.y) * (Q2_u.y);
  343.     
  344.     /* u = u - f(u)/f'(u) */
  345.     uPrime = u - (numerator/denominator);
  346.     return (uPrime);
  347. }
  348.  
  349.     
  350.                
  351. /*
  352.  *  Bezier :
  353.  *      Evaluate a Bezier curve at a particular parameter value
  354.  * 
  355.  */
  356. static Point2 BezierII(degree, V, t)
  357.     int        degree;        /* The degree of the bezier curve    */
  358.     Point2     *V;        /* Array of control points        */
  359.     double     t;        /* Parametric value to find point for    */
  360. {
  361.     int     i, j;        
  362.     Point2     Q;            /* Point on curve at parameter t    */
  363.     Point2     *Vtemp;        /* Local copy of control points        */
  364.  
  365.     /* Copy array    */
  366.     Vtemp = (Point2 *)malloc((unsigned)((degree+1) 
  367.                 * sizeof (Point2)));
  368.     for (i = 0; i <= degree; i++) {
  369.         Vtemp[i] = V[i];
  370.     }
  371.  
  372.     /* Triangle computation    */
  373.     for (i = 1; i <= degree; i++) {    
  374.         for (j = 0; j <= degree-i; j++) {
  375.             Vtemp[j].x = (1.0 - t) * Vtemp[j].x + t * Vtemp[j+1].x;
  376.             Vtemp[j].y = (1.0 - t) * Vtemp[j].y + t * Vtemp[j+1].y;
  377.         }
  378.     }
  379.  
  380.     Q = Vtemp[0];
  381.     free((void *)Vtemp);
  382.     return Q;
  383. }
  384.  
  385.  
  386. /*
  387.  *  B0, B1, B2, B3 :
  388.  *    Bezier multipliers
  389.  */
  390. static double B0(u)
  391.     double    u;
  392. {
  393.     double tmp = 1.0 - u;
  394.     return (tmp * tmp * tmp);
  395. }
  396.  
  397.  
  398. static double B1(u)
  399.     double    u;
  400. {
  401.     double tmp = 1.0 - u;
  402.     return (3 * u * (tmp * tmp));
  403. }
  404.  
  405. static double B2(u)
  406.     double    u;
  407. {
  408.     double tmp = 1.0 - u;
  409.     return (3 * u * u * tmp);
  410. }
  411.  
  412. static double B3(u)
  413.     double    u;
  414. {
  415.     return (u * u * u);
  416. }
  417.  
  418.  
  419.  
  420. /*
  421.  * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
  422.  *Approximate unit tangents at endpoints and "center" of digitized curve
  423.  */
  424. static Vector2 ComputeLeftTangent(d, end)
  425.     Point2    *d;            /*  Digitized points*/
  426.     int        end;        /*  Index to "left" end of region */
  427. {
  428.     Vector2    tHat1;
  429.     tHat1 = V2SubII(d[end+1], d[end]);
  430.     tHat1 = *V2Normalize(&tHat1);
  431.     return tHat1;
  432. }
  433.  
  434. static Vector2 ComputeRightTangent(d, end)
  435.     Point2    *d;            /*  Digitized points        */
  436.     int        end;        /*  Index to "right" end of region */
  437. {
  438.     Vector2    tHat2;
  439.     tHat2 = V2SubII(d[end-1], d[end]);
  440.     tHat2 = *V2Normalize(&tHat2);
  441.     return tHat2;
  442. }
  443.  
  444.  
  445. static Vector2 ComputeCenterTangent(d, center)
  446.     Point2    *d;            /*  Digitized points            */
  447.     int        center;        /*  Index to point inside region    */
  448. {
  449.     Vector2    V1, V2, tHatCenter;
  450.  
  451.     V1 = V2SubII(d[center-1], d[center]);
  452.     V2 = V2SubII(d[center], d[center+1]);
  453.     tHatCenter.x = (V1.x + V2.x)/2.0;
  454.     tHatCenter.y = (V1.y + V2.y)/2.0;
  455.     tHatCenter = *V2Normalize(&tHatCenter);
  456.     return tHatCenter;
  457. }
  458.  
  459.  
  460. /*
  461.  *  ChordLengthParameterize :
  462.  *    Assign parameter values to digitized points 
  463.  *    using relative distances between points.
  464.  */
  465. static double *ChordLengthParameterize(d, first, last)
  466.     Point2    *d;            /* Array of digitized points */
  467.     int        first, last;        /*  Indices defining region    */
  468. {
  469.     int        i;    
  470.     double    *u;            /*  Parameterization        */
  471.  
  472.     u = (double *)malloc((unsigned)(last-first+1) * sizeof(double));
  473.  
  474.     u[0] = 0.0;
  475.     for (i = first+1; i <= last; i++) {
  476.         u[i-first] = u[i-first-1] +
  477.                   V2DistanceBetween2Points(&d[i], &d[i-1]);
  478.     }
  479.  
  480.     for (i = first + 1; i <= last; i++) {
  481.         u[i-first] = u[i-first] / u[last-first];
  482.     }
  483.  
  484.     return(u);
  485. }
  486.  
  487.  
  488.  
  489.  
  490. /*
  491.  *  ComputeMaxError :
  492.  *    Find the maximum squared distance of digitized points
  493.  *    to fitted curve.
  494. */
  495. static double ComputeMaxError(d, first, last, bezCurve, u, splitPoint)
  496.     Point2    *d;            /*  Array of digitized points    */
  497.     int        first, last;        /*  Indices defining region    */
  498.     BezierCurve    bezCurve;        /*  Fitted Bezier curve        */
  499.     double    *u;            /*  Parameterization of points    */
  500.     int        *splitPoint;        /*  Point of maximum error    */
  501. {
  502.     int        i;
  503.     double    maxDist;        /*  Maximum error        */
  504.     double    dist;        /*  Current error        */
  505.     Point2    P;            /*  Point on curve        */
  506.     Vector2    v;            /*  Vector from point to curve    */
  507.  
  508.     *splitPoint = (last - first + 1)/2;
  509.     maxDist = 0.0;
  510.     for (i = first + 1; i < last; i++) {
  511.         P = BezierII(3, bezCurve, u[i-first]);
  512.         v = V2SubII(P, d[i]);
  513.         dist = V2SquaredLength(&v);
  514.         if (dist >= maxDist) {
  515.             maxDist = dist;
  516.             *splitPoint = i;
  517.         }
  518.     }
  519.     return (maxDist);
  520. }
  521. static Vector2 V2AddII(a, b)
  522.     Vector2 a, b;
  523. {
  524.     Vector2    c;
  525.     c.x = a.x + b.x;  c.y = a.y + b.y;
  526.     return (c);
  527. }
  528. static Vector2 V2ScaleIII(v, s)
  529.     Vector2    v;
  530.     double    s;
  531. {
  532.     Vector2 result;
  533.     result.x = v.x * s; result.y = v.y * s;
  534.     return (result);
  535. }
  536.  
  537. static Vector2 V2SubII(a, b)
  538.     Vector2    a, b;
  539. {
  540.     Vector2    c;
  541.     c.x = a.x - b.x; c.y = a.y - b.y;
  542.     return (c);
  543. }
  544.