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