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