fix for some errors.
[blender.git] / source / blender / freestyle / intern / geometry / FitCurve.cpp
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2010 Blender Foundation.
19  * All rights reserved.
20  *
21  * The Original Code is: all of this file.
22  *
23  * Contributor(s): none yet.
24  *
25  * ***** END GPL LICENSE BLOCK *****
26  */
27
28 /** \file blender/freestyle/intern/geometry/FitCurve.cpp
29  *  \ingroup freestyle
30  *  \brief An Algorithm for Automatically Fitting Digitized Curves by Philip J. Schneider,
31  *  \brief from "Graphics Gems", Academic Press, 1990
32  *  \author Stephane Grabli
33  *  \date 06/06/2003
34  */
35
36 #include <cstdlib> // for malloc and free
37 #include <stdio.h>
38 #include <math.h>
39
40 #include "FitCurve.h"
41
42 using namespace std;
43
44 typedef Vector2 *BezierCurve;
45
46 // XXX Do we need "#ifdef __cplusplus" at all here???
47 #ifdef __cplusplus
48 extern "C"
49 {
50 #endif
51
52 /* Forward declarations */
53 static double *Reparameterize(Vector2 *d, int first, int last, double *u, BezierCurve bezCurve);
54 static double NewtonRaphsonRootFind(BezierCurve Q, Vector2 P, double u);
55 static Vector2 BezierII(int degree, Vector2 *V, double t);
56 static double B0(double u);
57 static double B1(double u);
58 static double B2(double u);
59 static double B3(double u);
60 static Vector2 ComputeLeftTangent(Vector2 *d, int end);
61 static double ComputeMaxError(Vector2 *d, int first, int last, BezierCurve bezCurve, double *u, int *splitPoint);
62 static double *ChordLengthParameterize(Vector2 *d, int first, int last);
63 static BezierCurve GenerateBezier(Vector2 *d, int first, int last, double *uPrime, Vector2 tHat1, Vector2 tHat2);
64 static Vector2 V2AddII(Vector2 a, Vector2 b);
65 static Vector2 V2ScaleIII(Vector2 v, double s);
66 static Vector2 V2SubII(Vector2 a, Vector2 b);
67
68 #define MAXPOINTS 1000 /* The most points you can have */
69
70 /* returns squared length of input vector */
71 static double V2SquaredLength(Vector2 *a)
72 {
73         return (((*a)[0] * (*a)[0]) + ((*a)[1] * (*a)[1]));
74 }
75
76 /* returns length of input vector */
77 static double V2Length(Vector2 *a) 
78 {
79         return (sqrt(V2SquaredLength(a)));
80 }
81
82 static Vector2 *V2Scale(Vector2 *v, double newlen)
83 {
84         double len = V2Length(v);
85         if (len != 0.0) {
86                 (*v)[0] *= newlen / len;
87                 (*v)[1] *= newlen / len;
88         }
89         return v;
90 }
91
92 /* return the dot product of vectors a and b */
93 static double V2Dot(Vector2 *a, Vector2 *b)
94 {
95         return (((*a)[0] * (*b)[0]) + ((*a)[1] * (*b)[1]));
96 }
97
98 /* return the distance between two points */
99 static double V2DistanceBetween2Points(Vector2 *a, Vector2 *b)
100 {
101         double dx = (*a)[0] - (*b)[0];
102         double dy = (*a)[1] - (*b)[1];
103         return (sqrt((dx * dx) + (dy * dy)));
104 }
105
106 /* return vector sum c = a+b */
107 static Vector2 *V2Add(Vector2 *a, Vector2 *b, Vector2 *c)
108 {
109         (*c)[0] = (*a)[0] + (*b)[0];
110         (*c)[1] = (*a)[1] + (*b)[1];
111         return c;
112
113
114 /* normalizes the input vector and returns it */
115 static Vector2 *V2Normalize(Vector2 *v)
116 {
117         double len = V2Length(v);
118         if (len != 0.0) {
119                 (*v)[0] /= len;
120                 (*v)[1] /= len;
121         }
122         return v;
123 }
124
125 /* negates the input vector and returns it */
126 static Vector2 *V2Negate(Vector2 *v)
127 {
128         (*v)[0] = -(*v)[0];
129         (*v)[1] = -(*v)[1];
130         return v;
131 }
132
133 /*  GenerateBezier:
134  *  Use least-squares method to find Bezier control points for region.
135  *    Vector2 *d;           Array of digitized points
136  *    int     first, last;  Indices defining region
137  *    double  *uPrime;      Parameter values for region
138  *    Vector2 tHat1, tHat2; Unit tangents at endpoints
139  */
140 static BezierCurve  GenerateBezier(Vector2 *d, int first, int last, double *uPrime, Vector2 tHat1, Vector2 tHat2)
141 {
142         int i;
143         Vector2 A[MAXPOINTS][2]; /* Precomputed rhs for eqn */
144         int nPts;                /* Number of pts in sub-curve */
145         double C[2][2];          /* Matrix C */
146         double X[2];             /* Matrix X */
147         double det_C0_C1;        /* Determinants of matrices */
148         double det_C0_X;
149         double det_X_C1;
150         double alpha_l;          /* Alpha values, left and right */
151         double alpha_r;
152         Vector2 tmp;             /* Utility variable */
153         BezierCurve bezCurve;    /* RETURN bezier curve ctl pts */
154
155         bezCurve = (Vector2 *)malloc(4 * sizeof(Vector2));
156         nPts = last - first + 1;
157
158         /* Compute the A's */
159         for (i = 0; i < nPts; i++) {
160                 Vector2 v1, v2;
161                 v1 = tHat1;
162                 v2 = tHat2;
163                 V2Scale(&v1, B1(uPrime[i]));
164                 V2Scale(&v2, B2(uPrime[i]));
165                 A[i][0] = v1;
166                 A[i][1] = v2;
167         }
168
169         /* Create the C and X matrices */
170         C[0][0] = 0.0;
171         C[0][1] = 0.0;
172         C[1][0] = 0.0;
173         C[1][1] = 0.0;
174         X[0]    = 0.0;
175         X[1]    = 0.0;
176         for (i = 0; i < nPts; i++) {
177                 C[0][0] += V2Dot(&A[i][0], &A[i][0]);
178                 C[0][1] += V2Dot(&A[i][0], &A[i][1]);
179 //              C[1][0] += V2Dot(&A[i][0], &A[i][1]);
180                 C[1][0] = C[0][1];
181                 C[1][1] += V2Dot(&A[i][1], &A[i][1]);
182
183                 tmp = V2SubII(d[first + i],
184                               V2AddII(V2ScaleIII(d[first], B0(uPrime[i])),
185                                       V2AddII(V2ScaleIII(d[first], B1(uPrime[i])),
186                                               V2AddII(V2ScaleIII(d[last], B2(uPrime[i])),
187                                                       V2ScaleIII(d[last], B3(uPrime[i]))
188                                                      )
189                                              )
190                                      )
191                              );
192
193                 X[0] += V2Dot(&((A[i])[0]), &tmp);
194                 X[1] += V2Dot(&((A[i])[1]), &tmp);
195         }
196
197         /* Compute the determinants of C and X */
198         det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
199         det_C0_X  = C[0][0] * X[1]    - C[0][1] * X[0];
200         det_X_C1  = X[0]    * C[1][1] - X[1]    * C[0][1];
201
202         /* Finally, derive alpha values */
203         if (det_C0_C1 == 0.0) {
204                 det_C0_C1 = (C[0][0] * C[1][1]) * 10.0e-12;
205         }
206         alpha_l = det_X_C1 / det_C0_C1;
207         alpha_r = det_C0_X / det_C0_C1;
208
209
210         /* If alpha negative, use the Wu/Barsky heuristic (see text) (if alpha is 0, you get coincident control points
211          * that lead to divide by zero in any subsequent NewtonRaphsonRootFind() call).
212          */
213         if (alpha_l < 1.0e-6 || alpha_r < 1.0e-6) {
214                 double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;
215
216                 bezCurve[0] = d[first];
217                 bezCurve[3] = d[last];
218                 V2Add(&(bezCurve[0]), V2Scale(&(tHat1), dist), &(bezCurve[1]));
219                 V2Add(&(bezCurve[3]), V2Scale(&(tHat2), dist), &(bezCurve[2]));
220                 return bezCurve;
221         }
222
223         /* First and last control points of the Bezier curve are positioned exactly at the first and last data points
224          * Control points 1 and 2 are positioned an alpha distance out on the tangent vectors, left and right, respectively
225          */
226         bezCurve[0] = d[first];
227         bezCurve[3] = d[last];
228         V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
229         V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
230         return (bezCurve);
231 }
232
233 /*
234  *  Reparameterize:
235  *  Given set of points and their parameterization, try to find a better parameterization.
236  *    Vector2     *d;           Array of digitized points
237  *    int         first, last;  Indices defining region
238  *    double      *u;           Current parameter values
239  *    BezierCurve bezCurve;     Current fitted curve
240  */
241 static double *Reparameterize(Vector2 *d, int first, int last, double *u, BezierCurve bezCurve)
242 {
243         int nPts = last - first + 1;
244         int i;
245         double *uPrime; /* New parameter values */
246
247         uPrime = (double *)malloc(nPts * sizeof(double));
248         for (i = first; i <= last; i++) {
249                 uPrime[i - first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i - first]);
250         }
251         return (uPrime);
252 }
253
254 /*
255  *  NewtonRaphsonRootFind:
256  *  Use Newton-Raphson iteration to find better root.
257  *    BezierCurve Q;  Current fitted curve
258  *    Vector2     P;  Digitized point
259  *    double      u;  Parameter value for "P"
260  */
261 static double NewtonRaphsonRootFind(BezierCurve Q, Vector2 P, double u)
262 {
263         double  numerator, denominator;
264         Vector2 Q1[3], Q2[2];     /* Q' and Q'' */
265         Vector2 Q_u, Q1_u, Q2_u;  /* u evaluated at Q, Q', & Q'' */
266         double  uPrime;           /* Improved u */
267         int     i;
268
269         /* Compute Q(u) */
270         Q_u = BezierII(3, Q, u);
271
272         /* Generate control vertices for Q' */
273         for (i = 0; i <= 2; i++) {
274                 Q1[i][0] = (Q[i + 1][0] - Q[i][0]) * 3.0;
275                 Q1[i][1] = (Q[i + 1][1] - Q[i][1]) * 3.0;
276         }
277
278         /* Generate control vertices for Q'' */
279         for (i = 0; i <= 1; i++) {
280                 Q2[i][0] = (Q1[i + 1][0] - Q1[i][0]) * 2.0;
281                 Q2[i][1] = (Q1[i + 1][1] - Q1[i][1]) * 2.0;
282         }
283
284         /* Compute Q'(u) and Q''(u) */
285         Q1_u = BezierII(2, Q1, u);
286         Q2_u = BezierII(1, Q2, u);
287
288         /* Compute f(u)/f'(u) */
289         numerator = (Q_u[0] - P[0]) * (Q1_u[0]) + (Q_u[1] - P[1]) * (Q1_u[1]);
290         denominator = (Q1_u[0]) * (Q1_u[0]) + (Q1_u[1]) * (Q1_u[1]) +
291                       (Q_u[0] - P[0]) * (Q2_u[0]) + (Q_u[1] - P[1]) * (Q2_u[1]);
292
293         /* u = u - f(u)/f'(u) */
294         if (denominator == 0) // FIXME
295                 return u;
296         uPrime = u - (numerator / denominator);
297         return uPrime;
298 }
299
300 /*
301  *  Bezier:
302  *  Evaluate a Bezier curve at a particular parameter value
303  *    int     degree;  The degree of the bezier curve
304  *    Vector2 *V;      Array of control points
305  *    double  t;       Parametric value to find point for
306  */
307 static Vector2 BezierII(int degree, Vector2 *V, double t)
308 {
309         int i, j;
310         Vector2 Q;       /* Point on curve at parameter t */
311         Vector2 *Vtemp;  /* Local copy of control points */
312
313         /* Copy array */
314         Vtemp = (Vector2 *)malloc((unsigned)((degree + 1) * sizeof(Vector2)));
315         for (i = 0; i <= degree; i++) {
316                 Vtemp[i] = V[i];
317         }
318
319         /* Triangle computation */
320         for (i = 1; i <= degree; i++) {
321                 for (j = 0; j <= degree - i; j++) {
322                         Vtemp[j][0] = (1.0 - t) * Vtemp[j][0] + t * Vtemp[j + 1][0];
323                         Vtemp[j][1] = (1.0 - t) * Vtemp[j][1] + t * Vtemp[j + 1][1];
324                 }
325         }
326
327         Q = Vtemp[0];
328         free((void *)Vtemp);
329         return Q;
330 }
331
332 /*
333  *  B0, B1, B2, B3:
334  *  Bezier multipliers
335  */
336 static double B0(double u)
337 {
338         double tmp = 1.0 - u;
339         return (tmp * tmp * tmp);
340 }
341
342
343 static double B1(double u)
344 {
345         double tmp = 1.0 - u;
346         return (3 * u * (tmp * tmp));
347 }
348
349 static double B2(double u)
350 {
351         double tmp = 1.0 - u;
352         return (3 * u * u * tmp);
353 }
354
355 static double B3(double u)
356 {
357         return (u * u * u);
358 }
359
360 /*
361  * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent:
362  * Approximate unit tangents at endpoints and "center" of digitized curve
363  */
364 /*    Vector2 *d;   Digitized points
365  *    int     end;  Index to "left" end of region
366  */
367 static Vector2 ComputeLeftTangent(Vector2 *d, int end)
368 {
369         Vector2 tHat1;
370         tHat1 = V2SubII(d[end + 1], d[end]);
371         tHat1 = *V2Normalize(&tHat1);
372         return tHat1;
373 }
374
375 /*    Vector2 *d;   Digitized points
376  *    int     end;  Index to "right" end of region
377  */
378 static Vector2 ComputeRightTangent(Vector2 *d, int end)
379 {
380         Vector2 tHat2;
381         tHat2 = V2SubII(d[end - 1], d[end]);
382         tHat2 = *V2Normalize(&tHat2);
383         return tHat2;
384 }
385
386 /*    Vector2 *d;   Digitized points
387  *    int     end;  Index to point inside region
388  */
389 static Vector2 ComputeCenterTangent(Vector2 *d, int center)
390 {
391         Vector2 V1, V2, tHatCenter;
392
393         V1 = V2SubII(d[center - 1], d[center]);
394         V2 = V2SubII(d[center], d[center + 1]);
395         tHatCenter[0] = (V1[0] + V2[0]) / 2.0;
396         tHatCenter[1] = (V1[1] + V2[1]) / 2.0;
397         tHatCenter = *V2Normalize(&tHatCenter);
398         return tHatCenter;
399 }
400
401 /*
402  *  ChordLengthParameterize:
403  *  Assign parameter values to digitized points using relative distances between points.
404  *    Vector2 *d;           Array of digitized points
405  *    int     first, last;  Indices defining region
406  */
407 static double *ChordLengthParameterize(Vector2 *d, int first, int last)
408 {
409         int i;
410         double *u; /* Parameterization */
411
412         u = (double *)malloc((unsigned)(last - first + 1) * sizeof(double));
413
414         u[0] = 0.0;
415         for (i = first + 1; i <= last; i++) {
416                 u[i - first] = u[i - first - 1] + V2DistanceBetween2Points(&d[i], &d[i - 1]);
417         }
418
419         for (i = first + 1; i <= last; i++) {
420                 u[i - first] = u[i - first] / u[last - first];
421         }
422
423         return u;
424 }
425
426
427
428
429 /*
430  *  ComputeMaxError :
431  *  Find the maximum squared distance of digitized points to fitted curve.
432  *    Vector2     *d;           Array of digitized points
433  *    int         first, last;  Indices defining region
434  *    BezierCurve bezCurve;     Fitted Bezier curve
435  *    double      *u;           Parameterization of points
436  *    int         *splitPoint;  Point of maximum error
437  */
438 static double ComputeMaxError(Vector2 *d, int first, int last, BezierCurve bezCurve, double *u, int *splitPoint)
439 {
440         int i;
441         double maxDist; /* Maximum error */
442         double dist;    /* Current error */
443         Vector2 P;      /* Point on curve */
444         Vector2 v;      /* Vector from point to curve */
445
446         *splitPoint = (last - first + 1) / 2;
447         maxDist = 0.0;
448         for (i = first + 1; i < last; i++) {
449                 P = BezierII(3, bezCurve, u[i - first]);
450                 v = V2SubII(P, d[i]);
451                 dist = V2SquaredLength(&v);
452                 if (dist >= maxDist) {
453                         maxDist = dist;
454                         *splitPoint = i;
455                 }
456         }
457         return maxDist;
458 }
459
460 static Vector2 V2AddII(Vector2 a, Vector2 b)
461 {
462         Vector2 c;
463         c[0] = a[0] + b[0];
464         c[1] = a[1] + b[1];
465         return c;
466 }
467
468 static Vector2 V2ScaleIII(Vector2 v, double s)
469 {
470         Vector2 result;
471         result[0] = v[0] * s;
472         result[1] = v[1] * s;
473         return result;
474 }
475
476 static Vector2 V2SubII(Vector2 a, Vector2 b)
477 {
478         Vector2 c;
479         c[0] = a[0] - b[0];
480         c[1] = a[1] - b[1];
481         return c;
482 }
483
484 #ifdef __cplusplus
485 }
486 #endif
487
488
489 //------------------------- WRAPPER -----------------------------//
490
491 FitCurveWrapper::FitCurveWrapper()
492 {
493 }
494
495 FitCurveWrapper::~FitCurveWrapper()
496 {
497         _vertices.clear();
498 }
499
500 void FitCurveWrapper::DrawBezierCurve(int n, Vector2 *curve)
501 {
502         for (int i = 0; i < n + 1; ++i)
503                 _vertices.push_back(curve[i]);
504 }
505
506 void FitCurveWrapper::FitCurve(vector<Vec2d>& data, vector<Vec2d>& oCurve, double error)
507 {
508         int size = data.size();
509         Vector2 *d = new Vector2[size];
510         for (int i = 0; i < size; ++i) {
511                 d[i][0] = data[i][0];
512                 d[i][1] = data[i][1];
513         }
514
515         FitCurve(d, size, error);
516
517         // copy results
518         for (vector<Vector2>::iterator v = _vertices.begin(), vend = _vertices.end(); v != vend; ++v) {
519                 oCurve.push_back(Vec2d(v->x(), v->y())) ;
520         }
521 }
522
523 void FitCurveWrapper::FitCurve(Vector2 *d, int nPts, double error)
524 {
525         Vector2 tHat1, tHat2; /* Unit tangent vectors at endpoints */
526
527         tHat1 = ComputeLeftTangent(d, 0);
528         tHat2 = ComputeRightTangent(d, nPts - 1);
529         FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
530 }
531
532 void FitCurveWrapper::FitCubic(Vector2 *d, int first, int last, Vector2 tHat1, Vector2 tHat2, double error)
533 {
534         BezierCurve bezCurve;          /* Control points of fitted Bezier curve */
535         double      *u;                /* Parameter values for point */
536         double      *uPrime;           /* Improved parameter values */
537         double      maxError;          /* Maximum fitting error */
538         int         splitPoint;        /* Point to split point set at */
539         int         nPts;              /* Number of points in subset */
540         double      iterationError;    /* Error below which you try iterating */
541         int         maxIterations = 4; /* Max times to try iterating */
542         Vector2     tHatCenter;        /* Unit tangent vector at splitPoint */
543         int         i;
544
545         iterationError = error * error;
546         nPts = last - first + 1;
547
548         /* Use heuristic if region only has two points in it */
549         if (nPts == 2) {
550                 double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;
551
552                 bezCurve = (Vector2 *)malloc(4 * sizeof(Vector2));
553                 bezCurve[0] = d[first];
554                 bezCurve[3] = d[last];
555                 V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
556                 V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
557                 DrawBezierCurve(3, bezCurve);
558                 free((void *)bezCurve);
559                 return;
560         }
561
562         /* Parameterize points, and attempt to fit curve */
563         u = ChordLengthParameterize(d, first, last);
564         bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
565
566         /* Find max deviation of points to fitted curve */
567         maxError = ComputeMaxError(d, first, last, bezCurve, u, &splitPoint);
568         if (maxError < error) {
569                 DrawBezierCurve(3, bezCurve);
570                 free((void *)u);
571                 free((void *)bezCurve);
572                 return;
573         }
574
575         /* If error not too large, try some reparameterization and iteration */
576         if (maxError < iterationError) {
577                 for (i = 0; i < maxIterations; i++) {
578                         uPrime = Reparameterize(d, first, last, u, bezCurve);
579                         bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
580                         maxError = ComputeMaxError(d, first, last,
581                         bezCurve, uPrime, &splitPoint);
582                         if (maxError < error) {
583                                 DrawBezierCurve(3, bezCurve);
584                                 free((void *)u);
585                                 free((void *)bezCurve);
586                                 return;
587                         }
588                         free((void *)u);
589                         u = uPrime;
590                 }
591         }
592
593         /* Fitting failed -- split at max error point and fit recursively */
594         free((void *)u);
595         free((void *)bezCurve);
596         tHatCenter = ComputeCenterTangent(d, splitPoint);
597         FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
598         V2Negate(&tHatCenter);
599         FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
600 }