home *** CD-ROM | disk | FTP | other *** search
/ Club Amiga de Montreal - CAM / CAM_CD_1.iso / files / 505a.lha / GrapicsGems / FitCurves.c < prev    next >
C/C++ Source or Header  |  1991-05-01  |  14KB  |  538 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        Bezier();
  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        V2ScaleII();
  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.         return;
  140.     }
  141.  
  142.  
  143.     /*  If error not too large, try some reparameterization  */
  144.     /*  and iteration */
  145.     if (maxError < iterationError) {
  146.         for (i = 0; i < maxIterations; i++) {
  147.             uPrime = Reparameterize(d, first, last, u, bezCurve);
  148.             bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
  149.             maxError = ComputeMaxError(d, first, last,
  150.                        bezCurve, uPrime, &splitPoint);
  151.             if (maxError < error) {
  152.             DrawBezierCurve(3, bezCurve);
  153.             return;
  154.         }
  155.         free((char *)u);
  156.         u = uPrime;
  157.     }
  158. }
  159.  
  160.     /* Fitting failed -- split at max error point and fit recursively */
  161.     tHatCenter = ComputeCenterTangent(d, splitPoint);
  162.     FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
  163.     V2Negate(&tHatCenter);
  164.     FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
  165. }
  166.  
  167.  
  168. /*
  169.  *  GenerateBezier :
  170.  *  Use least-squares method to find Bezier control points for region.
  171.  *
  172.  */
  173. static BezierCurve  GenerateBezier(d, first, last, uPrime, tHat1, tHat2)
  174.     Point2    *d;            /*  Array of digitized points    */
  175.     int        first, last;        /*  Indices defining region    */
  176.     double    *uPrime;        /*  Parameter values for region */
  177.     Vector2    tHat1, tHat2;    /*  Unit tangents at endpoints    */
  178. {
  179.     int     i;
  180.     Vector2     A[MAXPOINTS][2];    /* Precomputed rhs for eqn    */
  181.     int     nPts;            /* Number of pts in sub-curve */
  182.     double     C[2][2];            /* Matrix C        */
  183.     double     X[2];            /* Matrix X            */
  184.     double     det_C0_C1,        /* Determinants of matrices    */
  185.                det_C0_X,
  186.                det_X_C1;
  187.     double     alpha_l,        /* Alpha values, left and right    */
  188.                alpha_r;
  189.     Vector2     tmp;            /* Utility variable        */
  190.     BezierCurve    bezCurve;    /* RETURN bezier curve ctl pts    */
  191.  
  192.     bezCurve = (Point2 *)malloc(4 * sizeof(Point2));
  193.     nPts = last - first + 1;
  194.  
  195.  
  196.     /* Compute the A's    */
  197.     for (i = 0; i < nPts; i++) {
  198.         Vector2        v1, v2;
  199.         v1 = tHat1;
  200.         v2 = tHat2;
  201.         V2Scale(&v1, B1(uPrime[i]));
  202.         V2Scale(&v2, B2(uPrime[i]));
  203.         A[i][0] = v1;
  204.         A[i][1] = v2;
  205.     }
  206.  
  207.     /* Create the C and X matrices    */
  208.     C[0][0] = 0.0;
  209.     C[0][1] = 0.0;
  210.     C[1][0] = 0.0;
  211.     C[1][1] = 0.0;
  212.     X[0]    = 0.0;
  213.     X[1]    = 0.0;
  214.  
  215.     for (i = 0; i < nPts; i++) {
  216.         C[0][0] += V2Dot(&A[i][0], &A[i][0]);
  217.         C[0][1] += V2Dot(&A[i][0], &A[i][1]);
  218. /*                    C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/    
  219.         C[1][0] = C[0][1];
  220.         C[1][1] += V2Dot(&A[i][1], &A[i][1]);
  221.  
  222.         tmp = V2SubII(d[first + i],
  223.             V2AddII(
  224.               V2ScaleII(d[first], B0(uPrime[i])),
  225.                 V2AddII(
  226.                       V2ScaleII(d[first], B1(uPrime[i])),
  227.                             V2AddII(
  228.                               V2ScaleII(d[last], B2(uPrime[i])),
  229.                                 V2ScaleII(d[last], B3(uPrime[i]))))));
  230.     
  231.  
  232.     X[0] += V2Dot(&A[i][0], &tmp);
  233.     X[1] += V2Dot(&A[i][1], &tmp);
  234.     }
  235.  
  236.     /* Compute the determinants of C and X    */
  237.     det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
  238.     det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
  239.     det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];
  240.  
  241.     /* Finally, derive alpha values    */
  242.     if (det_C0_C1 == 0.0) {
  243.         det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
  244.     }
  245.     alpha_l = det_X_C1 / det_C0_C1;
  246.     alpha_r = det_C0_X / det_C0_C1;
  247.  
  248.  
  249.     /*  If alpha negative, use the Wu/Barsky heuristic (see text) */
  250.     if (alpha_l < 0.0 || alpha_r < 0.0) {
  251.         double    dist = V2DistanceBetween2Points(&d[last], &d[first]) /
  252.                     3.0;
  253.  
  254.         bezCurve[0] = d[first];
  255.         bezCurve[3] = d[last];
  256.         V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
  257.         V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
  258.         return (bezCurve);
  259.     }
  260.  
  261.     /*  First and last control points of the Bezier curve are */
  262.     /*  positioned exactly at the first and last data points */
  263.     /*  Control points 1 and 2 are positioned an alpha distance out */
  264.     /*  on the tangent vectors, left and right, respectively */
  265.     bezCurve[0] = d[first];
  266.     bezCurve[3] = d[last];
  267.     V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
  268.     V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
  269.     return (bezCurve);
  270. }
  271.  
  272.  
  273. /*
  274.  *  Reparameterize:
  275.  *    Given set of points and their parameterization, try to find
  276.  *   a better parameterization.
  277.  *
  278.  */
  279. static double *Reparameterize(d, first, last, u, bezCurve)
  280.     Point2    *d;            /*  Array of digitized points    */
  281.     int        first, last;        /*  Indices defining region    */
  282.     double    *u;            /*  Current parameter values    */
  283.     BezierCurve    bezCurve;    /*  Current fitted curve    */
  284. {
  285.     int     nPts = last-first+1;    
  286.     int     i;
  287.     double    *uPrime;        /*  New parameter values    */
  288.  
  289.     uPrime = (double *)malloc(nPts * sizeof(double));
  290.     for (i = first; i <= last; i++) {
  291.         uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-
  292.                     first]);
  293.     }
  294.     return (uPrime);
  295. }
  296.  
  297.  
  298.  
  299. /*
  300.  *  NewtonRaphsonRootFind :
  301.  *    Use Newton-Raphson iteration to find better root.
  302.  */
  303. static double NewtonRaphsonRootFind(Q, P, u)
  304.     BezierCurve    Q;            /*  Current fitted curve    */
  305.     Point2         P;        /*  Digitized point        */
  306.     double         u;        /*  Parameter value for "P"    */
  307. {
  308.     double         numerator, denominator;
  309.     Point2         Q1[3], Q2[2];    /*  Q' and Q''            */
  310.     Point2        Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q''    */
  311.     double         uPrime;        /*  Improved u            */
  312.     int         i;
  313.     
  314.     /* Compute Q(u)    */
  315.     Q_u = Bezier(3, Q, u);
  316.     
  317.     /* Generate control vertices for Q'    */
  318.     for (i = 0; i <= 2; i++) {
  319.         Q1[i].x = (Q[i+1].x - Q[i].x) * 3.0;
  320.         Q1[i].y = (Q[i+1].y - Q[i].y) * 3.0;
  321.     }
  322.     
  323.     /* Generate control vertices for Q'' */
  324.     for (i = 0; i <= 1; i++) {
  325.         Q2[i].x = (Q1[i+1].x - Q1[i].x) * 2.0;
  326.         Q2[i].y = (Q1[i+1].y - Q1[i].y) * 2.0;
  327.     }
  328.     
  329.     /* Compute Q'(u) and Q''(u)    */
  330.     Q1_u = Bezier(2, Q1, u);
  331.     Q2_u = Bezier(1, Q2, u);
  332.     
  333.     /* Compute f(u)/f'(u) */
  334.     numerator = (Q_u.x - P.x) * (Q1_u.x) + (Q_u.y - P.y) * (Q1_u.y);
  335.     denominator = (Q1_u.x) * (Q1_u.x) + (Q1_u.y) * (Q1_u.y) +
  336.                     (Q_u.x - P.x) * (Q2_u.x) + (Q_u.y - P.y) * (Q2_u.y);
  337.     
  338.     /* u = u - f(u)/f'(u) */
  339.     uPrime = u - (numerator/denominator);
  340.     return (uPrime);
  341. }
  342.  
  343.     
  344.                
  345. /*
  346.  *  Bezier :
  347.  *      Evaluate a Bezier curve at a particular parameter value
  348.  * 
  349.  */
  350. static Point2 Bezier(degree, V, t)
  351.     int        degree;        /* The degree of the bezier curve    */
  352.     Point2     *V;        /* Array of control points        */
  353.     double     t;        /* Parametric value to find point for    */
  354. {
  355.     int     i, j;        
  356.     Point2     Q;            /* Point on curve at parameter t    */
  357.     Point2     *Vtemp;        /* Local copy of control points        */
  358.  
  359.     /* Copy array    */
  360.     Vtemp = (Point2 *)malloc((unsigned)((degree+1) 
  361.                 * sizeof (Point2)));
  362.     for (i = 0; i <= degree; i++) {
  363.         Vtemp[i] = V[i];
  364.     }
  365.  
  366.     /* Triangle computation    */
  367.     for (i = 1; i <= degree; i++) {    
  368.         for (j = 0; j <= degree-i; j++) {
  369.             Vtemp[j].x = (1.0 - t) * Vtemp[j].x + t * Vtemp[j+1].x;
  370.             Vtemp[j].y = (1.0 - t) * Vtemp[j].y + t * Vtemp[j+1].y;
  371.         }
  372.     }
  373.  
  374.     Q = Vtemp[0];
  375.     free((char *)Vtemp);
  376.     return Q;
  377. }
  378.  
  379.  
  380. /*
  381.  *  B0, B1, B2, B3 :
  382.  *    Bezier multipliers
  383.  */
  384. static double B0(u)
  385.     double    u;
  386. {
  387.     double tmp = 1.0 - u;
  388.     return (tmp * tmp * tmp);
  389. }
  390.  
  391.  
  392. static double B1(u)
  393.     double    u;
  394. {
  395.     double tmp = 1.0 - u;
  396.     return (3 * u * (tmp * tmp));
  397. }
  398.  
  399. static double B2(u)
  400.     double    u;
  401. {
  402.     double tmp = 1.0 - u;
  403.     return (3 * u * u * tmp);
  404. }
  405.  
  406. static double B3(u)
  407.     double    u;
  408. {
  409.     return (u * u * u);
  410. }
  411.  
  412.  
  413.  
  414. /*
  415.  * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
  416.  *Approximate unit tangents at endpoints and "center" of digitized curve
  417.  */
  418. static Vector2 ComputeLeftTangent(d, end)
  419.     Point2    *d;            /*  Digitized points*/
  420.     int        end;        /*  Index to "left" end of region */
  421. {
  422.     Vector2    tHat1;
  423.     tHat1 = V2SubII(d[end+1], d[end]);
  424.     tHat1 = *V2Normalize(&tHat1);
  425.     return tHat1;
  426. }
  427.  
  428. static Vector2 ComputeRightTangent(d, end)
  429.     Point2    *d;            /*  Digitized points        */
  430.     int        end;        /*  Index to "right" end of region */
  431. {
  432.     Vector2    tHat2;
  433.     tHat2 = V2SubII(d[end-1], d[end]);
  434.     tHat2 = *V2Normalize(&tHat2);
  435.     return tHat2;
  436. }
  437.  
  438.  
  439. static Vector2 ComputeCenterTangent(d, center)
  440.     Point2    *d;            /*  Digitized points            */
  441.     int        center;        /*  Index to point inside region    */
  442. {
  443.     Vector2    V1, V2, tHatCenter;
  444.  
  445.     V1 = V2SubII(d[center-1], d[center]);
  446.     V2 = V2SubII(d[center], d[center+1]);
  447.     tHatCenter.x = (V1.x + V2.x)/2.0;
  448.     tHatCenter.y = (V1.y + V2.y)/2.0;
  449.     tHatCenter = *V2Normalize(&tHatCenter);
  450.     return tHatCenter;
  451. }
  452.  
  453.  
  454. /*
  455.  *  ChordLengthParameterize :
  456.  *    Assign parameter values to digitized points 
  457.  *    using relative distances between points.
  458.  */
  459. static double *ChordLengthParameterize(d, first, last)
  460.     Point2    *d;            /* Array of digitized points */
  461.     int        first, last;        /*  Indices defining region    */
  462. {
  463.     int        i;    
  464.     double    *u;            /*  Parameterization        */
  465.  
  466.     u = (double *)malloc((unsigned)(last-first+1) * sizeof(double));
  467.  
  468.     u[0] = 0.0;
  469.     for (i = first+1; i <= last; i++) {
  470.         u[i-first] = u[i-first-1] +
  471.                   V2DistanceBetween2Points(&d[i], &d[i-1]);
  472.     }
  473.  
  474.     for (i = first + 1; i <= last; i++) {
  475.         u[i-first] = u[i-first] / u[last-first];
  476.     }
  477.  
  478.     return(u);
  479. }
  480.  
  481.  
  482.  
  483.  
  484. /*
  485.  *  ComputeMaxError :
  486.  *    Find the maximum squared distance of digitized points
  487.  *    to fitted curve.
  488. */
  489. static double ComputeMaxError(d, first, last, bezCurve, u, splitPoint)
  490.     Point2    *d;            /*  Array of digitized points    */
  491.     int        first, last;        /*  Indices defining region    */
  492.     BezierCurve    bezCurve;        /*  Fitted Bezier curve        */
  493.     double    *u;            /*  Parameterization of points    */
  494.     int        *splitPoint;        /*  Point of maximum error    */
  495. {
  496.     int        i;
  497.     double    maxDist;        /*  Maximum error        */
  498.     double    dist;        /*  Current error        */
  499.     Point2    P;            /*  Point on curve        */
  500.     Vector2    v;            /*  Vector from point to curve    */
  501.  
  502.     *splitPoint = (last - first + 1)/2;
  503.     maxDist = 0.0;
  504.     for (i = first + 1; i < last; i++) {
  505.         P = Bezier(3, bezCurve, u[i-first]);
  506.         v = V2SubII(P, d[i]);
  507.         dist = V2SquaredLength(&v);
  508.         if (dist >= maxDist) {
  509.             maxDist = dist;
  510.             *splitPoint = i;
  511.         }
  512.     }
  513.     return (maxDist);
  514. }
  515. static Vector2 V2AddII(a, b)
  516.     Vector2 a, b;
  517. {
  518.     Vector2    c;
  519.     c.x = a.x + b.x;  c.y = a.y + b.y;
  520.     return (c);
  521. }
  522. static Vector2 V2ScaleII(v, s)
  523.     Vector2    v;
  524.     double    s;
  525. {
  526.     Vector2 result;
  527.     result.x = v.x * s; result.y = v.y * s;
  528.     return (result);
  529. }
  530.  
  531. static Vector2 V2SubII(a, b)
  532.     Vector2    a, b;
  533. {
  534.     Vector2    c;
  535.     c.x = a.x - b.x; c.y = a.y - b.y;
  536.     return (c);
  537. }
  538.