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