src/display/bezier-utils.cpp

   1 #define __SP_BEZIER_UTILS_C__
   2
   3 /** \file
   4  * Bezier interpolation for inkscape drawing code.
   5  */
   6 /*
   7  * Original code published in:
   8  *   An Algorithm for Automatically Fitting Digitized Curves
   9  *   by Philip J. Schneider
  10  *  "Graphics Gems", Academic Press, 1990
  11  *
  12  * Authors:
  13  *   Philip J. Schneider
  14  *   Lauris Kaplinski <lauris@kaplinski.com>
  15  *   Peter Moulder <pmoulder@mail.csse.monash.edu.au>
  16  *
  17  * Copyright (C) 1990 Philip J. Schneider
  18  * Copyright (C) 2001 Lauris Kaplinski
  19  * Copyright (C) 2001 Ximian, Inc.
  20  * Copyright (C) 2003,2004 Monash University
  21  *
  22  * Released under GNU GPL, read the file 'COPYING' for more information
  23  */
  24
  25 #define SP_HUGE 1e5
  26 #define noBEZIER_DEBUG
  27
  28 #ifdef HAVE_CONFIG_H
  29 # include <config.h>
  30 #endif
  31
  32 #ifdef HAVE_IEEEFP_H
  33 # include <ieeefp.h>
  34 #endif
  35
  36 #include <glib.h> // g_assert()
  37 #include <glib/gmessages.h>
  38 #include <glib/gmem.h>
  39 #include "bezier-utils.h"
  40 #include <libnr/nr-point-fns.h>
  41
  42 #include "2geom/isnan.h"
  43
  44
  45 typedef NR::Point BezierCurve[];
  46
  47 /* Forward declarations */
  48 static void generate_bezier(NR::Point b[], NR::Point const d[], gdouble const u[], unsigned len,
  49                             NR::Point const &tHat1, NR::Point const &tHat2, double tolerance_sq);
  50 static void estimate_lengths(NR::Point bezier[],
  51                              NR::Point const data[], gdouble const u[], unsigned len,
  52                              NR::Point const &tHat1, NR::Point const &tHat2);
  53 static void estimate_bi(NR::Point b[4], unsigned ei,
  54                         NR::Point const data[], double const u[], unsigned len);
  55 static void reparameterize(NR::Point const d[], unsigned len, double u[], BezierCurve const bezCurve);
  56 static gdouble NewtonRaphsonRootFind(BezierCurve const Q, NR::Point const &P, gdouble u);
  57 static NR::Point sp_darray_center_tangent(NR::Point const d[], unsigned center, unsigned length);
  58 static NR::Point sp_darray_right_tangent(NR::Point const d[], unsigned const len);
  59 static unsigned copy_without_nans_or_adjacent_duplicates(NR::Point const src[], unsigned src_len, NR::Point dest[]);
  60 static void chord_length_parameterize(NR::Point const d[], gdouble u[], unsigned len);
  61 static double compute_max_error_ratio(NR::Point const d[], double const u[], unsigned len,
  62                                       BezierCurve const bezCurve, double tolerance,
  63                                       unsigned *splitPoint);
  64 static double compute_hook(NR::Point const &a, NR::Point const &b, double const u, BezierCurve const bezCurve,
  65                            double const tolerance);
  66
  67
  68 static NR::Point const unconstrained_tangent(0, 0);
  69
  70
  71 /*
  72  *  B0, B1, B2, B3 : Bezier multipliers
  73  */
  74
  75 #define B0(u) ( ( 1.0 - u )  *  ( 1.0 - u )  *  ( 1.0 - u ) )
  76 #define B1(u) ( 3 * u  *  ( 1.0 - u )  *  ( 1.0 - u ) )
  77 #define B2(u) ( 3 * u * u  *  ( 1.0 - u ) )
  78 #define B3(u) ( u * u * u )
  79
  80 #ifdef BEZIER_DEBUG
  81 # define DOUBLE_ASSERT(x) g_assert( ( (x) > -SP_HUGE ) && ( (x) < SP_HUGE ) )
  82 # define BEZIER_ASSERT(b) do { \
  83            DOUBLE_ASSERT((b)[0][NR::X]); DOUBLE_ASSERT((b)[0][NR::Y]);  \
  84            DOUBLE_ASSERT((b)[1][NR::X]); DOUBLE_ASSERT((b)[1][NR::Y]);  \
  85            DOUBLE_ASSERT((b)[2][NR::X]); DOUBLE_ASSERT((b)[2][NR::Y]);  \
  86            DOUBLE_ASSERT((b)[3][NR::X]); DOUBLE_ASSERT((b)[3][NR::Y]);  \
  87          } while(0)
  88 #else
  89 # define DOUBLE_ASSERT(x) do { } while(0)
  90 # define BEZIER_ASSERT(b) do { } while(0)
  91 #endif
  92
  93
  94 /**
  95  * Fit a single-segment Bezier curve to a set of digitized points.
  96  *
  97  * \return Number of segments generated, or -1 on error.
  98  */
  99 gint
 100 sp_bezier_fit_cubic(NR::Point *bezier, NR::Point const *data, gint len, gdouble error)
 101 {
 102     return sp_bezier_fit_cubic_r(bezier, data, len, error, 1);
 103 }
 104
 105 /**
 106  * Fit a multi-segment Bezier curve to a set of digitized points, with
 107  * possible weedout of identical points and NaNs.
 108  *
 109  * \param max_beziers Maximum number of generated segments
 110  * \param Result array, must be large enough for n. segments * 4 elements.
 111  *
 112  * \return Number of segments generated, or -1 on error.
 113  */
 114 gint
 115 sp_bezier_fit_cubic_r(NR::Point bezier[], NR::Point const data[], gint const len, gdouble const error, unsigned const max_beziers)
 116 {
 117     g_return_val_if_fail(bezier != NULL, -1);
 118     g_return_val_if_fail(data != NULL, -1);
 119     g_return_val_if_fail(len > 0, -1);
 120     g_return_val_if_fail(max_beziers < (1ul << (31 - 2 - 1 - 3)), -1);
 121
 122     NR::Point *uniqued_data = g_new(NR::Point, len);
 123     unsigned uniqued_len = copy_without_nans_or_adjacent_duplicates(data, len, uniqued_data);
 124
 125     if ( uniqued_len < 2 ) {
 126         g_free(uniqued_data);
 127         return 0;
 128     }
 129
 130     /* Call fit-cubic function with recursion. */
 131     gint const ret = sp_bezier_fit_cubic_full(bezier, NULL, uniqued_data, uniqued_len,
 132                                               unconstrained_tangent, unconstrained_tangent,
 133                                               error, max_beziers);
 134     g_free(uniqued_data);
 135     return ret;
 136 }
 137
 138 /**
 139  * Copy points from src to dest, filter out points containing NaN and
 140  * adjacent points with equal x and y.
 141  * \return length of dest
 142  */
 143 static unsigned
 144 copy_without_nans_or_adjacent_duplicates(NR::Point const src[], unsigned src_len, NR::Point dest[])
 145 {
 146     unsigned si = 0;
 147     for (;;) {
 148         if ( si == src_len ) {
 149             return 0;
 150         }
 151         if (!IS_NAN(src[si][NR::X]) &&
 152             !IS_NAN(src[si][NR::Y])) {
 153             dest[0] = NR::Point(src[si]);
 154             ++si;
 155             break;
 156         }
 157         si ++;
 158     }
 159     unsigned di = 0;
 160     for (; si < src_len; ++si) {
 161         NR::Point const src_pt = NR::Point(src[si]);
 162         if ( src_pt != dest[di]
 163              && !IS_NAN(src_pt[NR::X])
 164              && !IS_NAN(src_pt[NR::Y])) {
 165             dest[++di] = src_pt;
 166         }
 167     }
 168     unsigned dest_len = di + 1;
 169     g_assert( dest_len <= src_len );
 170     return dest_len;
 171 }
 172
 173 /**
 174  * Fit a multi-segment Bezier curve to a set of digitized points, without
 175  * possible weedout of identical points and NaNs.
 176  *
 177  * \pre data is uniqued, i.e. not exist i: data[i] == data[i + 1].
 178  * \param max_beziers Maximum number of generated segments
 179  * \param Result array, must be large enough for n. segments * 4 elements.
 180  */
 181 gint
 182 sp_bezier_fit_cubic_full(NR::Point bezier[], int split_points[],
 183                          NR::Point const data[], gint const len,
 184                          NR::Point const &tHat1, NR::Point const &tHat2,
 185                          double const error, unsigned const max_beziers)
 186 {
 187     int const maxIterations = 4;   /* Max times to try iterating */
 188
 189     g_return_val_if_fail(bezier != NULL, -1);
 190     g_return_val_if_fail(data != NULL, -1);
 191     g_return_val_if_fail(len > 0, -1);
 192     g_return_val_if_fail(max_beziers >= 1, -1);
 193     g_return_val_if_fail(error >= 0.0, -1);
 194
 195     if ( len < 2 ) return 0;
 196
 197     if ( len == 2 ) {
 198         /* We have 2 points, which can be fitted trivially. */
 199         bezier[0] = data[0];
 200         bezier[3] = data[len - 1];
 201         double const dist = ( L2( data[len - 1]
 202                                   - data[0] )
 203                               / 3.0 );
 204         if (IS_NAN(dist)) {
 205             /* Numerical problem, fall back to straight line segment. */
 206             bezier[1] = bezier[0];
 207             bezier[2] = bezier[3];
 208         } else {
 209             bezier[1] = ( is_zero(tHat1)
 210                           ? ( 2 * bezier[0] + bezier[3] ) / 3.
 211                           : bezier[0] + dist * tHat1 );
 212             bezier[2] = ( is_zero(tHat2)
 213                           ? ( bezier[0] + 2 * bezier[3] ) / 3.
 214                           : bezier[3] + dist * tHat2 );
 215         }
 216         BEZIER_ASSERT(bezier);
 217         return 1;
 218     }
 219
 220     /*  Parameterize points, and attempt to fit curve */
 221     unsigned splitPoint;   /* Point to split point set at. */
 222     bool is_corner;
 223     {
 224         double *u = g_new(double, len);
 225         chord_length_parameterize(data, u, len);
 226         if ( u[len - 1] == 0.0 ) {
 227             /* Zero-length path: every point in data[] is the same.
 228              *
 229              * (Clients aren't allowed to pass such data; handling the case is defensive
 230              * programming.)
 231              */
 232             g_free(u);
 233             return 0;
 234         }
 235
 236         generate_bezier(bezier, data, u, len, tHat1, tHat2, error);
 237         reparameterize(data, len, u, bezier);
 238
 239         /* Find max deviation of points to fitted curve. */
 240         double const tolerance = sqrt(error + 1e-9);
 241         double maxErrorRatio = compute_max_error_ratio(data, u, len, bezier, tolerance, &splitPoint);
 242
 243         if ( fabs(maxErrorRatio) <= 1.0 ) {
 244             BEZIER_ASSERT(bezier);
 245             g_free(u);
 246             return 1;
 247         }
 248
 249         /* If error not too large, then try some reparameterization and iteration. */
 250         if ( 0.0 <= maxErrorRatio && maxErrorRatio <= 3.0 ) {
 251             for (int i = 0; i < maxIterations; i++) {
 252                 generate_bezier(bezier, data, u, len, tHat1, tHat2, error);
 253                 reparameterize(data, len, u, bezier);
 254                 maxErrorRatio = compute_max_error_ratio(data, u, len, bezier, tolerance, &splitPoint);
 255                 if ( fabs(maxErrorRatio) <= 1.0 ) {
 256                     BEZIER_ASSERT(bezier);
 257                     g_free(u);
 258                     return 1;
 259                 }
 260             }
 261         }
 262         g_free(u);
 263         is_corner = (maxErrorRatio < 0);
 264     }
 265
 266     if (is_corner) {
 267         g_assert(splitPoint < unsigned(len));
 268         if (splitPoint == 0) {
 269             if (is_zero(tHat1)) {
 270                 /* Got spike even with unconstrained initial tangent. */
 271                 ++splitPoint;
 272             } else {
 273                 return sp_bezier_fit_cubic_full(bezier, split_points, data, len, unconstrained_tangent, tHat2,
 274                                                 error, max_beziers);
 275             }
 276         } else if (splitPoint == unsigned(len - 1)) {
 277             if (is_zero(tHat2)) {
 278                 /* Got spike even with unconstrained final tangent. */
 279                 --splitPoint;
 280             } else {
 281                 return sp_bezier_fit_cubic_full(bezier, split_points, data, len, tHat1, unconstrained_tangent,
 282                                                 error, max_beziers);
 283             }
 284         }
 285     }
 286
 287     if ( 1 < max_beziers ) {
 288         /*
 289          *  Fitting failed -- split at max error point and fit recursively
 290          */
 291         unsigned const rec_max_beziers1 = max_beziers - 1;
 292
 293         NR::Point recTHat2, recTHat1;
 294         if (is_corner) {
 295             g_return_val_if_fail(0 < splitPoint && splitPoint < unsigned(len - 1), -1);
 296             recTHat1 = recTHat2 = unconstrained_tangent;
 297         } else {
 298             /* Unit tangent vector at splitPoint. */
 299             recTHat2 = sp_darray_center_tangent(data, splitPoint, len);
 300             recTHat1 = -recTHat2;
 301         }
 302         gint const nsegs1 = sp_bezier_fit_cubic_full(bezier, split_points, data, splitPoint + 1,
 303                                                      tHat1, recTHat2, error, rec_max_beziers1);
 304         if ( nsegs1 < 0 ) {
 305 #ifdef BEZIER_DEBUG
 306             g_print("fit_cubic[1]: recursive call failed\n");
 307 #endif
 308             return -1;
 309         }
 310         g_assert( nsegs1 != 0 );
 311         if (split_points != NULL) {
 312             split_points[nsegs1 - 1] = splitPoint;
 313         }
 314         unsigned const rec_max_beziers2 = max_beziers - nsegs1;
 315         gint const nsegs2 = sp_bezier_fit_cubic_full(bezier + nsegs1*4,
 316                                                      ( split_points == NULL
 317                                                        ? NULL
 318                                                        : split_points + nsegs1 ),
 319                                                      data + splitPoint, len - splitPoint,
 320                                                      recTHat1, tHat2, error, rec_max_beziers2);
 321         if ( nsegs2 < 0 ) {
 322 #ifdef BEZIER_DEBUG
 323             g_print("fit_cubic[2]: recursive call failed\n");
 324 #endif
 325             return -1;
 326         }
 327
 328 #ifdef BEZIER_DEBUG
 329         g_print("fit_cubic: success[nsegs: %d+%d=%d] on max_beziers:%u\n",
 330                 nsegs1, nsegs2, nsegs1 + nsegs2, max_beziers);
 331 #endif
 332         return nsegs1 + nsegs2;
 333     } else {
 334         return -1;
 335     }
 336 }
 337
 338
 339 /**
 340  * Fill in \a bezier[] based on the given data and tangent requirements, using
 341  * a least-squares fit.
 342  *
 343  * Each of tHat1 and tHat2 should be either a zero vector or a unit vector.
 344  * If it is zero, then bezier[1 or 2] is estimated without constraint; otherwise,
 345  * it bezier[1 or 2] is placed in the specified direction from bezier[0 or 3].
 346  *
 347  * \param tolerance_sq Used only for an initial guess as to tangent directions
 348  *   when \a tHat1 or \a tHat2 is zero.
 349  */
 350 static void
 351 generate_bezier(NR::Point bezier[],
 352                 NR::Point const data[], gdouble const u[], unsigned const len,
 353                 NR::Point const &tHat1, NR::Point const &tHat2,
 354                 double const tolerance_sq)
 355 {
 356     bool const est1 = is_zero(tHat1);
 357     bool const est2 = is_zero(tHat2);
 358     NR::Point est_tHat1( est1
 359                          ? sp_darray_left_tangent(data, len, tolerance_sq)
 360                          : tHat1 );
 361     NR::Point est_tHat2( est2
 362                          ? sp_darray_right_tangent(data, len, tolerance_sq)
 363                          : tHat2 );
 364     estimate_lengths(bezier, data, u, len, est_tHat1, est_tHat2);
 365     /* We find that sp_darray_right_tangent tends to produce better results
 366        for our current freehand tool than full estimation. */
 367     if (est1) {
 368         estimate_bi(bezier, 1, data, u, len);
 369         if (bezier[1] != bezier[0]) {
 370             est_tHat1 = unit_vector(bezier[1] - bezier[0]);
 371         }
 372         estimate_lengths(bezier, data, u, len, est_tHat1, est_tHat2);
 373     }
 374 }
 375
 376
 377 static void
 378 estimate_lengths(NR::Point bezier[],
 379                  NR::Point const data[], gdouble const uPrime[], unsigned const len,
 380                  NR::Point const &tHat1, NR::Point const &tHat2)
 381 {
 382     double C[2][2];   /* Matrix C. */
 383     double X[2];      /* Matrix X. */
 384
 385     /* Create the C and X matrices. */
 386     C[0][0] = 0.0;
 387     C[0][1] = 0.0;
 388     C[1][0] = 0.0;
 389     C[1][1] = 0.0;
 390     X[0]    = 0.0;
 391     X[1]    = 0.0;
 392
 393     /* First and last control points of the Bezier curve are positioned exactly at the first and
 394        last data points. */
 395     bezier[0] = data[0];
 396     bezier[3] = data[len - 1];
 397
 398     for (unsigned i = 0; i < len; i++) {
 399         /* Bezier control point coefficients. */
 400         double const b0 = B0(uPrime[i]);
 401         double const b1 = B1(uPrime[i]);
 402         double const b2 = B2(uPrime[i]);
 403         double const b3 = B3(uPrime[i]);
 404
 405         /* rhs for eqn */
 406         NR::Point const a1 = b1 * tHat1;
 407         NR::Point const a2 = b2 * tHat2;
 408
 409         C[0][0] += dot(a1, a1);
 410         C[0][1] += dot(a1, a2);
 411         C[1][0] = C[0][1];
 412         C[1][1] += dot(a2, a2);
 413
 414         /* Additional offset to the data point from the predicted point if we were to set bezier[1]
 415            to bezier[0] and bezier[2] to bezier[3]. */
 416         NR::Point const shortfall
 417             = ( data[i]
 418                 - ( ( b0 + b1 ) * bezier[0] )
 419                 - ( ( b2 + b3 ) * bezier[3] ) );
 420         X[0] += dot(a1, shortfall);
 421         X[1] += dot(a2, shortfall);
 422     }
 423
 424     /* We've constructed a pair of equations in the form of a matrix product C * alpha = X.
 425        Now solve for alpha. */
 426     double alpha_l, alpha_r;
 427
 428     /* Compute the determinants of C and X. */
 429     double const det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
 430     if ( det_C0_C1 != 0 ) {
 431         /* Apparently Kramer's rule. */
 432         double const det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
 433         double const det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];
 434         alpha_l = det_X_C1 / det_C0_C1;
 435         alpha_r = det_C0_X / det_C0_C1;
 436     } else {
 437         /* The matrix is under-determined.  Try requiring alpha_l == alpha_r.
 438          *
 439          * One way of implementing the constraint alpha_l == alpha_r is to treat them as the same
 440          * variable in the equations.  We can do this by adding the columns of C to form a single
 441          * column, to be multiplied by alpha to give the column vector X.
 442          *
 443          * We try each row in turn.
 444          */
 445         double const c0 = C[0][0] + C[0][1];
 446         if (c0 != 0) {
 447             alpha_l = alpha_r = X[0] / c0;
 448         } else {
 449             double const c1 = C[1][0] + C[1][1];
 450             if (c1 != 0) {
 451                 alpha_l = alpha_r = X[1] / c1;
 452             } else {
 453                 /* Let the below code handle this. */
 454                 alpha_l = alpha_r = 0.;
 455             }
 456         }
 457     }
 458
 459     /* If alpha negative, use the Wu/Barsky heuristic (see text).  (If alpha is 0, you get
 460        coincident control points that lead to divide by zero in any subsequent
 461        NewtonRaphsonRootFind() call.) */
 462     /// \todo Check whether this special-casing is necessary now that
 463     /// NewtonRaphsonRootFind handles non-positive denominator.
 464     if ( alpha_l < 1.0e-6 ||
 465          alpha_r < 1.0e-6   )
 466     {
 467         alpha_l = alpha_r = ( L2( data[len - 1]
 468                                   - data[0] )
 469                               / 3.0 );
 470     }
 471
 472     /* Control points 1 and 2 are positioned an alpha distance out on the tangent vectors, left and
 473        right, respectively. */
 474     bezier[1] = alpha_l * tHat1 + bezier[0];
 475     bezier[2] = alpha_r * tHat2 + bezier[3];
 476
 477     return;
 478 }
 479
 480 static double lensq(NR::Point const p) {
 481     return dot(p, p);
 482 }
 483
 484 static void
 485 estimate_bi(NR::Point bezier[4], unsigned const ei,
 486             NR::Point const data[], double const u[], unsigned const len)
 487 {
 488     g_return_if_fail(1 <= ei && ei <= 2);
 489     unsigned const oi = 3 - ei;
 490     double num[2] = {0., 0.};
 491     double den = 0.;
 492     for (unsigned i = 0; i < len; ++i) {
 493         double const ui = u[i];
 494         double const b[4] = {
 495             B0(ui),
 496             B1(ui),
 497             B2(ui),
 498             B3(ui)
 499         };
 500
 501         for (unsigned d = 0; d < 2; ++d) {
 502             num[d] += b[ei] * (b[0]  * bezier[0][d] +
 503                                b[oi] * bezier[oi][d] +
 504                                b[3]  * bezier[3][d] +
 505                                - data[i][d]);
 506         }
 507         den -= b[ei] * b[ei];
 508     }
 509
 510     if (den != 0.) {
 511         for (unsigned d = 0; d < 2; ++d) {
 512             bezier[ei][d] = num[d] / den;
 513         }
 514     } else {
 515         bezier[ei] = ( oi * bezier[0] + ei * bezier[3] ) / 3.;
 516     }
 517 }
 518
 519 /**
 520  * Given set of points and their parameterization, try to find a better assignment of parameter
 521  * values for the points.
 522  *
 523  *  \param d  Array of digitized points.
 524  *  \param u  Current parameter values.
 525  *  \param bezCurve  Current fitted curve.
 526  *  \param len  Number of values in both d and u arrays.
 527  *              Also the size of the array that is allocated for return.
 528  */
 529 static void
 530 reparameterize(NR::Point const d[],
 531                unsigned const len,
 532                double u[],
 533                BezierCurve const bezCurve)
 534 {
 535     g_assert( 2 <= len );
 536
 537     unsigned const last = len - 1;
 538     g_assert( bezCurve[0] == d[0] );
 539     g_assert( bezCurve[3] == d[last] );
 540     g_assert( u[0] == 0.0 );
 541     g_assert( u[last] == 1.0 );
 542     /* Otherwise, consider including 0 and last in the below loop. */
 543
 544     for (unsigned i = 1; i < last; i++) {
 545         u[i] = NewtonRaphsonRootFind(bezCurve, d[i], u[i]);
 546     }
 547 }
 548
 549 /**
 550  *  Use Newton-Raphson iteration to find better root.
 551  *
 552  *  \param Q  Current fitted curve
 553  *  \param P  Digitized point
 554  *  \param u  Parameter value for "P"
 555  *
 556  *  \return Improved u
 557  */
 558 static gdouble
 559 NewtonRaphsonRootFind(BezierCurve const Q, NR::Point const &P, gdouble const u)
 560 {
 561     g_assert( 0.0 <= u );
 562     g_assert( u <= 1.0 );
 563
 564     /* Generate control vertices for Q'. */
 565     NR::Point Q1[3];
 566     for (unsigned i = 0; i < 3; i++) {
 567         Q1[i] = 3.0 * ( Q[i+1] - Q[i] );
 568     }
 569
 570     /* Generate control vertices for Q''. */
 571     NR::Point Q2[2];
 572     for (unsigned i = 0; i < 2; i++) {
 573         Q2[i] = 2.0 * ( Q1[i+1] - Q1[i] );
 574     }
 575
 576     /* Compute Q(u), Q'(u) and Q''(u). */
 577     NR::Point const Q_u  = bezier_pt(3, Q, u);
 578     NR::Point const Q1_u = bezier_pt(2, Q1, u);
 579     NR::Point const Q2_u = bezier_pt(1, Q2, u);
 580
 581     /* Compute f(u)/f'(u), where f is the derivative wrt u of distsq(u) = 0.5 * the square of the
 582        distance from P to Q(u).  Here we're using Newton-Raphson to find a stationary point in the
 583        distsq(u), hopefully corresponding to a local minimum in distsq (and hence a local minimum
 584        distance from P to Q(u)). */
 585     NR::Point const diff = Q_u - P;
 586     double numerator = dot(diff, Q1_u);
 587     double denominator = dot(Q1_u, Q1_u) + dot(diff, Q2_u);
 588
 589     double improved_u;
 590     if ( denominator > 0. ) {
 591         /* One iteration of Newton-Raphson:
 592            improved_u = u - f(u)/f'(u) */
 593         improved_u = u - ( numerator / denominator );
 594     } else {
 595         /* Using Newton-Raphson would move in the wrong direction (towards a local maximum rather
 596            than local minimum), so we move an arbitrary amount in the right direction. */
 597         if ( numerator > 0. ) {
 598             improved_u = u * .98 - .01;
 599         } else if ( numerator < 0. ) {
 600             /* Deliberately asymmetrical, to reduce the chance of cycling. */
 601             improved_u = .031 + u * .98;
 602         } else {
 603             improved_u = u;
 604         }
 605     }
 606
 607     if (!IS_FINITE(improved_u)) {
 608         improved_u = u;
 609     } else if ( improved_u < 0.0 ) {
 610         improved_u = 0.0;
 611     } else if ( improved_u > 1.0 ) {
 612         improved_u = 1.0;
 613     }
 614
 615     /* Ensure that improved_u isn't actually worse. */
 616     {
 617         double const diff_lensq = lensq(diff);
 618         for (double proportion = .125; ; proportion += .125) {
 619             if ( lensq( bezier_pt(3, Q, improved_u) - P ) > diff_lensq ) {
 620                 if ( proportion > 1.0 ) {
 621                     //g_warning("found proportion %g", proportion);
 622                     improved_u = u;
 623                     break;
 624                 }
 625                 improved_u = ( ( 1 - proportion ) * improved_u  +
 626                                proportion         * u            );
 627             } else {
 628                 break;
 629             }
 630         }
 631     }
 632
 633     DOUBLE_ASSERT(improved_u);
 634     return improved_u;
 635 }
 636
 637 /**
 638  * Evaluate a Bezier curve at parameter value \a t.
 639  *
 640  * \param degree The degree of the Bezier curve: 3 for cubic, 2 for quadratic etc.
 641  * \param V The control points for the Bezier curve.  Must have (\a degree+1)
 642  *    elements.
 643  * \param t The "parameter" value, specifying whereabouts along the curve to
 644  *    evaluate.  Typically in the range [0.0, 1.0].
 645  *
 646  * Let s = 1 - t.
 647  * BezierII(1, V) gives (s, t) * V, i.e. t of the way
 648  * from V[0] to V[1].
 649  * BezierII(2, V) gives (s**2, 2*s*t, t**2) * V.
 650  * BezierII(3, V) gives (s**3, 3 s**2 t, 3s t**2, t**3) * V.
 651  *
 652  * The derivative of BezierII(i, V) with respect to t
 653  * is i * BezierII(i-1, V'), where for all j, V'[j] =
 654  * V[j + 1] - V[j].
 655  */
 656 NR::Point
 657 bezier_pt(unsigned const degree, NR::Point const V[], gdouble const t)
 658 {
 659     /** Pascal's triangle. */
 660     static int const pascal[4][4] = {{1},
 661                                      {1, 1},
 662                                      {1, 2, 1},
 663                                      {1, 3, 3, 1}};
 664     g_assert( degree < G_N_ELEMENTS(pascal) );
 665     gdouble const s = 1.0 - t;
 666
 667     /* Calculate powers of t and s. */
 668     double spow[4];
 669     double tpow[4];
 670     spow[0] = 1.0; spow[1] = s;
 671     tpow[0] = 1.0; tpow[1] = t;
 672     for (unsigned i = 1; i < degree; ++i) {
 673         spow[i + 1] = spow[i] * s;
 674         tpow[i + 1] = tpow[i] * t;
 675     }
 676
 677     NR::Point ret = spow[degree] * V[0];
 678     for (unsigned i = 1; i <= degree; ++i) {
 679         ret += pascal[degree][i] * spow[degree - i] * tpow[i] * V[i];
 680     }
 681     return ret;
 682 }
 683
 684 /*
 685  * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
 686  * Approximate unit tangents at endpoints and "center" of digitized curve
 687  */
 688
 689 /**
 690  * Estimate the (forward) tangent at point d[first + 0.5].
 691  *
 692  * Unlike the center and right versions, this calculates the tangent in
 693  * the way one might expect, i.e., wrt increasing index into d.
 694  * \pre (2 \<= len) and (d[0] != d[1]).
 695  **/
 696 NR::Point
 697 sp_darray_left_tangent(NR::Point const d[], unsigned const len)
 698 {
 699     g_assert( len >= 2 );
 700     g_assert( d[0] != d[1] );
 701     return unit_vector( d[1] - d[0] );
 702 }
 703
 704 /**
 705  * Estimates the (backward) tangent at d[last - 0.5].
 706  *
 707  * \note The tangent is "backwards", i.e. it is with respect to
 708  * decreasing index rather than increasing index.
 709  *
 710  * \pre 2 \<= len.
 711  * \pre d[len - 1] != d[len - 2].
 712  * \pre all[p in d] in_svg_plane(p).
 713  */
 714 static NR::Point
 715 sp_darray_right_tangent(NR::Point const d[], unsigned const len)
 716 {
 717     g_assert( 2 <= len );
 718     unsigned const last = len - 1;
 719     unsigned const prev = last - 1;
 720     g_assert( d[last] != d[prev] );
 721     return unit_vector( d[prev] - d[last] );
 722 }
 723
 724 /**
 725  * Estimate the (forward) tangent at point d[0].
 726  *
 727  * Unlike the center and right versions, this calculates the tangent in
 728  * the way one might expect, i.e., wrt increasing index into d.
 729  *
 730  * \pre 2 \<= len.
 731  * \pre d[0] != d[1].
 732  * \pre all[p in d] in_svg_plane(p).
 733  * \post is_unit_vector(ret).
 734  **/
 735 NR::Point
 736 sp_darray_left_tangent(NR::Point const d[], unsigned const len, double const tolerance_sq)
 737 {
 738     g_assert( 2 <= len );
 739     g_assert( 0 <= tolerance_sq );
 740     for (unsigned i = 1;;) {
 741         NR::Point const pi(d[i]);
 742         NR::Point const t(pi - d[0]);
 743         double const distsq = dot(t, t);
 744         if ( tolerance_sq < distsq ) {
 745             return unit_vector(t);
 746         }
 747         ++i;
 748         if (i == len) {
 749             return ( distsq == 0
 750                      ? sp_darray_left_tangent(d, len)
 751                      : unit_vector(t) );
 752         }
 753     }
 754 }
 755
 756 /**
 757  * Estimates the (backward) tangent at d[last].
 758  *
 759  * \note The tangent is "backwards", i.e. it is with respect to
 760  * decreasing index rather than increasing index.
 761  *
 762  * \pre 2 \<= len.
 763  * \pre d[len - 1] != d[len - 2].
 764  * \pre all[p in d] in_svg_plane(p).
 765  */
 766 NR::Point
 767 sp_darray_right_tangent(NR::Point const d[], unsigned const len, double const tolerance_sq)
 768 {
 769     g_assert( 2 <= len );
 770     g_assert( 0 <= tolerance_sq );
 771     unsigned const last = len - 1;
 772     for (unsigned i = last - 1;; i--) {
 773         NR::Point const pi(d[i]);
 774         NR::Point const t(pi - d[last]);
 775         double const distsq = dot(t, t);
 776         if ( tolerance_sq < distsq ) {
 777             return unit_vector(t);
 778         }
 779         if (i == 0) {
 780             return ( distsq == 0
 781                      ? sp_darray_right_tangent(d, len)
 782                      : unit_vector(t) );
 783         }
 784     }
 785 }
 786
 787 /**
 788  * Estimates the (backward) tangent at d[center], by averaging the two
 789  * segments connected to d[center] (and then normalizing the result).
 790  *
 791  * \note The tangent is "backwards", i.e. it is with respect to
 792  * decreasing index rather than increasing index.
 793  *
 794  * \pre (0 \< center \< len - 1) and d is uniqued (at least in
 795  * the immediate vicinity of \a center).
 796  */
 797 static NR::Point
 798 sp_darray_center_tangent(NR::Point const d[],
 799                          unsigned const center,
 800                          unsigned const len)
 801 {
 802     g_assert( center != 0 );
 803     g_assert( center < len - 1 );
 804
 805     NR::Point ret;
 806     if ( d[center + 1] == d[center - 1] ) {
 807         /* Rotate 90 degrees in an arbitrary direction. */
 808         NR::Point const diff = d[center] - d[center - 1];
 809         ret = NR::rot90(diff);
 810     } else {
 811         ret = d[center - 1] - d[center + 1];
 812     }
 813     ret.normalize();
 814     return ret;
 815 }
 816
 817
 818 /**
 819  *  Assign parameter values to digitized points using relative distances between points.
 820  *
 821  *  \pre Parameter array u must have space for \a len items.
 822  */
 823 static void
 824 chord_length_parameterize(NR::Point const d[], gdouble u[], unsigned const len)
 825 {
 826     g_return_if_fail( 2 <= len );
 827
 828     /* First let u[i] equal the distance travelled along the path from d[0] to d[i]. */
 829     u[0] = 0.0;
 830     for (unsigned i = 1; i < len; i++) {
 831         double const dist = L2( d[i] - d[i-1] );
 832         u[i] = u[i-1] + dist;
 833     }
 834
 835     /* Then scale to [0.0 .. 1.0]. */
 836     gdouble tot_len = u[len - 1];
 837     g_return_if_fail( tot_len != 0 );
 838     if (IS_FINITE(tot_len)) {
 839         for (unsigned i = 1; i < len; ++i) {
 840             u[i] /= tot_len;
 841         }
 842     } else {
 843         /* We could do better, but this probably never happens anyway. */
 844         for (unsigned i = 1; i < len; ++i) {
 845             u[i] = i / (gdouble) ( len - 1 );
 846         }
 847     }
 848
 849     /** \todo
 850      * It's been reported that u[len - 1] can differ from 1.0 on some
 851      * systems (amd64), despite it having been calculated as x / x where x
 852      * is IS_FINITE and non-zero.
 853      */
 854     if (u[len - 1] != 1) {
 855         double const diff = u[len - 1] - 1;
 856         if (fabs(diff) > 1e-13) {
 857             g_warning("u[len - 1] = %19g (= 1 + %19g), expecting exactly 1",
 858                       u[len - 1], diff);
 859         }
 860         u[len - 1] = 1;
 861     }
 862
 863 #ifdef BEZIER_DEBUG
 864     g_assert( u[0] == 0.0 && u[len - 1] == 1.0 );
 865     for (unsigned i = 1; i < len; i++) {
 866         g_assert( u[i] >= u[i-1] );
 867     }
 868 #endif
 869 }
 870
 871
 872
 873
 874 /**
 875  * Find the maximum squared distance of digitized points to fitted curve, and (if this maximum
 876  * error is non-zero) set \a *splitPoint to the corresponding index.
 877  *
 878  * \pre 2 \<= len.
 879  * \pre u[0] == 0.
 880  * \pre u[len - 1] == 1.0.
 881  * \post ((ret == 0.0)
 882  *        || ((*splitPoint \< len - 1)
 883  *            \&\& (*splitPoint != 0 || ret \< 0.0))).
 884  */
 885 static gdouble
 886 compute_max_error_ratio(NR::Point const d[], double const u[], unsigned const len,
 887                         BezierCurve const bezCurve, double const tolerance,
 888                         unsigned *const splitPoint)
 889 {
 890     g_assert( 2 <= len );
 891     unsigned const last = len - 1;
 892     g_assert( bezCurve[0] == d[0] );
 893     g_assert( bezCurve[3] == d[last] );
 894     g_assert( u[0] == 0.0 );
 895     g_assert( u[last] == 1.0 );
 896     /* I.e. assert that the error for the first & last points is zero.
 897      * Otherwise we should include those points in the below loop.
 898      * The assertion is also necessary to ensure 0 < splitPoint < last.
 899      */
 900
 901     double maxDistsq = 0.0; /* Maximum error */
 902     double max_hook_ratio = 0.0;
 903     unsigned snap_end = 0;
 904     NR::Point prev = bezCurve[0];
 905     for (unsigned i = 1; i <= last; i++) {
 906         NR::Point const curr = bezier_pt(3, bezCurve, u[i]);
 907         double const distsq = lensq( curr - d[i] );
 908         if ( distsq > maxDistsq ) {
 909             maxDistsq = distsq;
 910             *splitPoint = i;
 911         }
 912         double const hook_ratio = compute_hook(prev, curr, .5 * (u[i - 1] + u[i]), bezCurve, tolerance);
 913         if (max_hook_ratio < hook_ratio) {
 914             max_hook_ratio = hook_ratio;
 915             snap_end = i;
 916         }
 917         prev = curr;
 918     }
 919
 920     double const dist_ratio = sqrt(maxDistsq) / tolerance;
 921     double ret;
 922     if (max_hook_ratio <= dist_ratio) {
 923         ret = dist_ratio;
 924     } else {
 925         g_assert(0 < snap_end);
 926         ret = -max_hook_ratio;
 927         *splitPoint = snap_end - 1;
 928     }
 929     g_assert( ret == 0.0
 930               || ( ( *splitPoint < last )
 931                    && ( *splitPoint != 0 || ret < 0. ) ) );
 932     return ret;
 933 }
 934
 935 /**
 936  * Whereas compute_max_error_ratio() checks for itself that each data point
 937  * is near some point on the curve, this function checks that each point on
 938  * the curve is near some data point (or near some point on the polyline
 939  * defined by the data points, or something like that: we allow for a
 940  * "reasonable curviness" from such a polyline).  "Reasonable curviness"
 941  * means we draw a circle centred at the midpoint of a..b, of radius
 942  * proportional to the length |a - b|, and require that each point on the
 943  * segment of bezCurve between the parameters of a and b be within that circle.
 944  * If any point P on the bezCurve segment is outside of that allowable
 945  * region (circle), then we return some metric that increases with the
 946  * distance from P to the circle.
 947  *
 948  *  Given that this is a fairly arbitrary criterion for finding appropriate
 949  *  places for sharp corners, we test only one point on bezCurve, namely
 950  *  the point on bezCurve with parameter halfway between our estimated
 951  *  parameters for a and b.  (Alternatives are taking the farthest of a
 952  *  few parameters between those of a and b, or even using a variant of
 953  *  NewtonRaphsonFindRoot() for finding the maximum rather than minimum
 954  *  distance.)
 955  */
 956 static double
 957 compute_hook(NR::Point const &a, NR::Point const &b, double const u, BezierCurve const bezCurve,
 958              double const tolerance)
 959 {
 960     NR::Point const P = bezier_pt(3, bezCurve, u);
 961     NR::Point const diff = .5 * (a + b) - P;
 962     double const dist = NR::L2(diff);
 963     if (dist < tolerance) {
 964         return 0;
 965     }
 966     double const allowed = NR::L2(b - a) + tolerance;
 967     return dist / allowed;
 968     /** \todo
 969      * effic: Hooks are very rare.  We could start by comparing
 970      * distsq, only resorting to the more expensive L2 in cases of
 971      * uncertainty.
 972      */
 973 }
 974
 975 /*
 976   Local Variables:
 977   mode:c++
 978   c-file-style:"stroustrup"
 979   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 980   indent-tabs-mode:nil
 981   fill-column:99
 982   End:
 983 */
 984 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :