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