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