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