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