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