1 /* This file is part of the KDE project
2 Copyright (C) 2001-2003 Rob Buis <buis@kde.org>
3 Copyright (C) 2007, 2009 Jan Hambrecht <jaham@gmx.net>
4
5 This library is free software; you can redistribute it and/or
6 modify it under the terms of the GNU Library General Public
7 License as published by the Free Software Foundation; either
8 version 2 of the License, or (at your option) any later version.
9
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 Library General Public License for more details.
14
15 You should have received a copy of the GNU Library General Public License
16 along with this library; see the file COPYING.LIB. If not, write to
17 the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
18 * Boston, MA 02110-1301, USA.
19 */
20
21 #include "KoCurveFit.h"
22 #include <KoPathShape.h>
23 #include <QVector>
24 #include <math.h>
25
26 /// our equivalent to zero
27 const qreal Zero = 10e-12;
28
29 /*
30 An Algorithm for Automatically Fitting Digitized Curves
31 by Philip J. Schneider
32 from "Graphics Gems", Academic Press, 1990
33
34 http://tog.acm.org/resources/GraphicsGems/gems/FitCurves.c
35 http://tog.acm.org/resources/GraphicsGems/gems/README
36 */
37
38 class FitVector {
39 public:
FitVector(const QPointF & p)40 FitVector(const QPointF &p)
41 : m_X(p.x())
42 , m_Y(p.y())
43 {
44 }
45
FitVector()46 FitVector()
47 : m_X(0)
48 , m_Y(0)
49 {
50 }
51
FitVector(const QPointF & a,const QPointF & b)52 FitVector(const QPointF &a, const QPointF &b)
53 : m_X(a.x() - b.x())
54 , m_Y(a.y() - b.y())
55 {
56 }
57
normalize()58 void normalize() {
59 qreal len = length();
60 if (qFuzzyCompare(len, qreal(0.0))) {
61 return;
62 }
63 m_X /= len; m_Y /= len;
64 }
65
negate()66 void negate() {
67 m_X = -m_X;
68 m_Y = -m_Y;
69 }
70
scale(qreal s)71 void scale(qreal s) {
72 qreal len = length();
73 if (qFuzzyCompare(len, qreal(0.0))) {
74 return;
75 }
76 m_X *= s / len;
77 m_Y *= s / len;
78 }
79
dot(const FitVector & v) const80 qreal dot(const FitVector &v) const {
81 return ((m_X*v.m_X) + (m_Y*v.m_Y));
82 }
83
length() const84 qreal length() const {
85 return (qreal) sqrt(m_X*m_X + m_Y*m_Y);
86 }
87
operator +(const QPointF & p)88 QPointF operator+(const QPointF &p) {
89 QPointF b(p.x() + m_X, p.y() + m_Y);
90 return b;
91 }
92
93 public:
94 qreal m_X, m_Y;
95 };
96
distance(const QPointF & p1,const QPointF & p2)97 qreal distance(const QPointF &p1, const QPointF &p2)
98 {
99 qreal dx = (p1.x() - p2.x());
100 qreal dy = (p1.y() - p2.y());
101 return sqrt(dx*dx + dy*dy);
102 }
103
104
ComputeLeftTangent(const QList<QPointF> & points,int end)105 FitVector ComputeLeftTangent(const QList<QPointF> &points, int end)
106 {
107 FitVector tHat1(points.at(end + 1), points.at(end));
108
109 tHat1.normalize();
110
111 return tHat1;
112 }
113
ComputeRightTangent(const QList<QPointF> & points,int end)114 FitVector ComputeRightTangent(const QList<QPointF> &points, int end)
115 {
116 FitVector tHat1(points.at(end - 1), points.at(end));
117
118 tHat1.normalize();
119
120 return tHat1;
121 }
122
123 /*
124 * ChordLengthParameterize :
125 * Assign parameter values to digitized points
126 * using relative distances between points.
127 */
ChordLengthParameterize(const QList<QPointF> & points,int first,int last)128 static qreal *ChordLengthParameterize(const QList<QPointF> &points, int first, int last)
129 {
130 int i;
131 qreal *u; /* Parameterization */
132
133 u = new qreal[(last-first+1)];
134
135 u[0] = 0.0;
136 for (i = first + 1; i <= last; ++i) {
137 u[i-first] = u[i-first-1] +
138 distance(points.at(i), points.at(i - 1));
139 }
140
141 qreal denominator = u[last-first];
142 if (qFuzzyCompare(denominator, qreal(0.0))) {
143 denominator = Zero;
144 }
145
146 for (i = first + 1; i <= last; ++i) {
147 u[i-first] = u[i-first] / denominator;
148 }
149
150 return(u);
151 }
152
VectorAdd(FitVector a,FitVector b)153 static FitVector VectorAdd(FitVector a, FitVector b)
154 {
155 FitVector c;
156 c.m_X = a.m_X + b.m_X; c.m_Y = a.m_Y + b.m_Y;
157 return (c);
158 }
VectorScale(FitVector v,qreal s)159 static FitVector VectorScale(FitVector v, qreal s)
160 {
161 FitVector result;
162 result.m_X = v.m_X * s; result.m_Y = v.m_Y * s;
163 return (result);
164 }
165
VectorSub(FitVector a,FitVector b)166 static FitVector VectorSub(FitVector a, FitVector b)
167 {
168 FitVector c;
169 c.m_X = a.m_X - b.m_X; c.m_Y = a.m_Y - b.m_Y;
170 return (c);
171 }
172
ComputeCenterTangent(const QList<QPointF> & points,int center)173 static FitVector ComputeCenterTangent(const QList<QPointF> &points, int center)
174 {
175 FitVector V1, V2, tHatCenter;
176
177 FitVector cpointb(points.at(center - 1));
178 FitVector cpoint(points.at(center));
179 FitVector cpointa(points.at(center + 1));
180
181 V1 = VectorSub(cpointb, cpoint);
182 V2 = VectorSub(cpoint, cpointa);
183 tHatCenter.m_X = ((V1.m_X + V2.m_X) / 2.0);
184 tHatCenter.m_Y = ((V1.m_Y + V2.m_Y) / 2.0);
185 tHatCenter.normalize();
186 return tHatCenter;
187 }
188
189 /*
190 * B0, B1, B2, B3 :
191 * Bezier multipliers
192 */
B0(qreal u)193 static qreal B0(qreal u)
194 {
195 qreal tmp = 1.0 - u;
196 return (tmp * tmp * tmp);
197 }
198
199
B1(qreal u)200 static qreal B1(qreal u)
201 {
202 qreal tmp = 1.0 - u;
203 return (3 * u *(tmp * tmp));
204 }
205
B2(qreal u)206 static qreal B2(qreal u)
207 {
208 qreal tmp = 1.0 - u;
209 return (3 * u * u * tmp);
210 }
211
B3(qreal u)212 static qreal B3(qreal u)
213 {
214 return (u * u * u);
215 }
216
217 /*
218 * GenerateBezier :
219 * Use least-squares method to find Bezier control points for region.
220 *
221 */
GenerateBezier(const QList<QPointF> & points,int first,int last,qreal * uPrime,FitVector tHat1,FitVector tHat2)222 QPointF* GenerateBezier(const QList<QPointF> &points, int first, int last, qreal *uPrime, FitVector tHat1, FitVector tHat2)
223 {
224 int i;
225 int nPts; /* Number of pts in sub-curve */
226 qreal C[2][2]; /* Matrix C */
227 qreal X[2]; /* Matrix X */
228 qreal det_C0_C1, /* Determinants of matrices */
229 det_C0_X,
230 det_X_C1;
231 qreal alpha_l, /* Alpha values, left and right */
232 alpha_r;
233 FitVector tmp; /* Utility variable */
234 QPointF *curve;
235
236 curve = new QPointF[4];
237 nPts = last - first + 1;
238
239 /* Precomputed rhs for eqn */
240 // FitVector A[nPts][2]
241 QVector< QVector<FitVector> > A(nPts, QVector<FitVector>(2));
242
243 /* Compute the A's */
244 for (i = 0; i < nPts; ++i) {
245 FitVector v1, v2;
246 v1 = tHat1;
247 v2 = tHat2;
248 v1.scale(B1(uPrime[i]));
249 v2.scale(B2(uPrime[i]));
250 A[i][0] = v1;
251 A[i][1] = v2;
252 }
253
254 /* Create the C and X matrices */
255 C[0][0] = 0.0;
256 C[0][1] = 0.0;
257 C[1][0] = 0.0;
258 C[1][1] = 0.0;
259 X[0] = 0.0;
260 X[1] = 0.0;
261
262 for (i = 0; i < nPts; ++i) {
263 C[0][0] += (A[i][0]).dot(A[i][0]);
264 C[0][1] += A[i][0].dot(A[i][1]);
265 /* C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/
266 C[1][0] = C[0][1];
267 C[1][1] += A[i][1].dot(A[i][1]);
268
269 FitVector vfirstp1(points.at(first + i));
270 FitVector vfirst(points.at(first));
271 FitVector vlast(points.at(last));
272
273 tmp = VectorSub(vfirstp1,
274 VectorAdd(
275 VectorScale(vfirst, B0(uPrime[i])),
276 VectorAdd(
277 VectorScale(vfirst, B1(uPrime[i])),
278 VectorAdd(
279 VectorScale(vlast, B2(uPrime[i])),
280 VectorScale(vlast, B3(uPrime[i]))))));
281
282
283 X[0] += A[i][0].dot(tmp);
284 X[1] += A[i][1].dot(tmp);
285 }
286
287 /* Compute the determinants of C and X */
288 det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
289 det_C0_X = C[0][0] * X[1] - C[0][1] * X[0];
290 det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
291
292 /* Finally, derive alpha values */
293 if (qFuzzyCompare(det_C0_C1, qreal(0.0))) {
294 det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
295 if (qFuzzyCompare(det_C0_C1, qreal(0.0))) {
296 det_C0_C1 = Zero;
297 }
298 }
299 alpha_l = det_X_C1 / det_C0_C1;
300 alpha_r = det_C0_X / det_C0_C1;
301
302
303 /* If alpha negative, use the Wu/Barsky heuristic (see text) */
304 /* (if alpha is 0, you get coincident control points that lead to
305 * divide by zero in any subsequent NewtonRaphsonRootFind() call. */
306 if (alpha_l < 1.0e-6 || alpha_r < 1.0e-6) {
307 qreal dist = distance(points.at(last), points.at(first)) / 3.0;
308
309 curve[0] = points.at(first);
310 curve[3] = points.at(last);
311
312 tHat1.scale(dist);
313 tHat2.scale(dist);
314
315 curve[1] = tHat1 + curve[0];
316 curve[2] = tHat2 + curve[3];
317 return curve;
318 }
319
320 /* First and last control points of the Bezier curve are */
321 /* positioned exactly at the first and last data points */
322 /* Control points 1 and 2 are positioned an alpha distance out */
323 /* on the tangent vectors, left and right, respectively */
324 curve[0] = points.at(first);
325 curve[3] = points.at(last);
326
327 tHat1.scale(alpha_l);
328 tHat2.scale(alpha_r);
329
330 curve[1] = tHat1 + curve[0];
331 curve[2] = tHat2 + curve[3];
332
333 return (curve);
334 }
335
336 /*
337 * Bezier :
338 * Evaluate a Bezier curve at a particular parameter value
339 *
340 */
BezierII(int degree,QPointF * V,qreal t)341 static QPointF BezierII(int degree, QPointF *V, qreal t)
342 {
343 int i, j;
344 QPointF Q; /* Point on curve at parameter t */
345 QPointF *Vtemp; /* Local copy of control points */
346
347 Vtemp = new QPointF[degree+1];
348
349 for (i = 0; i <= degree; ++i) {
350 Vtemp[i] = V[i];
351 }
352
353 /* Triangle computation */
354 for (i = 1; i <= degree; ++i) {
355 for (j = 0; j <= degree - i; ++j) {
356 Vtemp[j].setX((1.0 - t) * Vtemp[j].x() + t * Vtemp[j+1].x());
357 Vtemp[j].setY((1.0 - t) * Vtemp[j].y() + t * Vtemp[j+1].y());
358 }
359 }
360
361 Q = Vtemp[0];
362 delete[] Vtemp;
363 return Q;
364 }
365
366 /*
367 * ComputeMaxError :
368 * Find the maximum squared distance of digitized points
369 * to fitted curve.
370 */
ComputeMaxError(const QList<QPointF> & points,int first,int last,QPointF * curve,qreal * u,int * splitPoint)371 static qreal ComputeMaxError(const QList<QPointF> &points, int first, int last, QPointF *curve, qreal *u, int *splitPoint)
372 {
373 int i;
374 qreal maxDist; /* Maximum error */
375 qreal dist; /* Current error */
376 QPointF P; /* Point on curve */
377 FitVector v; /* Vector from point to curve */
378
379 *splitPoint = (last - first + 1) / 2;
380 maxDist = 0.0;
381 for (i = first + 1; i < last; ++i) {
382 P = BezierII(3, curve, u[i-first]);
383 v = VectorSub(P, points.at(i));
384 dist = v.length();
385 if (dist >= maxDist) {
386 maxDist = dist;
387 *splitPoint = i;
388 }
389 }
390 return (maxDist);
391 }
392
393
394 /*
395 * NewtonRaphsonRootFind :
396 * Use Newton-Raphson iteration to find better root.
397 */
NewtonRaphsonRootFind(QPointF * Q,QPointF P,qreal u)398 static qreal NewtonRaphsonRootFind(QPointF *Q, QPointF P, qreal u)
399 {
400 qreal numerator, denominator;
401 QPointF Q1[3], Q2[2]; /* Q' and Q'' */
402 QPointF Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q'' */
403 qreal uPrime; /* Improved u */
404 int i;
405
406 /* Compute Q(u) */
407 Q_u = BezierII(3, Q, u);
408
409 /* Generate control vertices for Q' */
410 for (i = 0; i <= 2; ++i) {
411 Q1[i].setX((Q[i+1].x() - Q[i].x()) * 3.0);
412 Q1[i].setY((Q[i+1].y() - Q[i].y()) * 3.0);
413 }
414
415 /* Generate control vertices for Q'' */
416 for (i = 0; i <= 1; ++i) {
417 Q2[i].setX((Q1[i+1].x() - Q1[i].x()) * 2.0);
418 Q2[i].setY((Q1[i+1].y() - Q1[i].y()) * 2.0);
419 }
420
421 /* Compute Q'(u) and Q''(u) */
422 Q1_u = BezierII(2, Q1, u);
423 Q2_u = BezierII(1, Q2, u);
424
425 /* Compute f(u)/f'(u) */
426 numerator = (Q_u.x() - P.x()) * (Q1_u.x()) + (Q_u.y() - P.y()) * (Q1_u.y());
427 denominator = (Q1_u.x()) * (Q1_u.x()) + (Q1_u.y()) * (Q1_u.y()) +
428 (Q_u.x() - P.x()) * (Q2_u.x()) + (Q_u.y() - P.y()) * (Q2_u.y());
429
430 if (qFuzzyCompare(denominator, qreal(0.0))) {
431 denominator = Zero;
432 }
433
434 /* u = u - f(u)/f'(u) */
435 uPrime = u - (numerator / denominator);
436 return (uPrime);
437 }
438
439 /*
440 * Reparameterize:
441 * Given set of points and their parameterization, try to find
442 * a better parameterization.
443 *
444 */
Reparameterize(const QList<QPointF> & points,int first,int last,qreal * u,QPointF * curve)445 static qreal *Reparameterize(const QList<QPointF> &points, int first, int last, qreal *u, QPointF *curve)
446 {
447 int nPts = last - first + 1;
448 int i;
449 qreal *uPrime; /* New parameter values */
450
451 uPrime = new qreal[nPts];
452 for (i = first; i <= last; ++i) {
453 uPrime[i-first] = NewtonRaphsonRootFind(curve, points.at(i), u[i-first]);
454 }
455 return (uPrime);
456 }
457
FitCubic(const QList<QPointF> & points,int first,int last,FitVector tHat1,FitVector tHat2,float error,int & width)458 QPointF *FitCubic(const QList<QPointF> &points, int first, int last, FitVector tHat1, FitVector tHat2, float error, int &width)
459 {
460 qreal *u;
461 qreal *uPrime;
462 qreal maxError;
463 int splitPoint;
464 int nPts;
465 qreal iterationError;
466 int maxIterations = 4;
467 FitVector tHatCenter;
468 QPointF *curve;
469 int i;
470
471 width = 0;
472
473
474 iterationError = error * error;
475 nPts = last - first + 1;
476
477 if (nPts == 2) {
478 qreal dist = distance(points.at(last), points.at(first)) / 3.0;
479
480 curve = new QPointF[4];
481
482 curve[0] = points.at(first);
483 curve[3] = points.at(last);
484
485 tHat1.scale(dist);
486 tHat2.scale(dist);
487
488 curve[1] = tHat1 + curve[0];
489 curve[2] = tHat2 + curve[3];
490
491 width = 4;
492 return curve;
493 }
494
495 /* Parameterize points, and attempt to fit curve */
496 u = ChordLengthParameterize(points, first, last);
497 curve = GenerateBezier(points, first, last, u, tHat1, tHat2);
498
499
500 /* Find max deviation of points to fitted curve */
501 maxError = ComputeMaxError(points, first, last, curve, u, &splitPoint);
502 if (maxError < error) {
503 delete[] u;
504 width = 4;
505 return curve;
506 }
507
508
509 /* If error not too large, try some reparameterization */
510 /* and iteration */
511 if (maxError < iterationError) {
512 for (i = 0; i < maxIterations; ++i) {
513 uPrime = Reparameterize(points, first, last, u, curve);
514 delete[] curve;
515 curve = GenerateBezier(points, first, last, uPrime, tHat1, tHat2);
516 maxError = ComputeMaxError(points, first, last,
517 curve, uPrime, &splitPoint);
518 if (maxError < error) {
519 delete[] u;
520 delete[] uPrime;
521 width = 4;
522 return curve;
523 }
524 delete[] u;
525 u = uPrime;
526 }
527 }
528
529 /* Fitting failed -- split at max error point and fit recursively */
530 delete[] u;
531 delete[] curve;
532 tHatCenter = ComputeCenterTangent(points, splitPoint);
533
534 int w1, w2;
535 QPointF *cu1 = 0, *cu2 = 0;
536 cu1 = FitCubic(points, first, splitPoint, tHat1, tHatCenter, error, w1);
537
538 tHatCenter.negate();
539 cu2 = FitCubic(points, splitPoint, last, tHatCenter, tHat2, error, w2);
540
541 QPointF *newcurve = new QPointF[w1+w2];
542 for (int i = 0; i < w1; ++i) {
543 newcurve[i] = cu1[i];
544 }
545 for (int i = 0; i < w2; ++i) {
546 newcurve[i+w1] = cu2[i];
547 }
548
549 delete[] cu1;
550 delete[] cu2;
551 width = w1 + w2;
552 return newcurve;
553 }
554
555
bezierFit(const QList<QPointF> & points,float error)556 KoPathShape * bezierFit(const QList<QPointF> &points, float error)
557 {
558 FitVector tHat1, tHat2;
559
560 tHat1 = ComputeLeftTangent(points, 0);
561 tHat2 = ComputeRightTangent(points, points.count() - 1);
562
563 int width = 0;
564 QPointF *curve;
565 curve = FitCubic(points, 0, points.count() - 1, tHat1, tHat2, error, width);
566
567 KoPathShape * path = new KoPathShape();
568
569 if (width > 3) {
570 path->moveTo(curve[0]);
571 path->curveTo(curve[1], curve[2], curve[3]);
572 for (int i = 4; i < width; i += 4) {
573 path->curveTo(curve[i+1], curve[i+2], curve[i+3]);
574 }
575 }
576
577 delete[] curve;
578 return path;
579 }
580
581